Fawkes API Fawkes Development Version
navigator_thread.cpp
1
2/***************************************************************************
3 * navigator_thread.cpp - Robotino ROS Navigator Thread
4 *
5 * Created: Sat June 09 15:13:27 2012
6 * Copyright 2012 Sebastian Reuter
7 ****************************************************************************/
8
9/* This program is free software; you can redistribute it and/or modify
10 * it under the terms of the GNU General Public License as published by
11 * the Free Software Foundation; either version 2 of the License, or
12 * (at your option) any later version.
13 *
14 * This program is distributed in the hope that it will be useful,
15 * but WITHOUT ANY WARRANTY; without even the implied warranty of
16 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17 * GNU Library General Public License for more details.
18 *
19 * Read the full text in the LICENSE.GPL file in the doc directory.
20 */
21
22#include "navigator_thread.h"
23
24using namespace fawkes;
25
26/** @class RosNavigatorThread "navigator_thread.h"
27 * Send Fawkes locomotion commands off to ROS.
28 * @author Sebastian Reuter
29 */
30
31/** Constructor.
32 * @param cfg_prefix configuration prefix specific for the ros/navigator
33 */
35: Thread("RosNavigatorThread", Thread::OPMODE_WAITFORWAKEUP),
37 cfg_prefix_(cfg_prefix)
38{
39}
40
41void
43{
44 // navigator interface to give feedback of navigation task (writer)
45 try {
46 nav_if_ = blackboard->open_for_writing<NavigatorInterface>("Navigator");
47 } catch (Exception &e) {
48 e.append("%s initialization failed, could not open navigator "
49 "interface for writing",
50 name());
51 logger->log_error(name(), e);
52 throw;
53 }
54
55 //tell the action client that we want to spin a thread by default
56 ac_ = new MoveBaseClient("move_base", false);
57
58 cmd_sent_ = false;
59 connected_history_ = false;
60 nav_if_->set_final(true);
61 nav_if_->write();
62 load_config();
63
64 ac_init_checktime_ = new fawkes::Time(clock);
65}
66
67void
69{
70 // close interfaces
71 try {
72 blackboard->close(nav_if_);
73 } catch (Exception &e) {
74 logger->log_error(name(), "Closing interface failed!");
75 logger->log_error(name(), e);
76 }
77 delete ac_;
78 delete ac_init_checktime_;
79}
80
81void
82RosNavigatorThread::check_status()
83{
84 bool write = false;
85 if (rosnode->hasParam(cfg_dynreconf_path_ + "/" + cfg_dynreconf_trans_vel_name_)) {
86 rosnode->getParam(cfg_dynreconf_path_ + "/" + cfg_dynreconf_trans_vel_name_, param_max_vel);
87 nav_if_->set_max_velocity(param_max_vel);
88 write = true;
89 }
90 if (rosnode->hasParam(cfg_dynreconf_path_ + "/" + cfg_dynreconf_rot_vel_name_)) {
91 rosnode->getParam(cfg_dynreconf_path_ + "/" + cfg_dynreconf_rot_vel_name_, param_max_rot);
92 nav_if_->set_max_rotation(param_max_rot);
93 write = true;
94 }
95
96 if (cmd_sent_) {
97 if (ac_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED
98 || ac_->getState() == actionlib::SimpleClientGoalState::PREEMPTED) {
99 nav_if_->set_final(true);
100
101 // Check if we reached the goal
102 fawkes::tf::Quaternion q_base_rotation;
103 q_base_rotation.setX(base_position.pose.orientation.x);
104 q_base_rotation.setY(base_position.pose.orientation.y);
105 q_base_rotation.setZ(base_position.pose.orientation.z);
106 q_base_rotation.setW(base_position.pose.orientation.w);
107
108 double base_position_x = base_position.pose.position.x;
109 double base_position_y = base_position.pose.position.y;
110 double base_position_yaw = fawkes::tf::get_yaw(q_base_rotation);
111
112 double diff_x = fabs(base_position_x - goal_position_x);
113 double diff_y = fabs(base_position_y - goal_position_y);
114 double diff_yaw = normalize_mirror_rad(base_position_yaw - goal_position_yaw);
115
116 if (diff_x >= goal_tolerance_trans || diff_y >= goal_tolerance_trans
117 || diff_yaw >= goal_tolerance_yaw) {
118 nav_if_->set_error_code(NavigatorInterface::ERROR_OBSTRUCTION);
119 } else {
120 nav_if_->set_error_code(NavigatorInterface::ERROR_NONE);
121 }
122 nav_if_->write();
123 } else if (ac_->getState() == actionlib::SimpleClientGoalState::LOST) {
124 nav_if_->set_final(true);
125 nav_if_->set_error_code(NavigatorInterface::ERROR_UNKNOWN_PLACE);
126 } else if (ac_->getState() == actionlib::SimpleClientGoalState::ABORTED
127 || ac_->getState() == actionlib::SimpleClientGoalState::REJECTED) {
128 nav_if_->set_final(true);
129 nav_if_->set_error_code(NavigatorInterface::ERROR_PATH_GEN_FAIL);
130 } else {
131 nav_if_->set_final(false);
132 nav_if_->set_error_code(0);
133 }
134 write = true;
135 }
136 if (write)
137 nav_if_->write();
138}
139
140void
141RosNavigatorThread::send_goal()
142{
143 //get goal from Navigation interface
144 goal_.target_pose.header.frame_id = nav_if_->target_frame();
145 goal_.target_pose.header.stamp = ros::Time::now();
146 goal_.target_pose.pose.position.x = nav_if_->dest_x();
147 goal_.target_pose.pose.position.y = nav_if_->dest_y();
148 //move_base discards goals with an invalid quaternion
149 fawkes::tf::Quaternion q(std::isfinite(nav_if_->dest_ori()) ? nav_if_->dest_ori() : 0.0, 0, 0);
150 goal_.target_pose.pose.orientation.x = q.x();
151 goal_.target_pose.pose.orientation.y = q.y();
152 goal_.target_pose.pose.orientation.z = q.z();
153 goal_.target_pose.pose.orientation.w = q.w();
154
155 ac_->sendGoal(goal_,
156 boost::bind(&RosNavigatorThread::doneCb, this, _1, _2),
157 boost::bind(&RosNavigatorThread::activeCb, this),
158 boost::bind(&RosNavigatorThread::feedbackCb, this, _1));
159
160 cmd_sent_ = true;
161}
162
163// Called once when the goal becomes active
164void
165RosNavigatorThread::activeCb()
166{
167}
168
169// Called every time feedback is received for the goal
170void
171RosNavigatorThread::feedbackCb(const move_base_msgs::MoveBaseFeedbackConstPtr &feedback)
172{
173 base_position = feedback->base_position;
174}
175
176void
177RosNavigatorThread::doneCb(const actionlib::SimpleClientGoalState & state,
178 const move_base_msgs::MoveBaseResultConstPtr &result)
179{
180 logger->log_info(name(), "Finished in state [%s]", state.toString().c_str());
181}
182
183bool
184RosNavigatorThread::set_dynreconf_value(const std::string &path, const float value)
185{
186 dynreconf_double_param.name = path;
187 dynreconf_double_param.value = value;
188 dynreconf_conf.doubles.push_back(dynreconf_double_param);
189 dynreconf_srv_req.config = dynreconf_conf;
190
191 if (!ros::service::call(cfg_dynreconf_path_ + "/set_parameters",
192 dynreconf_srv_req,
193 dynreconf_srv_resp)) {
195 "Error in setting dynreconf value %s to %f in path %s",
196 path.c_str(),
197 value,
198 cfg_dynreconf_path_.c_str());
199 dynreconf_conf.doubles.clear();
200 return false;
201 } else {
202 logger->log_info(name(), "Setting %s to %f", path.c_str(), value);
203 dynreconf_conf.doubles.clear();
204 return true;
205 }
206}
207
208void
209RosNavigatorThread::stop_goals()
210{
211 //cancel all existing goals and send Goal={0/0/0}
212 ac_->cancelAllGoals();
213}
214
215void
217{
218 if (!ac_->isServerConnected()) {
219 fawkes::Time now(clock);
220 if (now - ac_init_checktime_ >= 5.0) {
221 // action client never connected, yet. Re-create to avoid stale client.
222 delete ac_;
223 ac_ = new MoveBaseClient("move_base", false);
224 ac_init_checktime_->stamp();
225 }
226 if (!nav_if_->msgq_empty()) {
228 "Command received while ROS ActionClient "
229 "not reachable, ignoring");
230 nav_if_->set_error_code(NavigatorInterface::ERROR_PATH_GEN_FAIL);
231 nav_if_->write();
232 nav_if_->msgq_flush();
233 }
234
235 if (connected_history_) {
236 delete ac_;
237 ac_ = new MoveBaseClient("move_base", false);
238 connected_history_ = false;
239 }
240
241 } else {
242 connected_history_ = true;
243 // process incoming messages from fawkes
244 while (!nav_if_->msgq_empty()) {
245 // stop
246 if (NavigatorInterface::StopMessage *msg = nav_if_->msgq_first_safe(msg)) {
247 if (msg->msgid() == 0 || msg->msgid() == nav_if_->msgid()) {
248 logger->log_info(name(), "Stop message received");
249 nav_if_->set_dest_x(0);
250 nav_if_->set_dest_y(0);
251 nav_if_->set_dest_ori(0);
252
253 nav_if_->set_msgid(msg->id());
254 nav_if_->write();
255
256 stop_goals();
257 } else {
259 "Received stop message for non-active command "
260 "(got %u, running %u)",
261 msg->msgid(),
262 nav_if_->msgid());
263 }
264 }
265
266 // cartesian goto
267 else if (NavigatorInterface::CartesianGotoMessage *msg = nav_if_->msgq_first_safe(msg)) {
269 "Cartesian goto message received (x,y,ori) = (%f,%f,%f)",
270 msg->x(),
271 msg->y(),
272 std::isfinite(msg->orientation()) ? msg->orientation() : 0.0);
273 nav_if_->set_dest_x(msg->x());
274 nav_if_->set_dest_y(msg->y());
275 nav_if_->set_dest_ori(msg->orientation());
276 nav_if_->set_target_frame("base_link");
277
278 nav_if_->set_msgid(msg->id());
279
280 nav_if_->write();
281
282 goal_position_x = nav_if_->dest_x();
283 goal_position_y = nav_if_->dest_y();
284 goal_position_yaw = nav_if_->dest_ori();
285
286 goal_tolerance_trans = cfg_trans_tolerance_;
287 goal_tolerance_yaw = cfg_ori_tolerance_;
288
289 // Transform the desired goal position into the fixed frame
290 // so we can check whether we reached the goal or not
291 if (strcmp(cfg_fixed_frame_.c_str(), nav_if_->target_frame()) != 0) {
292 transform_to_fixed_frame();
293 }
294
295 send_goal();
296 }
297
298 // cartesian goto
300 nav_if_->msgq_first_safe(msg)) {
302 "Cartesian goto message received (x,y,ori) = (%f,%f,%f)",
303 msg->x(),
304 msg->y(),
305 std::isfinite(msg->orientation()) ? msg->orientation() : 0.0);
306 nav_if_->set_dest_x(msg->x());
307 nav_if_->set_dest_y(msg->y());
308 nav_if_->set_dest_ori(msg->orientation());
309 nav_if_->set_target_frame(msg->target_frame());
310
311 nav_if_->set_msgid(msg->id());
312
313 nav_if_->write();
314
315 goal_position_x = nav_if_->dest_x();
316 goal_position_y = nav_if_->dest_y();
317 goal_position_yaw = nav_if_->dest_ori();
318
319 goal_tolerance_trans = cfg_trans_tolerance_;
320 goal_tolerance_yaw = cfg_ori_tolerance_;
321
322 // Transform the desired goal position into the fixed frame
323 // so we can check whether we reached the goal or not
324 if (strcmp(cfg_fixed_frame_.c_str(), nav_if_->target_frame()) != 0) {
325 transform_to_fixed_frame();
326 }
327
328 send_goal();
329 }
330
331 // cartesian goto with tolerance
333 nav_if_->msgq_first_safe(msg)) {
335 "Cartesian goto with tolerance message received "
336 "(x,y,ori,trans_tolerance,ori_tolerance) = (%f,%f,%f,%f,%f)",
337 msg->x(),
338 msg->y(),
339 std::isfinite(msg->orientation()) ? msg->orientation() : 0.0,
340 msg->translation_tolerance(),
341 msg->orientation_tolerance());
342 nav_if_->set_dest_x(msg->x());
343 nav_if_->set_dest_y(msg->y());
344 nav_if_->set_dest_ori(msg->orientation());
345 nav_if_->set_target_frame("base_link");
346
347 nav_if_->set_msgid(msg->id());
348
349 nav_if_->write();
350
351 goal_position_x = nav_if_->dest_x();
352 goal_position_y = nav_if_->dest_y();
353 goal_position_yaw = nav_if_->dest_ori();
354
355 goal_tolerance_trans = msg->translation_tolerance();
356 goal_tolerance_yaw = msg->orientation_tolerance();
357
358 // Transform the desired goal position into the fixed frame
359 // so we can check whether we reached the goal or not
360 if (strcmp(cfg_fixed_frame_.c_str(), nav_if_->target_frame()) != 0) {
361 transform_to_fixed_frame();
362 }
363
364 send_goal();
365 }
366
367 // cartesian goto with frame and tolerance
369 nav_if_->msgq_first_safe(msg)) {
371 "Cartesian goto with tolerance message received "
372 "(x,y,ori,trans_tolerance,ori_tolerance) = (%f,%f,%f,%f,%f)",
373 msg->x(),
374 msg->y(),
375 std::isfinite(msg->orientation()) ? msg->orientation() : 0.0,
376 msg->translation_tolerance(),
377 msg->orientation_tolerance());
378 nav_if_->set_dest_x(msg->x());
379 nav_if_->set_dest_y(msg->y());
380 nav_if_->set_dest_ori(msg->orientation());
381 nav_if_->set_target_frame(msg->target_frame());
382
383 nav_if_->set_msgid(msg->id());
384
385 nav_if_->write();
386
387 goal_position_x = nav_if_->dest_x();
388 goal_position_y = nav_if_->dest_y();
389 goal_position_yaw = nav_if_->dest_ori();
390
391 goal_tolerance_trans = msg->translation_tolerance();
392 goal_tolerance_yaw = msg->orientation_tolerance();
393
394 // Transform the desired goal position into the fixed frame
395 // so we can check whether we reached the goal or not
396 if (strcmp(cfg_fixed_frame_.c_str(), nav_if_->target_frame()) != 0) {
397 transform_to_fixed_frame();
398 }
399
400 send_goal();
401 }
402
403 // polar goto
404 else if (NavigatorInterface::PolarGotoMessage *msg = nav_if_->msgq_first_safe(msg)) {
406 "Polar goto message received (phi,dist) = (%f,%f)",
407 msg->phi(),
408 msg->dist());
409 nav_if_->set_dest_x(msg->dist() * cos(msg->phi()));
410 nav_if_->set_dest_y(msg->dist() * cos(msg->phi()));
411 nav_if_->set_dest_ori(msg->phi());
412 nav_if_->set_msgid(msg->id());
413 nav_if_->write();
414
415 goal_tolerance_trans = cfg_trans_tolerance_;
416 goal_tolerance_yaw = cfg_ori_tolerance_;
417
418 send_goal();
419 }
420
421 // max velocity
422 else if (NavigatorInterface::SetMaxVelocityMessage *msg = nav_if_->msgq_first_safe(msg)) {
423 logger->log_info(name(), "velocity message received %f", msg->max_velocity());
424 nav_if_->set_max_velocity(msg->max_velocity());
425 nav_if_->set_msgid(msg->id());
426 nav_if_->write();
427
428 set_dynreconf_value(cfg_dynreconf_trans_vel_name_, msg->max_velocity());
429
430 send_goal();
431 }
432
433 // max rotation velocity
434 else if (NavigatorInterface::SetMaxRotationMessage *msg = nav_if_->msgq_first_safe(msg)) {
435 logger->log_info(name(), "rotation message received %f", msg->max_rotation());
436 nav_if_->set_max_rotation(msg->max_rotation());
437 nav_if_->set_msgid(msg->id());
438 nav_if_->write();
439
440 set_dynreconf_value(cfg_dynreconf_rot_vel_name_, msg->max_rotation());
441
442 send_goal();
443 }
444
446 nav_if_->msgq_first_safe(msg)) {
447 logger->log_info(name(), "velocity message received %f", msg->security_distance());
448 nav_if_->set_security_distance(msg->security_distance());
449 nav_if_->set_msgid(msg->id());
450 nav_if_->write();
451
452 send_goal();
453 }
454
455 nav_if_->msgq_pop();
456 } // while
457
458 check_status();
459 }
460}
461
462void
463RosNavigatorThread::load_config()
464{
465 try {
466 cfg_dynreconf_path_ = config->get_string(cfg_prefix_ + "/dynreconf/path");
467 cfg_dynreconf_trans_vel_name_ = config->get_string(cfg_prefix_ + "/dynreconf/trans_vel_name");
468 cfg_dynreconf_rot_vel_name_ = config->get_string(cfg_prefix_ + "/dynreconf/rot_vel_name");
469 cfg_fixed_frame_ = config->get_string("/frames/fixed");
470 cfg_ori_tolerance_ = config->get_float(cfg_prefix_ + "/ori_tolerance");
471 cfg_trans_tolerance_ = config->get_float(cfg_prefix_ + "/trans_tolerance");
472
473 logger->log_info(name(), "fixed frame: %s", cfg_fixed_frame_.c_str());
474
475 } catch (Exception &e) {
476 logger->log_error(name(), "Error in loading config: %s", e.what());
477 }
478}
479
480void
481RosNavigatorThread::transform_to_fixed_frame()
482{
483 fawkes::tf::Quaternion goal_q = fawkes::tf::create_quaternion_from_yaw(nav_if_->dest_ori());
484 fawkes::tf::Point goal_p(nav_if_->dest_x(), nav_if_->dest_y(), 0.);
485 fawkes::tf::Pose goal_pos(goal_q, goal_p);
486 fawkes::tf::Stamped<fawkes::tf::Pose> goal_pos_stamped(goal_pos,
487 fawkes::Time(0, 0),
488 nav_if_->target_frame());
489 fawkes::tf::Stamped<fawkes::tf::Pose> goal_pos_stamped_transformed;
490
491 try {
492 tf_listener->transform_pose(cfg_fixed_frame_, goal_pos_stamped, goal_pos_stamped_transformed);
493 } catch (fawkes::Exception &e) {
494 }
495
496 goal_position_x = goal_pos_stamped_transformed.getOrigin().getX();
497 goal_position_y = goal_pos_stamped_transformed.getOrigin().getY();
498 goal_position_yaw = fawkes::tf::get_yaw(goal_pos_stamped_transformed.getRotation());
499}
virtual void loop()
Code to execute in the thread.
virtual void init()
Initialize the thread.
virtual void finalize()
Finalize the thread.
RosNavigatorThread(std::string &cfg_prefix)
Constructor.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual void close(Interface *interface)=0
Close interface.
Thread aspect to use blocked timing.
Clock * clock
By means of this member access to the clock is given.
Definition: clock.h:42
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual const char * what() const noexcept
Get primary string.
Definition: exception.cpp:639
void append(const char *format,...) noexcept
Append messages to the message list.
Definition: exception.cpp:333
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1215
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:501
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1062
void msgq_flush()
Flush all messages.
Definition: interface.cpp:1079
MessageType * msgq_first_safe(MessageType *&msg) noexcept
Get first message casted to the desired type without exceptions.
Definition: interface.h:340
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
CartesianGotoMessage Fawkes BlackBoard Interface Message.
CartesianGotoWithFrameMessage Fawkes BlackBoard Interface Message.
CartesianGotoWithFrameWithToleranceMessage Fawkes BlackBoard Interface Message.
CartesianGotoWithToleranceMessage Fawkes BlackBoard Interface Message.
PolarGotoMessage Fawkes BlackBoard Interface Message.
SetMaxRotationMessage Fawkes BlackBoard Interface Message.
SetMaxVelocityMessage Fawkes BlackBoard Interface Message.
SetSecurityDistanceMessage Fawkes BlackBoard Interface Message.
StopMessage Fawkes BlackBoard Interface Message.
NavigatorInterface Fawkes BlackBoard Interface.
void set_max_rotation(const float new_max_rotation)
Set max_rotation value.
void set_msgid(const uint32_t new_msgid)
Set msgid value.
void set_max_velocity(const float new_max_velocity)
Set max_velocity value.
float dest_x() const
Get dest_x value.
float dest_y() const
Get dest_y value.
char * target_frame() const
Get target_frame value.
void set_target_frame(const char *new_target_frame)
Set target_frame value.
void set_security_distance(const float new_security_distance)
Set security_distance value.
void set_error_code(const uint32_t new_error_code)
Set error_code value.
uint32_t msgid() const
Get msgid value.
void set_dest_ori(const float new_dest_ori)
Set dest_ori value.
void set_dest_y(const float new_dest_y)
Set dest_y value.
void set_dest_x(const float new_dest_x)
Set dest_x value.
float dest_ori() const
Get dest_ori value.
void set_final(const bool new_final)
Set final value.
LockPtr< ros::NodeHandle > rosnode
Central ROS node handle.
Definition: ros.h:47
Thread class encapsulation of pthreads.
Definition: thread.h:46
const char * name() const
Get name of thread.
Definition: thread.h:100
A class for handling time.
Definition: time.h:93
Time & stamp()
Set this time to the current time.
Definition: time.cpp:704
tf::Transformer * tf_listener
This is the transform listener which saves transforms published by other threads in the system.
Definition: tf.h:67
void transform_pose(const std::string &target_frame, const Stamped< Pose > &stamped_in, Stamped< Pose > &stamped_out) const
Transform a stamped pose into the target frame.
Fawkes library namespace.
float normalize_mirror_rad(float angle_rad)
Normalize angle in radian between -PI (inclusive) and PI (exclusive).
Definition: angle.h:72