Statistics
| Branch: | Tag: | Revision:

hlrc / client / cpp / src / MiddlewareROS.cpp @ dff2f3b4

History | View | Annotate | Download (8.309 KB)

1
/*
2
* This file is part of hlrc
3
*
4
* Copyright(c) sschulz <AT> techfak.uni-bielefeld.de
5
* http://opensource.cit-ec.de/projects/hlrc
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
#ifdef ROS_SUPPORT
30

    
31
#include "MiddlewareROS.h"
32
#include <stdio.h>
33
#include <boost/algorithm/string.hpp>
34
#include <boost/range/algorithm_ext/erase.hpp>
35
#include <boost/range/algorithm/remove_if.hpp>
36

    
37
using namespace std;
38

    
39
MiddlewareROS::MiddlewareROS(string scope) : Middleware(scope) {
40
    printf("> new MiddlewareROS() on base scope '%s'\n",base_scope.c_str());
41
    init();
42
}
43

    
44
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
        char buf[256];
52
        snprintf(buf, 256, "ERROR: action service %s not ready", scope.c_str());
53
        throw runtime_error(buf);
54
    }
55
    return ac;
56
}
57

    
58
void MiddlewareROS::init(void){
59
    printf("> MiddlewareROS::init()\n");
60

    
61
    string scope = base_scope + "/set";
62

    
63
    string node_name = "hlrc_client__"+ base_scope;
64
    node_name.erase(boost::remove_if(node_name, boost::is_any_of("/")), node_name.end());
65

    
66
    printf("> registering on ROS as node %s, scope = %s\n",node_name.c_str(), scope.c_str());
67
    if (!ros::isInitialized()){
68
        ros::M_string no_remapping;
69
        ros::init(no_remapping, node_name);
70
    }
71

    
72
    //create node handle
73
    //ros_node_handle = new ros::NodeHandle();
74
    //node_handle = boost::shared_ptr<ros::NodeHandle>(new ros::NodeHandle());
75

    
76
    printf("> setting up ROS action clients...\n");
77
    animation_ac = create_action_client<hlrc_server::animationAction>(scope + "/animation");
78
    utterance_ac = create_action_client<hlrc_server::utteranceAction>(scope + "/utterance");
79
    current_emotionstate_ac = create_action_client<hlrc_server::emotionstateAction>(scope + "/currentEmotion");
80
    default_emotionstate_ac = create_action_client<hlrc_server::emotionstateAction>(scope + "/defaultEmotion");
81
    gazetarget_ac = create_action_client<hlrc_server::gazetargetAction>(scope + "/gaze");
82
    mouthtarget_ac = create_action_client<hlrc_server::mouthtargetAction>(scope + "/mouth");
83
    speech_ac = create_action_client<hlrc_server::speechAction>(scope + "/speech");
84

    
85
    printf("> init done.\n");
86
}
87

    
88

    
89
void MiddlewareROS::publish_emotion(string scope_target, RobotEmotion e, bool blocking){
90
    hlrc_server::emotionstateGoal goal;
91

    
92
    switch(e.value){
93
        default:
94
            printf("> WANRING: invalid emotion id %d. defaulting to NEUTRAL\n",e.value);
95
            //fall through:
96
        case(RobotEmotion::NEUTRAL):
97
            goal.value = hlrc_server::emotionstateGoal::NEUTRAL;
98
            break;
99
        case(RobotEmotion::HAPPY):
100
            goal.value = hlrc_server::emotionstateGoal::HAPPY;
101
            break;
102
        case(RobotEmotion::SAD):
103
            goal.value = hlrc_server::emotionstateGoal::SAD;
104
            break;
105
        case(RobotEmotion::ANGRY):
106
            goal.value = hlrc_server::emotionstateGoal::ANGRY;
107
            break;
108
        case(RobotEmotion::SURPRISED):
109
            goal.value = hlrc_server::emotionstateGoal::SURPRISED;
110
            break;
111
        case(RobotEmotion::FEAR):
112
            goal.value = hlrc_server::emotionstateGoal::FEAR;
113
            break;
114
    }
115

    
116
    goal.duration = e.time_ms;
117

    
118
    actionlib::SimpleActionClient<hlrc_server::emotionstateAction>  *ac;
119
    if (scope_target == "defaultEmotion"){
120
        ac = default_emotionstate_ac;
121
    }else{
122
        ac = current_emotionstate_ac;
123
    }
124

    
125
    //send
126
    ac->sendGoal(goal);
127

    
128
    if (blocking){
129
        bool finished_before_timeout = ac->waitForResult(ros::Duration(ROS_ACTION_CALL_TIMEOUT));
130
        if (!finished_before_timeout){
131
            printf("> ERROR: blocking call timed out!\n");
132
        }
133
    }
134
}
135

    
136

    
137
void MiddlewareROS::publish_current_emotion(RobotEmotion e, bool blocking){
138
    publish_emotion("currentEmotion", e, blocking);
139
}
140

    
141
void MiddlewareROS::publish_default_emotion(RobotEmotion e, bool blocking){
142
    publish_emotion("defaultEmotion", e, blocking);
143
}
144

    
145
void MiddlewareROS::publish_gaze_target(RobotGaze target, bool blocking){
146
    hlrc_server::gazetargetGoal goal;
147

    
148
    goal.pan = target.pan;
149
    goal.tilt = target.tilt;
150
    goal.roll = target.roll;
151
    goal.vergence = target.vergence;
152

    
153
    goal.tilt_offset = target.tilt_offset;
154
    goal.pan_offset = target.pan_offset;
155

    
156
    //send
157
    gazetarget_ac->sendGoal(goal);
158

    
159
    if (blocking){
160
        bool finished_before_timeout = gazetarget_ac->waitForResult(ros::Duration(ROS_ACTION_CALL_TIMEOUT));
161
        if (!finished_before_timeout){
162
            printf("> ERROR: blocking call timed out!\n");
163
        }
164
    }
165
}
166

    
167

    
168
void MiddlewareROS::publish_mouth_target(RobotMouth target, bool blocking){
169
    hlrc_server::mouthtargetGoal goal;
170

    
171
    goal.position_left   = target.position_left;
172
    goal.position_center = target.position_center;
173
    goal.position_right  = target.position_right;
174

    
175
    goal.opening_left   = target.opening_left;
176
    goal.opening_center = target.opening_center;
177
    goal.opening_right  = target.opening_right;
178

    
179

    
180
    //send
181
    mouthtarget_ac->sendGoal(goal);
182

    
183
    if (blocking){
184
        bool finished_before_timeout = mouthtarget_ac->waitForResult(ros::Duration(ROS_ACTION_CALL_TIMEOUT));
185
        if (!finished_before_timeout){
186
            printf("> ERROR: blocking call timed out!\n");
187
        }
188
    }
189
}
190

    
191
void MiddlewareROS::publish_head_animation(RobotHeadAnimation a, bool blocking){
192
    hlrc_server::animationGoal goal;
193

    
194
    switch(a.value){
195
        default:
196
            printf("> WANRING: invalid animation id %d. defaulting to IDLE",a.value);
197
            //fall through:
198
        case(RobotHeadAnimation::IDLE):
199
            goal.target = hlrc_server::animationGoal::IDLE;
200
            break;
201
        case(RobotHeadAnimation::HEAD_NOD):
202
            goal.target = hlrc_server::animationGoal::HEAD_NOD;
203
            break;
204
        case(RobotHeadAnimation::HEAD_SHAKE):
205
            goal.target = hlrc_server::animationGoal::HEAD_SHAKE;
206
            break;
207
        case(RobotHeadAnimation::EYEBLINK_L):
208
            goal.target = hlrc_server::animationGoal::EYEBLINK_L;
209
            break;
210
        case(RobotHeadAnimation::EYEBLINK_R):
211
            goal.target = hlrc_server::animationGoal::EYEBLINK_R;
212
            break;
213
        case(RobotHeadAnimation::EYEBLINK_BOTH):
214
            goal.target = hlrc_server::animationGoal::EYEBLINK_BOTH;
215
            break;
216
        case(RobotHeadAnimation::EYEBROWS_RAISE):
217
            goal.target = hlrc_server::animationGoal::EYEBROWS_RAISE;
218
            break;
219
        case(RobotHeadAnimation::EYEBROWS_LOWER):
220
            goal.target = hlrc_server::animationGoal::EYEBROWS_LOWER;
221
            break;
222
    }
223

    
224
    goal.repetitions = a.repetitions;
225
    goal.scale       = a.scale;
226
    goal.duration_each = a.time_ms;
227

    
228
    //send
229
    animation_ac->sendGoal(goal);
230

    
231
    if (blocking){
232
        bool finished_before_timeout = animation_ac->waitForResult(ros::Duration(ROS_ACTION_CALL_TIMEOUT));
233
        if (!finished_before_timeout){
234
            printf("> ERROR: blocking call timed out!\n");
235
        }
236
    }
237
}
238

    
239
void MiddlewareROS::publish_speech(string text, bool blocking){
240
    hlrc_server::speechGoal goal;
241

    
242
    goal.text = text;
243

    
244
    //send
245
    speech_ac->sendGoal(goal);
246

    
247
    if (blocking){
248
        bool finished_before_timeout = speech_ac->waitForResult(ros::Duration(ROS_ACTION_CALL_TIMEOUT));
249
        if (!finished_before_timeout){
250
            printf("> ERROR: blocking call timed out!\n");
251
        }
252
    }
253
}
254

    
255
#endif