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 
24 using 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),
36  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_ACT),
37  cfg_prefix_(cfg_prefix)
38 {
39 }
40 
41 void
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 
67 void
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 
81 void
82 RosNavigatorThread::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 
140 void
141 RosNavigatorThread::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
164 void
165 RosNavigatorThread::activeCb()
166 {
167 }
168 
169 // Called every time feedback is received for the goal
170 void
171 RosNavigatorThread::feedbackCb(const move_base_msgs::MoveBaseFeedbackConstPtr &feedback)
172 {
173  base_position = feedback->base_position;
174 }
175 
176 void
177 RosNavigatorThread::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 
183 bool
184 RosNavigatorThread::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)) {
194  logger->log_error(name(),
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 
208 void
209 RosNavigatorThread::stop_goals()
210 {
211  //cancel all existing goals and send Goal={0/0/0}
212  ac_->cancelAllGoals();
213 }
214 
215 void
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()) {
227  logger->log_warn(name(),
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 {
258  logger->log_warn(name(),
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)) {
268  logger->log_info(name(),
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)) {
301  logger->log_info(name(),
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)) {
334  logger->log_info(name(),
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)) {
370  logger->log_info(name(),
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)) {
405  logger->log_info(name(),
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 
462 void
463 RosNavigatorThread::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 
480 void
481 RosNavigatorThread::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
Get primary string.
Definition: exception.cpp:639
void append(const char *format,...)
Append messages to the message list.
Definition: exception.cpp:333
MessageType * msgq_first_safe(MessageType *&msg)
Get first message casted to the desired type without exceptions.
Definition: interface.h:303
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1182
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:494
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1029
void msgq_flush()
Flush all messages.
Definition: interface.cpp:1046
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