Statistics
| Branch: | Tag: | Revision:

hlrc / client / cpp / src / MiddlewareROS.cpp @ 888a909b

History | View | Annotate | Download (9.387 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
#ifdef ROS_SUPPORT
29

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

    
36
using namespace std;
37

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

    
43
template <typename actionspec>
44
actionlib::SimpleActionClient<actionspec> * create_action_client(string scope){
45
    printf("> starting SimpleActionClient on %s\n",scope.c_str());
46

    
47
    actionlib::SimpleActionClient<actionspec> *ac = new actionlib::SimpleActionClient<actionspec>(scope, true);
48

    
49
    if (!ac->waitForServer(ros::Duration(1))){
50
        char buf[256];
51
        snprintf(buf, 256, "ERROR: action service %s not ready", scope.c_str());
52
        throw runtime_error(buf);
53
    }
54
    return ac;
55
}
56

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

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

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

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

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

    
75
    printf("> setting up ROS action clients...\n");
76
    animation_ac = create_action_client<hlrc_server::animationAction>(scope + "/animation");
77
    utterance_ac = create_action_client<hlrc_server::utteranceAction>(scope + "/utterance");
78
    current_emotionstate_ac = create_action_client<hlrc_server::emotionstateAction>(scope + "/currentEmotion");
79
    default_emotionstate_ac = create_action_client<hlrc_server::emotionstateAction>(scope + "/defaultEmotion");
80
    gazetarget_ac = create_action_client<hlrc_server::gazetargetAction>(scope + "/gaze");
81
    lookattarget_ac = create_action_client<hlrc_server::lookattargetAction>(scope + "/lookat");
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
    //timestamp:
157
    goal.gaze_timestamp.sec  = target.gaze_timestamp.sec;
158
    goal.gaze_timestamp.nsec = target.gaze_timestamp.nsec;
159

    
160
    if (target.gaze_type == RobotGaze::ABSOLUTE){
161
        goal.gaze_type = hlrc_server::gazetargetGoal::GAZETARGET_ABSOLUTE;
162
    }else{
163
        goal.gaze_type = hlrc_server::gazetargetGoal::GAZETARGET_RELATIVE;
164
    }
165

    
166
    //send
167
    gazetarget_ac->sendGoal(goal);
168

    
169
    if (blocking){
170
        bool finished_before_timeout = gazetarget_ac->waitForResult(ros::Duration(ROS_ACTION_CALL_TIMEOUT));
171
        if (!finished_before_timeout){
172
            printf("> ERROR: blocking call timed out!\n");
173
        }
174
    }
175
}
176

    
177
void MiddlewareROS::publish_lookat_target(float x, float y, float z, const string frame_id, bool blocking, float roll)
178
{
179
    hlrc_server::lookattargetGoal goal;
180
    goal.point.header.frame_id = frame_id;
181
    goal.point.header.stamp = ros::Time::now();
182

    
183
    geometry_msgs::Point &p = goal.point.point;
184
    p.x = x;
185
    p.y = y;
186
    p.z = z;
187
    goal.roll = roll;
188

    
189
    //send
190
    lookattarget_ac->sendGoal(goal);
191

    
192
    if (blocking){
193
        bool finished_before_timeout = gazetarget_ac->waitForResult(ros::Duration(ROS_ACTION_CALL_TIMEOUT));
194
        if (!finished_before_timeout){
195
            printf("> ERROR: blocking call timed out!\n");
196
        }
197
    }
198
}
199

    
200

    
201
void MiddlewareROS::publish_mouth_target(RobotMouth target, bool blocking){
202
    hlrc_server::mouthtargetGoal goal;
203

    
204
    goal.position_left   = target.position_left;
205
    goal.position_center = target.position_center;
206
    goal.position_right  = target.position_right;
207

    
208
    goal.opening_left   = target.opening_left;
209
    goal.opening_center = target.opening_center;
210
    goal.opening_right  = target.opening_right;
211

    
212

    
213
    //send
214
    mouthtarget_ac->sendGoal(goal);
215

    
216
    if (blocking){
217
        bool finished_before_timeout = mouthtarget_ac->waitForResult(ros::Duration(ROS_ACTION_CALL_TIMEOUT));
218
        if (!finished_before_timeout){
219
            printf("> ERROR: blocking call timed out!\n");
220
        }
221
    }
222
}
223

    
224
void MiddlewareROS::publish_head_animation(RobotHeadAnimation a, bool blocking){
225
    hlrc_server::animationGoal goal;
226

    
227
    switch(a.value){
228
        default:
229
            printf("> WANRING: invalid animation id %d. defaulting to IDLE",a.value);
230
            //fall through:
231
        case(RobotHeadAnimation::IDLE):
232
            goal.target = hlrc_server::animationGoal::IDLE;
233
            break;
234
        case(RobotHeadAnimation::HEAD_NOD):
235
            goal.target = hlrc_server::animationGoal::HEAD_NOD;
236
            break;
237
        case(RobotHeadAnimation::HEAD_SHAKE):
238
            goal.target = hlrc_server::animationGoal::HEAD_SHAKE;
239
            break;
240
        case(RobotHeadAnimation::EYEBLINK_L):
241
            goal.target = hlrc_server::animationGoal::EYEBLINK_L;
242
            break;
243
        case(RobotHeadAnimation::EYEBLINK_R):
244
            goal.target = hlrc_server::animationGoal::EYEBLINK_R;
245
            break;
246
        case(RobotHeadAnimation::EYEBLINK_BOTH):
247
            goal.target = hlrc_server::animationGoal::EYEBLINK_BOTH;
248
            break;
249
        case(RobotHeadAnimation::EYEBROWS_RAISE):
250
            goal.target = hlrc_server::animationGoal::EYEBROWS_RAISE;
251
            break;
252
        case(RobotHeadAnimation::EYEBROWS_LOWER):
253
            goal.target = hlrc_server::animationGoal::EYEBROWS_LOWER;
254
            break;
255
    }
256

    
257
    goal.repetitions = a.repetitions;
258
    goal.scale       = a.scale;
259
    goal.duration_each = a.time_ms;
260

    
261
    //send
262
    animation_ac->sendGoal(goal);
263

    
264
    if (blocking){
265
        bool finished_before_timeout = animation_ac->waitForResult(ros::Duration(ROS_ACTION_CALL_TIMEOUT));
266
        if (!finished_before_timeout){
267
            printf("> ERROR: blocking call timed out!\n");
268
        }
269
    }
270
}
271

    
272
void MiddlewareROS::publish_speech(string text, bool blocking){
273
    hlrc_server::speechGoal goal;
274

    
275
    goal.text = text;
276

    
277
    //send
278
    speech_ac->sendGoal(goal);
279

    
280
    if (blocking){
281
        bool finished_before_timeout = speech_ac->waitForResult(ros::Duration(ROS_ACTION_CALL_TIMEOUT));
282
        if (!finished_before_timeout){
283
            printf("> ERROR: blocking call timed out!\n");
284
        }
285
    }
286
}
287

    
288
#endif
289