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  */
34 RosNavigatorThread::RosNavigatorThread(std::string &cfg_prefix)
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 }
fawkes::NavigatorInterface::dest_y
float dest_y() const
Get dest_y value.
Definition: NavigatorInterface.cpp:306
RosNavigatorThread::loop
virtual void loop()
Code to execute in the thread.
Definition: navigator_thread.cpp:215
fawkes::NavigatorInterface::set_security_distance
void set_security_distance(const float new_security_distance)
Set security_distance value.
Definition: NavigatorInterface.cpp:582
fawkes::Interface::msgq_pop
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1182
fawkes::Interface::msgq_empty
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1029
fawkes::NavigatorInterface::StopMessage
Definition: NavigatorInterface.h:119
fawkes::NavigatorInterface::target_frame
char * target_frame() const
Get target_frame value.
Definition: NavigatorInterface.cpp:754
fawkes::Interface::msgq_first_safe
MessageType * msgq_first_safe(MessageType *&msg)
Get first message casted to the desired type without exceptions.
Definition: interface.h:302
fawkes::NavigatorInterface::set_dest_ori
void set_dest_ori(const float new_dest_ori)
Set dest_ori value.
Definition: NavigatorInterface.cpp:357
RosNavigatorThread::init
virtual void init()
Initialize the thread.
Definition: navigator_thread.cpp:41
fawkes::tf::Transformer::transform_pose
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.
Definition: transformer.cpp:443
fawkes::NavigatorInterface::set_error_code
void set_error_code(const uint32_t new_error_code)
Set error_code value.
Definition: NavigatorInterface.cpp:489
fawkes::NavigatorInterface::set_dest_x
void set_dest_x(const float new_dest_x)
Set dest_x value.
Definition: NavigatorInterface.cpp:295
fawkes::NavigatorInterface::dest_x
float dest_x() const
Get dest_x value.
Definition: NavigatorInterface.cpp:275
fawkes::ROSAspect::rosnode
LockPtr< ros::NodeHandle > rosnode
Definition: ros.h:46
fawkes::NavigatorInterface::msgid
uint32_t msgid() const
Get msgid value.
Definition: NavigatorInterface.cpp:400
fawkes::Logger::log_info
virtual void log_info(const char *component, const char *format,...)=0
fawkes::NavigatorInterface::CartesianGotoWithFrameMessage
Definition: NavigatorInterface.h:264
fawkes::NavigatorInterface::set_max_rotation
void set_max_rotation(const float new_max_rotation)
Set max_rotation value.
Definition: NavigatorInterface.cpp:551
fawkes::NavigatorInterface::set_max_velocity
void set_max_velocity(const float new_max_velocity)
Set max_velocity value.
Definition: NavigatorInterface.cpp:520
fawkes::BlockedTimingAspect
Definition: blocked_timing.h:54
fawkes::Thread::name
const char * name() const
Definition: thread.h:99
fawkes::ClockAspect::clock
Clock * clock
Definition: clock.h:53
fawkes::tf::Stamped< fawkes::tf::Pose >
fawkes::Exception::append
void append(const char *format,...)
Append messages to the message list.
Definition: exception.cpp:332
fawkes::NavigatorInterface::SetMaxVelocityMessage
Definition: NavigatorInterface.h:500
fawkes::NavigatorInterface::PolarGotoMessage
Definition: NavigatorInterface.h:350
fawkes::NavigatorInterface::set_dest_y
void set_dest_y(const float new_dest_y)
Set dest_y value.
Definition: NavigatorInterface.cpp:326
fawkes::NavigatorInterface::set_msgid
void set_msgid(const uint32_t new_msgid)
Set msgid value.
Definition: NavigatorInterface.cpp:421
fawkes::LoggingAspect::logger
Logger * logger
Definition: logging.h:50
fawkes::BlackBoard::close
virtual void close(Interface *interface)=0
fawkes::Logger::log_error
virtual void log_error(const char *component, const char *format,...)=0
fawkes::NavigatorInterface::set_final
void set_final(const bool new_final)
Set final value.
Definition: NavigatorInterface.cpp:454
fawkes
RosNavigatorThread::RosNavigatorThread
RosNavigatorThread(std::string &cfg_prefix)
Constructor.
Definition: navigator_thread.cpp:33
fawkes::NavigatorInterface::CartesianGotoMessage
Definition: NavigatorInterface.h:186
fawkes::NavigatorInterface::set_target_frame
void set_target_frame(const char *new_target_frame)
Set target_frame value.
Definition: NavigatorInterface.cpp:774
fawkes::NavigatorInterface::SetMaxRotationMessage
Definition: NavigatorInterface.h:527
fawkes::Logger::log_warn
virtual void log_warn(const char *component, const char *format,...)=0
fawkes::ConfigurableAspect::config
Configuration * config
Definition: configurable.h:50
fawkes::NavigatorInterface::SetSecurityDistanceMessage
Definition: NavigatorInterface.h:582
fawkes::NavigatorInterface::dest_ori
float dest_ori() const
Get dest_ori value.
Definition: NavigatorInterface.cpp:337
fawkes::Time
Definition: time.h:96
fawkes::TransformAspect::tf_listener
tf::Transformer * tf_listener
Definition: tf.h:70
fawkes::NavigatorInterface::CartesianGotoWithToleranceMessage
Definition: NavigatorInterface.h:221
fawkes::normalize_mirror_rad
float normalize_mirror_rad(float angle_rad)
Normalize angle in radian between -PI (inclusive) and PI (exclusive).
Definition: angle.h:76
fawkes::Exception::what
virtual const char * what() const
Get primary string.
Definition: exception.cpp:638
fawkes::Configuration::get_float
virtual float get_float(const char *path)=0
fawkes::Thread
Definition: thread.h:44
fawkes::BlackBoardAspect::blackboard
BlackBoard * blackboard
Definition: blackboard.h:47
fawkes::Configuration::get_string
virtual std::string get_string(const char *path)=0
fawkes::Time::stamp
Time & stamp()
Set this time to the current time.
Definition: time.cpp:709
fawkes::Interface::msgq_flush
void msgq_flush()
Flush all messages.
Definition: interface.cpp:1046
RosNavigatorThread::finalize
virtual void finalize()
Finalize the thread.
Definition: navigator_thread.cpp:67
fawkes::NavigatorInterface::CartesianGotoWithFrameWithToleranceMessage
Definition: NavigatorInterface.h:303
fawkes::Interface::write
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:497
fawkes::NavigatorInterface
Definition: NavigatorInterface.h:37
fawkes::BlackBoard::open_for_writing
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
fawkes::Exception
Definition: exception.h:39