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 >= cfg_trans_tolerance_ || diff_y >= cfg_trans_tolerance_
117  || diff_yaw >= cfg_ori_tolerance_) {
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_->msgq_flush();
231  }
232 
233  if (connected_history_) {
234  delete ac_;
235  ac_ = new MoveBaseClient("move_base", false);
236  connected_history_ = false;
237  }
238 
239  } else {
240  connected_history_ = true;
241  // process incoming messages from fawkes
242  while (!nav_if_->msgq_empty()) {
243  // stop
244  if (NavigatorInterface::StopMessage *msg = nav_if_->msgq_first_safe(msg)) {
245  if (msg->msgid() == 0 || msg->msgid() == nav_if_->msgid()) {
246  logger->log_info(name(), "Stop message received");
247  nav_if_->set_dest_x(0);
248  nav_if_->set_dest_y(0);
249  nav_if_->set_dest_ori(0);
250 
251  nav_if_->set_msgid(msg->id());
252  nav_if_->write();
253 
254  stop_goals();
255  } else {
256  logger->log_warn(name(),
257  "Received stop message for non-active command "
258  "(got %u, running %u)",
259  msg->msgid(),
260  nav_if_->msgid());
261  }
262  }
263 
264  // cartesian goto
265  else if (NavigatorInterface::CartesianGotoMessage *msg = nav_if_->msgq_first_safe(msg)) {
266  logger->log_info(name(),
267  "Cartesian goto message received (x,y,ori) = (%f,%f,%f)",
268  msg->x(),
269  msg->y(),
270  std::isfinite(msg->orientation()) ? msg->orientation() : 0.0);
271  nav_if_->set_dest_x(msg->x());
272  nav_if_->set_dest_y(msg->y());
273  nav_if_->set_dest_ori(msg->orientation());
274  nav_if_->set_target_frame("base_link");
275 
276  nav_if_->set_msgid(msg->id());
277 
278  nav_if_->write();
279 
280  goal_position_x = nav_if_->dest_x();
281  goal_position_y = nav_if_->dest_y();
282  goal_position_yaw = nav_if_->dest_ori();
283 
284  // Transform the desired goal position into the fixed frame
285  // so we can check whether we reached the goal or not
286  if (strcmp(cfg_fixed_frame_.c_str(), nav_if_->target_frame()) != 0) {
287  transform_to_fixed_frame();
288  }
289 
290  send_goal();
291  }
292 
293  // cartesian goto
295  nav_if_->msgq_first_safe(msg)) {
296  logger->log_info(name(),
297  "Cartesian goto message received (x,y,ori) = (%f,%f,%f)",
298  msg->x(),
299  msg->y(),
300  std::isfinite(msg->orientation()) ? msg->orientation() : 0.0);
301  nav_if_->set_dest_x(msg->x());
302  nav_if_->set_dest_y(msg->y());
303  nav_if_->set_dest_ori(msg->orientation());
304  nav_if_->set_target_frame(msg->target_frame());
305 
306  nav_if_->set_msgid(msg->id());
307 
308  nav_if_->write();
309 
310  goal_position_x = nav_if_->dest_x();
311  goal_position_y = nav_if_->dest_y();
312  goal_position_yaw = nav_if_->dest_ori();
313 
314  // Transform the desired goal position into the fixed frame
315  // so we can check whether we reached the goal or not
316  if (strcmp(cfg_fixed_frame_.c_str(), nav_if_->target_frame()) != 0) {
317  transform_to_fixed_frame();
318  }
319 
320  send_goal();
321  }
322 
323  // polar goto
324  else if (NavigatorInterface::PolarGotoMessage *msg = nav_if_->msgq_first_safe(msg)) {
325  logger->log_info(name(),
326  "Polar goto message received (phi,dist) = (%f,%f)",
327  msg->phi(),
328  msg->dist());
329  nav_if_->set_dest_x(msg->dist() * cos(msg->phi()));
330  nav_if_->set_dest_y(msg->dist() * cos(msg->phi()));
331  nav_if_->set_dest_ori(msg->phi());
332  nav_if_->set_msgid(msg->id());
333  nav_if_->write();
334 
335  send_goal();
336  }
337 
338  // max velocity
339  else if (NavigatorInterface::SetMaxVelocityMessage *msg = nav_if_->msgq_first_safe(msg)) {
340  logger->log_info(name(), "velocity message received %f", msg->max_velocity());
341  nav_if_->set_max_velocity(msg->max_velocity());
342  nav_if_->set_msgid(msg->id());
343  nav_if_->write();
344 
345  set_dynreconf_value(cfg_dynreconf_trans_vel_name_, msg->max_velocity());
346 
347  send_goal();
348  }
349 
350  // max rotation velocity
351  else if (NavigatorInterface::SetMaxRotationMessage *msg = nav_if_->msgq_first_safe(msg)) {
352  logger->log_info(name(), "rotation message received %f", msg->max_rotation());
353  nav_if_->set_max_rotation(msg->max_rotation());
354  nav_if_->set_msgid(msg->id());
355  nav_if_->write();
356 
357  set_dynreconf_value(cfg_dynreconf_rot_vel_name_, msg->max_rotation());
358 
359  send_goal();
360  }
361 
363  nav_if_->msgq_first_safe(msg)) {
364  logger->log_info(name(), "velocity message received %f", msg->security_distance());
365  nav_if_->set_security_distance(msg->security_distance());
366  nav_if_->set_msgid(msg->id());
367  nav_if_->write();
368 
369  send_goal();
370  }
371 
372  nav_if_->msgq_pop();
373  } // while
374 
375  check_status();
376  }
377 }
378 
379 void
380 RosNavigatorThread::load_config()
381 {
382  try {
383  cfg_dynreconf_path_ = config->get_string(cfg_prefix_ + "/dynreconf/path");
384  cfg_dynreconf_trans_vel_name_ = config->get_string(cfg_prefix_ + "/dynreconf/trans_vel_name");
385  cfg_dynreconf_rot_vel_name_ = config->get_string(cfg_prefix_ + "/dynreconf/rot_vel_name");
386  cfg_fixed_frame_ = config->get_string("/frames/fixed");
387  cfg_ori_tolerance_ = config->get_float(cfg_prefix_ + "/ori_tolerance");
388  cfg_trans_tolerance_ = config->get_float(cfg_prefix_ + "/trans_tolerance");
389 
390  logger->log_info(name(), "fixed frame: %s", cfg_fixed_frame_.c_str());
391 
392  } catch (Exception &e) {
393  logger->log_error(name(), "Error in loading config: %s", e.what());
394  }
395 }
396 
397 void
398 RosNavigatorThread::transform_to_fixed_frame()
399 {
400  fawkes::tf::Quaternion goal_q = fawkes::tf::create_quaternion_from_yaw(nav_if_->dest_ori());
401  fawkes::tf::Point goal_p(nav_if_->dest_x(), nav_if_->dest_y(), 0.);
402  fawkes::tf::Pose goal_pos(goal_q, goal_p);
403  fawkes::tf::Stamped<fawkes::tf::Pose> goal_pos_stamped(goal_pos,
404  fawkes::Time(0, 0),
405  nav_if_->target_frame());
406  fawkes::tf::Stamped<fawkes::tf::Pose> goal_pos_stamped_transformed;
407 
408  try {
409  tf_listener->transform_pose(cfg_fixed_frame_, goal_pos_stamped, goal_pos_stamped_transformed);
410  } catch (fawkes::Exception &e) {
411  }
412 
413  goal_position_x = goal_pos_stamped_transformed.getOrigin().getX();
414  goal_position_y = goal_pos_stamped_transformed.getOrigin().getY();
415  goal_position_yaw = fawkes::tf::get_yaw(goal_pos_stamped_transformed.getRotation());
416 }
void set_target_frame(const char *new_target_frame)
Set target_frame value.
virtual void finalize()
Finalize the thread.
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1026
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Fawkes library namespace.
void set_final(const bool new_final)
Set final value.
SetSecurityDistanceMessage Fawkes BlackBoard Interface Message.
A class for handling time.
Definition: time.h:92
PolarGotoMessage Fawkes BlackBoard Interface Message.
virtual const char * what() const
Get primary string.
Definition: exception.cpp:639
Thread class encapsulation of pthreads.
Definition: thread.h:45
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:494
float dest_ori() const
Get dest_ori value.
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.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
void set_security_distance(const float new_security_distance)
Set security_distance value.
Clock * clock
By means of this member access to the clock is given.
Definition: clock.h:42
Thread aspect to use blocked timing.
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1179
float dest_y() const
Get dest_y value.
float normalize_mirror_rad(float angle_rad)
Normalize angle in radian between -PI (inclusive) and PI (exclusive).
Definition: angle.h:72
virtual void init()
Initialize the thread.
Base class for exceptions in Fawkes.
Definition: exception.h:35
SetMaxRotationMessage Fawkes BlackBoard Interface Message.
CartesianGotoMessage Fawkes BlackBoard Interface Message.
CartesianGotoWithFrameMessage Fawkes BlackBoard Interface Message.
virtual void loop()
Code to execute in the thread.
const char * name() const
Get name of thread.
Definition: thread.h:100
void set_max_velocity(const float new_max_velocity)
Set max_velocity value.
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.
void set_error_code(const uint32_t new_error_code)
Set error_code value.
void set_dest_ori(const float new_dest_ori)
Set dest_ori value.
void set_max_rotation(const float new_max_rotation)
Set max_rotation value.
char * target_frame() const
Get target_frame value.
void set_dest_x(const float new_dest_x)
Set dest_x value.
MessageType * msgq_first_safe(MessageType *&msg)
Get first message casted to the desired type without exceptions.
Definition: interface.h:303
float dest_x() const
Get dest_x value.
void msgq_flush()
Flush all messages.
Definition: interface.cpp:1043
LockPtr< ros::NodeHandle > rosnode
Central ROS node handle.
Definition: ros.h:47
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 set_dest_y(const float new_dest_y)
Set dest_y value.
void set_msgid(const uint32_t new_msgid)
Set msgid value.
uint32_t msgid() const
Get msgid value.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
SetMaxVelocityMessage Fawkes BlackBoard Interface Message.
void append(const char *format,...)
Append messages to the message list.
Definition: exception.cpp:333
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
RosNavigatorThread(std::string &cfg_prefix)
Constructor.
StopMessage Fawkes BlackBoard Interface Message.
NavigatorInterface Fawkes BlackBoard Interface.
virtual void close(Interface *interface)=0
Close interface.