Fawkes API  Fawkes Development Version
move_base_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 "move_base_thread.h"
23 
24 #include <core/threading/mutex_locker.h>
25 
26 using namespace fawkes;
27 
28 /** @class RosMoveBaseThread "move_base_thread.h"
29  * Accept locomotion commands from ROS (emulate move_base).
30  * @author Sebastian Reuter
31  */
32 
33 /** Contructor. */
35 : Thread("RosMoveBaseThread", Thread::OPMODE_WAITFORWAKEUP),
36  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_ACT),
37  TransformAspect(TransformAspect::ONLY_LISTENER)
38 {
39 }
40 
41 void
43 {
44  exec_request_ = false;
45  exec_running_ = false;
46  exec_as_ = false;
47  exec_type_ = NAVGRAPH;
48 
49  cfg_base_frame_ = config->get_string("/frames/base");
50  cfg_fixed_frame_ = config->get_string("/frames/fixed");
51 
52  // basic sanity check, test if a node named move_base has been launched
53  // of course it might have a different name, but try at least the obvious
54  ros::V_string nodes;
55  if (ros::master::getNodes(nodes)) {
56  for (size_t i = 0; i < nodes.size(); ++i) {
57  if (nodes[i] == "/move_base" || nodes[i] == "/move_base_node") {
58  throw Exception("move_base appears to be already running"
59  " (node %s detected)",
60  nodes[i].c_str());
61  }
62  }
63  }
64 
65  try {
66  nav_navgraph_if_ = blackboard->open_for_reading<NavigatorInterface>("Pathplan");
67  nav_colli_if_ = blackboard->open_for_reading<NavigatorInterface>("Navigator");
68  } catch (Exception &e) {
69  e.append("%s initialization failed, could not open navigator "
70  "interface for reading",
71  name());
72  logger->log_error(name(), e);
73  throw;
74  }
75 
76  as_colli_ = new MoveBaseServer(**rosnode,
77  "move_base_colli",
78  boost::bind(&RosMoveBaseThread::action_goal_cb, this, _1, COLLI),
79  boost::bind(&RosMoveBaseThread::action_cancel_cb, this, _1),
80  /* auto_start */ false);
81 
82  as_navgraph_ =
83  new MoveBaseServer(**rosnode,
84  "move_base",
85  boost::bind(&RosMoveBaseThread::action_goal_cb, this, _1, NAVGRAPH),
86  boost::bind(&RosMoveBaseThread::action_cancel_cb, this, _1),
87  /* auto_start */ false);
88 
89  sub_colli_ = rosnode->subscribe<geometry_msgs::PoseStamped>(
90  "move_base_simple/goal_colli", 1, boost::bind(&RosMoveBaseThread::message_cb, this, _1, COLLI));
91  sub_navgraph_ = rosnode->subscribe<geometry_msgs::PoseStamped>(
92  "move_base_simple/goal", 1, boost::bind(&RosMoveBaseThread::message_cb, this, _1, NAVGRAPH));
93 }
94 
95 void
97 {
98  try {
99  blackboard->close(nav_navgraph_if_);
100  blackboard->close(nav_colli_if_);
101  } catch (Exception &e) {
102  logger->log_error(name(), "Closing interface failed!");
103  logger->log_error(name(), e);
104  }
105  delete as_navgraph_;
106  delete as_colli_;
107 }
108 
109 void
111 {
112  as_navgraph_->start();
113  as_colli_->start();
114 }
115 
116 void
117 RosMoveBaseThread::stop()
118 {
120  if (exec_type_ == NAVGRAPH) {
121  if (nav_navgraph_if_->has_writer())
122  nav_navgraph_if_->msgq_enqueue(msg);
123  } else {
124  if (nav_colli_if_->has_writer())
125  nav_colli_if_->msgq_enqueue(msg);
126  }
127  if (exec_as_) {
128  as_goal_.setAborted(create_result());
129  }
130  exec_running_ = false;
131 }
132 
133 void
134 RosMoveBaseThread::action_goal_cb(MoveBaseServer::GoalHandle goal, ExecType ext)
135 {
136  MutexLocker lock(loop_mutex);
137  if (exec_running_ && exec_as_) {
138  as_goal_.setAborted(create_result(), "Replaced by new goal");
139  }
140  as_goal_ = goal;
141  goal_pose_ = goal.getGoal()->target_pose;
142  exec_type_ = ext;
143  exec_request_ = true;
144  exec_as_ = true;
145 
146  goal.setAccepted();
147 }
148 
149 void
150 RosMoveBaseThread::action_cancel_cb(MoveBaseServer::GoalHandle goal)
151 {
152  MutexLocker lock(loop_mutex);
153  stop();
154  goal.setCanceled(create_result(), "Abort on request");
155 }
156 
157 void
158 RosMoveBaseThread::message_cb(geometry_msgs::PoseStampedConstPtr goal_pose, ExecType ext)
159 {
160  MutexLocker lock(loop_mutex);
161  goal_pose_ = *goal_pose;
162  exec_type_ = ext;
163  exec_request_ = true;
164  exec_as_ = false;
165 }
166 
167 move_base_msgs::MoveBaseResult
168 RosMoveBaseThread::create_result()
169 {
170  return move_base_msgs::MoveBaseResult();
171 }
172 
173 move_base_msgs::MoveBaseFeedback
174 RosMoveBaseThread::create_feedback()
175 {
176  move_base_msgs::MoveBaseFeedback feedback;
177 
178  // origin in base frame
179  tf::Stamped<tf::Pose> transform_pose_in;
180  transform_pose_in.frame_id = cfg_base_frame_;
181  tf::Stamped<tf::Pose> transform_pose_out;
182 
183  try {
184  tf_listener->transform_pose(cfg_fixed_frame_, transform_pose_in, transform_pose_out);
185 
186  feedback.base_position.header.stamp =
187  ros::Time(transform_pose_out.stamp.get_sec(), transform_pose_out.stamp.get_nsec());
188  feedback.base_position.header.frame_id = cfg_fixed_frame_;
189  feedback.base_position.pose.orientation.x = transform_pose_out.getRotation().x();
190  feedback.base_position.pose.orientation.y = transform_pose_out.getRotation().y();
191  feedback.base_position.pose.orientation.z = transform_pose_out.getRotation().z();
192  feedback.base_position.pose.orientation.w = transform_pose_out.getRotation().w();
193  feedback.base_position.pose.position.x = transform_pose_out.getOrigin().x();
194  feedback.base_position.pose.position.y = transform_pose_out.getOrigin().y();
195  feedback.base_position.pose.position.z = transform_pose_out.getOrigin().z();
196  } catch (Exception &e) {
197  logger->log_warn(name(), "Failed to determine fixed frame pose");
198  }
199 
200  return feedback;
201 }
202 
203 void
205 {
206  if (exec_request_) {
207  exec_request_ = false;
208 
209  if (exec_type_ == NAVGRAPH && !nav_navgraph_if_->has_writer()) {
210  logger->log_warn(name(), "No writer for navgraph, cannot move to goal");
211  stop();
212  return;
213  } else if (exec_type_ == COLLI && !nav_colli_if_->has_writer()) {
214  logger->log_warn(name(), "No writer for navigator, cannot move to goal");
215  stop();
216  return;
217  }
218 
219  std::string target_frame;
220  if (exec_type_ == COLLI) {
221  logger->log_info(name(), "Running with colli");
222  target_frame = cfg_base_frame_;
223  nav_if_ = nav_colli_if_;
224  } else if (exec_type_ == NAVGRAPH) {
225  logger->log_info(name(), "Running with navgraph");
226  target_frame = cfg_fixed_frame_;
227  nav_if_ = nav_navgraph_if_;
228  } else {
229  // This should really never happen, if it does, someone fucked up our memory
230  logger->log_warn(name(), "Internal error, invalid execution type");
231  return;
232  }
233 
234  fawkes::Time goal_time(0, 0);
235  // transform pose to target frame
236  tf::Stamped<tf::Pose> target_pose_in(
237  tf::Transform(tf::Quaternion(goal_pose_.pose.orientation.x,
238  goal_pose_.pose.orientation.y,
239  goal_pose_.pose.orientation.z,
240  goal_pose_.pose.orientation.w),
241  tf::Vector3(goal_pose_.pose.position.x,
242  goal_pose_.pose.position.y,
243  goal_pose_.pose.position.z)),
244  goal_time,
245  goal_pose_.header.frame_id);
246 
247  tf::Stamped<tf::Pose> target_pose_out;
248 
249  try {
250  tf_listener->transform_pose(target_frame, target_pose_in, target_pose_out);
251  } catch (Exception &e) {
252  logger->log_warn(name(),
253  "Failed to transform target pose (%s -> %s), cannot move",
254  target_pose_in.frame_id.c_str(),
255  target_frame.c_str());
256  logger->log_warn(name(), e);
257  stop();
258  return;
259  }
260 
261  tf::Vector3 &pos = target_pose_out.getOrigin();
262 
264  new NavigatorInterface::CartesianGotoMessage(/* target_frame.c_str(), */ pos.x(),
265  pos.y(),
266  tf::get_yaw(target_pose_out.getRotation()));
267 
268  logger->log_info(name(),
269  "Goto (%f,%f,%f) in %s",
270  msg->x(),
271  msg->y(),
272  msg->orientation(),
273  target_frame.c_str());
274 
275  try {
276  nav_if_->msgq_enqueue(msg);
277  exec_running_ = true;
278  exec_msgid_ = msg->id();
279  } catch (Exception &e) {
280  logger->log_warn(name(), "Failed to enqueue cartesian goto, exception follows");
281  logger->log_warn(name(), e);
282  stop();
283  }
284  }
285 
286  if (exec_running_) {
287  nav_if_->read();
288 
289  if (exec_as_)
290  as_goal_.publishFeedback(create_feedback());
291 
292  if (nav_if_->msgid() == exec_msgid_ && nav_if_->is_final()) {
293  exec_running_ = false;
294  if (exec_as_) {
295  if (nav_if_->error_code() == 0) {
296  logger->log_info(name(), "Target reached");
297  as_goal_.setSucceeded(create_result(), "Target reached");
298  } else {
299  logger->log_info(name(), "Failed to reach target");
300  as_goal_.setAborted(create_result(), "Failed to reach target");
301  }
302  }
303  }
304  }
305 }
RosMoveBaseThread()
Contructor.
std::string frame_id
The frame_id associated this data.
Definition: types.h:133
unsigned int id() const
Get message ID.
Definition: message.cpp:190
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
virtual void init()
Initialize the thread.
virtual void loop()
Code to execute in the thread.
Fawkes library namespace.
Mutex locking helper.
Definition: mutex_locker.h:33
A class for handling time.
Definition: time.h:92
Thread class encapsulation of pthreads.
Definition: thread.h:45
uint32_t error_code() const
Get error_code 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.
Mutex * loop_mutex
Mutex that is used to protect a call to loop().
Definition: thread.h:152
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
float orientation() const
Get orientation value.
Thread aspect to access the transform system.
Definition: tf.h:38
Thread aspect to use blocked timing.
fawkes::Time stamp
The timestamp associated with this data.
Definition: types.h:132
bool is_final() const
Get final value.
Base class for exceptions in Fawkes.
Definition: exception.h:35
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:472
CartesianGotoMessage Fawkes BlackBoard Interface Message.
long get_nsec() const
Get nanoseconds.
Definition: time.h:132
bool has_writer() const
Check if there is a writer for the interface.
Definition: interface.cpp:814
const char * name() const
Get name of thread.
Definition: thread.h:100
virtual void once()
Execute an action exactly once.
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.
Wrapper class to add time stamp and frame ID to base types.
Definition: types.h:129
unsigned int msgq_enqueue(Message *message)
Enqueue message at end of queue.
Definition: interface.cpp:879
long get_sec() const
Get seconds.
Definition: time.h:117
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
LockPtr< ros::NodeHandle > rosnode
Central ROS node handle.
Definition: ros.h:47
virtual void finalize()
Finalize the thread.
tf::Transformer * tf_listener
This is the transform listener which saves transforms published by other threads in the system.
Definition: tf.h:67
uint32_t msgid() const
Get msgid value.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
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
StopMessage Fawkes BlackBoard Interface Message.
NavigatorInterface Fawkes BlackBoard Interface.
virtual void close(Interface *interface)=0
Close interface.