Revision f0dcf4ca client/cpp/src/MiddlewareROS.cpp
client/cpp/src/MiddlewareROS.cpp | ||
---|---|---|
78 | 78 |
current_emotionstate_ac = create_action_client<hlrc_server::emotionstateAction>(scope + "/currentEmotion"); |
79 | 79 |
default_emotionstate_ac = create_action_client<hlrc_server::emotionstateAction>(scope + "/defaultEmotion"); |
80 | 80 |
gazetarget_ac = create_action_client<hlrc_server::gazetargetAction>(scope + "/gaze"); |
81 |
lookattarget_ac = create_action_client<hlrc_server::lookattargetAction>(scope + "/lookat"); |
|
81 | 82 |
mouthtarget_ac = create_action_client<hlrc_server::mouthtargetAction>(scope + "/mouth"); |
82 | 83 |
speech_ac = create_action_client<hlrc_server::speechAction>(scope + "/speech"); |
83 | 84 |
|
... | ... | |
173 | 174 |
} |
174 | 175 |
} |
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 |
|
|
176 | 200 |
|
177 | 201 |
void MiddlewareROS::publish_mouth_target(RobotMouth target, bool blocking){ |
178 | 202 |
hlrc_server::mouthtargetGoal goal; |
Also available in: Unified diff