hlrc / client / cpp / src / MiddlewareROS.cpp @ 9e1f91c5
History | View | Annotate | Download (9.389 KB)
1 | 0c286af0 | Simon Schulz | /*
|
---|---|---|---|
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 = boost::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 | f0dcf4ca | Robert Haschke | lookattarget_ac = create_action_client<hlrc_server::lookattargetAction>(scope + "/lookat");
|
82 | 0c286af0 | Simon Schulz | 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 | 62aeb8ce | Simon Schulz | //timestamp:
|
157 | 3877047d | Simon Schulz | goal.gaze_timestamp.sec = target.gaze_timestamp.sec; |
158 | goal.gaze_timestamp.nsec = target.gaze_timestamp.nsec; |
||
159 | 62aeb8ce | Simon Schulz | |
160 | c5a47e56 | Simon Schulz | if (target.gaze_type == RobotGaze::ABSOLUTE){
|
161 | 62aeb8ce | Simon Schulz | goal.gaze_type = hlrc_server::gazetargetGoal::GAZETARGET_ABSOLUTE; |
162 | }else{
|
||
163 | goal.gaze_type = hlrc_server::gazetargetGoal::GAZETARGET_RELATIVE; |
||
164 | } |
||
165 | |||
166 | 0c286af0 | Simon Schulz | //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 | f0dcf4ca | Robert Haschke | 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 | 0c286af0 | Simon Schulz | 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
|