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)", nodes[i].c_str());
60  }
61  }
62  }
63 
64  try {
65  nav_navgraph_if_ = blackboard->open_for_reading<NavigatorInterface>("Pathplan");
66  nav_colli_if_ = blackboard->open_for_reading<NavigatorInterface>("Navigator");
67  } catch (Exception& e) {
68  e.append("%s initialization failed, could not open navigator "
69  "interface for reading", name());
70  logger->log_error(name(), e);
71  throw;
72  }
73 
74  as_colli_ = new MoveBaseServer(**rosnode, "move_base_colli",
75  boost::bind(&RosMoveBaseThread::action_goal_cb, this, _1, COLLI),
76  boost::bind(&RosMoveBaseThread::action_cancel_cb, this, _1),
77  /* auto_start */ false);
78 
79  as_navgraph_ = new MoveBaseServer(**rosnode, "move_base",
80  boost::bind(&RosMoveBaseThread::action_goal_cb, this, _1, NAVGRAPH),
81  boost::bind(&RosMoveBaseThread::action_cancel_cb, this, _1),
82  /* auto_start */ false);
83 
84  sub_colli_ = rosnode->subscribe<geometry_msgs::PoseStamped>
85  ("move_base_simple/goal_colli", 1, boost::bind(&RosMoveBaseThread::message_cb, this, _1, COLLI));
86  sub_navgraph_ = rosnode->subscribe<geometry_msgs::PoseStamped>
87  ("move_base_simple/goal", 1, boost::bind(&RosMoveBaseThread::message_cb, this, _1, NAVGRAPH));
88 }
89 
90 void
92 {
93  try {
94  blackboard->close(nav_navgraph_if_);
95  blackboard->close(nav_colli_if_);
96  } catch (Exception& e) {
97  logger->log_error(name(), "Closing interface failed!");
98  logger->log_error(name(), e);
99  }
100  delete as_navgraph_;
101  delete as_colli_;
102 }
103 
104 
105 void
107 {
108  as_navgraph_->start();
109  as_colli_->start();
110 }
111 
112 
113 void
114 RosMoveBaseThread::stop()
115 {
118  if (exec_type_ == NAVGRAPH) {
119  if (nav_navgraph_if_->has_writer()) nav_navgraph_if_->msgq_enqueue(msg);
120  } else {
121  if (nav_colli_if_->has_writer()) nav_colli_if_->msgq_enqueue(msg);
122  }
123  if (exec_as_) {
124  as_goal_.setAborted(create_result());
125  }
126  exec_running_ = false;
127 }
128 
129 void
130 RosMoveBaseThread::action_goal_cb(MoveBaseServer::GoalHandle goal, ExecType ext)
131 {
132  MutexLocker lock(loop_mutex);
133  if (exec_running_ && exec_as_) {
134  as_goal_.setAborted(create_result(), "Replaced by new goal");
135  }
136  as_goal_ = goal;
137  goal_pose_ = goal.getGoal()->target_pose;
138  exec_type_ = ext;
139  exec_request_ = true;
140  exec_as_ = true;
141 
142  goal.setAccepted();
143 }
144 
145 void
146 RosMoveBaseThread::action_cancel_cb(MoveBaseServer::GoalHandle goal)
147 {
148  MutexLocker lock(loop_mutex);
149  stop();
150  goal.setCanceled(create_result(), "Abort on request");
151 }
152 
153 void
154 RosMoveBaseThread::message_cb(geometry_msgs::PoseStampedConstPtr goal_pose, ExecType ext)
155 {
156  MutexLocker lock(loop_mutex);
157  goal_pose_ = *goal_pose;
158  exec_type_ = ext;
159  exec_request_ = true;
160  exec_as_ = false;
161 }
162 
163 move_base_msgs::MoveBaseResult
164 RosMoveBaseThread::create_result()
165 {
166  return move_base_msgs::MoveBaseResult();
167 }
168 
169 move_base_msgs::MoveBaseFeedback
170 RosMoveBaseThread::create_feedback()
171 {
172  move_base_msgs::MoveBaseFeedback feedback;
173 
174  // origin in base frame
175  tf::Stamped<tf::Pose> transform_pose_in;
176  transform_pose_in.frame_id = cfg_base_frame_;
177  tf::Stamped<tf::Pose> transform_pose_out;
178 
179  try {
180  tf_listener->transform_pose(cfg_fixed_frame_, transform_pose_in, transform_pose_out);
181 
182  feedback.base_position.header.stamp = ros::Time(transform_pose_out.stamp.get_sec(),
183  transform_pose_out.stamp.get_nsec());
184  feedback.base_position.header.frame_id = cfg_fixed_frame_;
185  feedback.base_position.pose.orientation.x = transform_pose_out.getRotation().x();
186  feedback.base_position.pose.orientation.y = transform_pose_out.getRotation().y();
187  feedback.base_position.pose.orientation.z = transform_pose_out.getRotation().z();
188  feedback.base_position.pose.orientation.w = transform_pose_out.getRotation().w();
189  feedback.base_position.pose.position.x = transform_pose_out.getOrigin().x();
190  feedback.base_position.pose.position.y = transform_pose_out.getOrigin().y();
191  feedback.base_position.pose.position.z = transform_pose_out.getOrigin().z();
192  } catch (Exception &e) {
193  logger->log_warn(name(),
194  "Failed to determine fixed frame pose");
195  }
196 
197  return feedback;
198 }
199 
200 void
202 {
203  if (exec_request_) {
204  exec_request_ = false;
205 
206  if (exec_type_ == NAVGRAPH && ! nav_navgraph_if_->has_writer()) {
207  logger->log_warn(name(), "No writer for navgraph, cannot move to goal");
208  stop();
209  return;
210  } else if (exec_type_ == COLLI && ! nav_colli_if_->has_writer()) {
211  logger->log_warn(name(), "No writer for navigator, cannot move to goal");
212  stop();
213  return;
214  }
215 
216  std::string target_frame;
217  if (exec_type_ == COLLI) {
218  logger->log_info(name(), "Running with colli");
219  target_frame = cfg_base_frame_;
220  nav_if_ = nav_colli_if_;
221  } else if (exec_type_ == NAVGRAPH) {
222  logger->log_info(name(), "Running with navgraph");
223  target_frame = cfg_fixed_frame_;
224  nav_if_ = nav_navgraph_if_;
225  } else {
226  // This should really never happen, if it does, someone fucked up our memory
227  logger->log_warn(name(), "Internal error, invalid execution type");
228  return;
229  }
230 
231  fawkes::Time goal_time(0, 0);
232  // transform pose to target frame
234  target_pose_in(tf::Transform(tf::Quaternion(goal_pose_.pose.orientation.x,
235  goal_pose_.pose.orientation.y,
236  goal_pose_.pose.orientation.z,
237  goal_pose_.pose.orientation.w),
238  tf::Vector3(goal_pose_.pose.position.x,
239  goal_pose_.pose.position.y,
240  goal_pose_.pose.position.z)),
241  goal_time, goal_pose_.header.frame_id);
242 
243  tf::Stamped<tf::Pose> target_pose_out;
244 
245  try {
246  tf_listener->transform_pose(target_frame, target_pose_in, target_pose_out);
247  } catch (Exception &e) {
248  logger->log_warn(name(),
249  "Failed to transform target pose (%s -> %s), cannot move",
250  target_pose_in.frame_id.c_str(), target_frame.c_str());
251  logger->log_warn(name(), e);
252  stop();
253  return;
254  }
255 
256  tf::Vector3 &pos = target_pose_out.getOrigin();
257 
259  new NavigatorInterface::CartesianGotoMessage(/* target_frame.c_str(), */ pos.x(), pos.y(),
260  tf::get_yaw(target_pose_out.getRotation()));
261 
262  logger->log_info(name(), "Goto (%f,%f,%f) in %s",
263  msg->x(), msg->y(), msg->orientation(), target_frame.c_str());
264 
265  try {
266  nav_if_->msgq_enqueue(msg);
267  exec_running_ = true;
268  exec_msgid_ = msg->id();
269  } catch (Exception &e) {
270  logger->log_warn(name(), "Failed to enqueue cartesian goto, exception follows");
271  logger->log_warn(name(), e);
272  stop();
273  }
274  }
275 
276  if (exec_running_) {
277  nav_if_->read();
278 
279  if (exec_as_) as_goal_.publishFeedback(create_feedback());
280 
281  if (nav_if_->msgid() == exec_msgid_ && nav_if_->is_final()) {
282  exec_running_ = false;
283  if (exec_as_) {
284  if (nav_if_->error_code() == 0) {
285  logger->log_info(name(), "Target reached");
286  as_goal_.setSucceeded(create_result(), "Target reached");
287  } else {
288  logger->log_info(name(), "Failed to reach target");
289  as_goal_.setAborted(create_result(), "Failed to reach target");
290  }
291  }
292  }
293  }
294 }
RosMoveBaseThread()
Contructor.
std::string frame_id
The frame_id associated this data.
Definition: types.h:136
unsigned int id() const
Get message ID.
Definition: message.cpp:197
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:91
Thread class encapsulation of pthreads.
Definition: thread.h:42
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:139
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
float orientation() const
Get orientation value.
Thread aspect to access the transform system.
Definition: tf.h:42
Thread aspect to use blocked timing.
fawkes::Time stamp
The timestamp associated with this data.
Definition: types.h:135
bool is_final() const
Get final value.
Base class for exceptions in Fawkes.
Definition: exception.h:36
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:477
CartesianGotoMessage Fawkes BlackBoard Interface Message.
long get_nsec() const
Get nanoseconds.
Definition: time.h:113
bool has_writer() const
Check if there is a writer for the interface.
Definition: interface.cpp:834
const char * name() const
Get name of thread.
Definition: thread.h:95
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:133
unsigned int msgq_enqueue(Message *message)
Enqueue message at end of queue.
Definition: interface.cpp:903
long get_sec() const
Get seconds.
Definition: time.h:110
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:48
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:70
uint32_t msgid() const
Get msgid value.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:44
void append(const char *format,...)
Append messages to the message list.
Definition: exception.cpp:341
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.