Statistics
| Branch: | Tag: | Revision:

hlrc / server / src / MiddlewareROS.cpp @ c72977ba

History | View | Annotate | Download (5.076 KB)

1 0c286af0 Simon Schulz
/*
2
* This file is part of hlrc_server
3
*
4
* Copyright(c) sschulz <AT> techfak.uni-bielefeld.de
5
* http://opensource.cit-ec.de/projects/hlrc_server
6
*
7
* This file may be licensed under the terms of the
8
* GNU General Public License Version 3 (the ``GPL''),
9
* or (at your option) any later version.
10
*
11
* Software distributed under the License is distributed
12
* on an ``AS IS'' basis, WITHOUT WARRANTY OF ANY KIND, either
13
* express or implied. See the GPL for the specific language
14
* governing rights and limitations.
15
*
16
* You should have received a copy of the GPL along with this
17
* program. If not, go to http://www.gnu.org/licenses/gpl.html
18
* or write to the Free Software Foundation, Inc.,
19
* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
20
*
21
* The development of this software was supported by the
22
* Excellence Cluster EXC 277 Cognitive Interaction Technology.
23
* The Excellence Cluster EXC 277 is a grant of the Deutsche
24
* Forschungsgemeinschaft (DFG) in the context of the German
25
* Excellence Initiative.
26
*
27
*/
28
29
#include "MiddlewareROS.h"
30
31
#ifdef ROS_SUPPORT
32
33
#include <cstdio>
34
35
//CallbackWrapper:
36
#include <boost/range/algorithm/remove_if.hpp>
37
#include <boost/algorithm/string/classification.hpp>
38 0c15613f Simon Schulz
#include <actionlib/client/simple_action_client.h>
39
#include "ROS/UtteranceROS.h"
40 0c286af0 Simon Schulz
41
using namespace std;
42
using namespace hlrc_server;
43
44 0c15613f Simon Schulz
template <typename actionspec>
45
actionlib::SimpleActionClient<actionspec> * create_action_client(string scope){
46
    printf("> starting SimpleActionClient on %s\n",scope.c_str());
47
48
    actionlib::SimpleActionClient<actionspec> *ac = new actionlib::SimpleActionClient<actionspec>(scope, true);
49
50
    if (!ac->waitForServer(ros::Duration(1))){
51 b1a51350 Simon Schulz
        printf("> ERROR: action service %s not ready, wait timed out...\n", scope.c_str());
52
        return NULL;
53 0c15613f Simon Schulz
    }
54
    return ac;
55
}
56
57
58 0c286af0 Simon Schulz
MiddlewareROS::MiddlewareROS(Arbiter *arbiter, std::string scope) : Middleware(arbiter, scope){
59
    init();
60
}
61
62
MiddlewareROS::~MiddlewareROS(){
63
64
}
65
66
void MiddlewareROS::init(){
67
    string scope = base_scope + "/set/";
68
69
    string node_name = "hlrc_server__"+ base_scope;
70
    node_name.erase(boost::remove_if(node_name, boost::is_any_of("/")), node_name.end());
71
72
    printf("> registering on ROS as node %s\n",node_name.c_str());
73
    if (ros::isInitialized()){
74
        //ros is already active, no need to take care of that
75 2cf3c285 Robert Haschke
        //FIXME: Instead of deciding based on ros::isInitialized() to tick or not,
76
        //we shoud use our own MessageQueue and AsyncSpinner.
77 0c286af0 Simon Schulz
        tick_necessary = false;
78
    }else{
79
        printf("> ROS not initialized, calling ros::init()\n");
80
        ros::M_string no_remapping;
81
        ros::init(no_remapping, node_name);
82
        tick_necessary = true;
83
    }
84
85
    //create node handle
86
    //printf("> creating ros node handle\n");
87
    //ros::NodeHa
88
    ros::NodeHandle n;
89
90
    printf("> setting up ROS services...\n");
91
    //ros::AsyncSpinner *spinner = new ros::AsyncSpinner(4); // Use 4 threads
92
    //spinner->start();
93
94 2cf3c285 Robert Haschke
    tfListener.reset(new tf2_ros::TransformListener(tfBuffer));
95
    tfBuffer.clear();
96
97 0c286af0 Simon Schulz
    animation_action_server = new AnimationCallbackWrapper(this, scope, "animation");
98
    utterance_action_server = new UtteranceCallbackWrapper(this, scope, "utterance");
99
    current_emotion_action_server = new EmotionCallbackWrapper(this, scope, "currentEmotion");
100
    default_emotion_action_server = new EmotionCallbackWrapper(this, scope, "defaultEmotion");
101
    gaze_action_server = new GazeCallbackWrapper(this, scope, "gaze");
102 2cf3c285 Robert Haschke
    lookat_action_server = new LookatCallbackWrapper(this, scope, "lookat", tfBuffer);
103 0c286af0 Simon Schulz
    mouth_action_server = new MouthCallbackWrapper(this, scope, "mouth");
104
    speech_action_server = new SpeechCallbackWrapper(this, scope, "speech");
105
106 0c15613f Simon Schulz
    //create tts client
107
    tts_ac = create_action_client<hlrc_server::ttsAction>(base_scope + "/tts_provider");
108 b1a51350 Simon Schulz
    if (tts_ac == NULL){
109
        printf("> tts action service not available, will not do any TTS !\n");
110
    }
111 0c15613f Simon Schulz
112 0c286af0 Simon Schulz
    printf("> init done\n");
113
}
114
115
void MiddlewareROS::tick(){
116
    if (tick_necessary){
117
        ros::spinOnce();
118
    }
119
}
120
121
//call a tts system to convert a string to an utterance
122 2e526a15 Simon Schulz
boost::shared_ptr<Utterance> MiddlewareROS::tts_call(string text){
123
    boost::shared_ptr<Utterance> utterance(new Utterance());
124 0c286af0 Simon Schulz
125 b1a51350 Simon Schulz
    //double tts_timeout = 1.0; //seconds. DO NOT CHANGE THIS!
126
    if (tts_ac == NULL){
127
        printf("> tts action service not available. will not speak '%s'\n", text.c_str());
128
        return utterance;
129
    }
130
131 0c15613f Simon Schulz
    hlrc_server::ttsGoal goal;
132 0c286af0 Simon Schulz
133 0c15613f Simon Schulz
    goal.text = text;
134 0c286af0 Simon Schulz
135 0c15613f Simon Schulz
    //send
136
    tts_ac->sendGoal(goal);
137 0c286af0 Simon Schulz
138 0c15613f Simon Schulz
    //call ros:
139
    bool finished_before_timeout = tts_ac->waitForResult(ros::Duration(ROS_ACTION_CALL_TIMEOUT));
140 0c286af0 Simon Schulz
141 0c15613f Simon Schulz
    if (!finished_before_timeout){
142
        printf("> ERROR: NO REPLY to utterance action call received within %4.2f s\n",ROS_ACTION_CALL_TIMEOUT);
143
    }else{
144
        //done, return utterance ptr
145
        ttsResultConstPtr tts_res = tts_ac->getResult();
146
        boost::shared_ptr<Utterance> utterance(new UtteranceROS(tts_res));
147
        printf("> done. got utterance (text=%s)\n",utterance->get_text().c_str());
148
        return utterance;
149 0c286af0 Simon Schulz
    }
150
151 0c15613f Simon Schulz
    printf("> failed... got no utterance\n");
152 0c286af0 Simon Schulz
    return utterance;
153
}
154
155
156
#endif