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
26using 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),
37 TransformAspect(TransformAspect::ONLY_LISTENER)
38{
39}
40
41void
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
95void
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
109void
111{
112 as_navgraph_->start();
113 as_colli_->start();
114}
115
116void
117RosMoveBaseThread::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
133void
134RosMoveBaseThread::action_goal_cb(MoveBaseServer::GoalHandle goal, ExecType ext)
135{
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
149void
150RosMoveBaseThread::action_cancel_cb(MoveBaseServer::GoalHandle goal)
151{
153 stop();
154 goal.setCanceled(create_result(), "Abort on request");
155}
156
157void
158RosMoveBaseThread::message_cb(geometry_msgs::PoseStampedConstPtr goal_pose, ExecType ext)
159{
161 goal_pose_ = *goal_pose;
162 exec_type_ = ext;
163 exec_request_ = true;
164 exec_as_ = false;
165}
166
167move_base_msgs::MoveBaseResult
168RosMoveBaseThread::create_result()
169{
170 return move_base_msgs::MoveBaseResult();
171}
172
173move_base_msgs::MoveBaseFeedback
174RosMoveBaseThread::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
203void
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) {
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
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.
virtual void loop()
Code to execute in the thread.
virtual void once()
Execute an action exactly once.
virtual void init()
Initialize the thread.
virtual void finalize()
Finalize the thread.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
virtual void close(Interface *interface)=0
Close interface.
Thread aspect to use blocked timing.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
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
void append(const char *format,...) noexcept
Append messages to the message list.
Definition: exception.cpp:333
unsigned int msgq_enqueue(Message *message, bool proxy=false)
Enqueue message at end of queue.
Definition: interface.cpp:915
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:479
bool has_writer() const
Check if there is a writer for the interface.
Definition: interface.cpp:848
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
unsigned int id() const
Get message ID.
Definition: message.cpp:181
Mutex locking helper.
Definition: mutex_locker.h:34
CartesianGotoMessage Fawkes BlackBoard Interface Message.
float orientation() const
Get orientation value.
StopMessage Fawkes BlackBoard Interface Message.
NavigatorInterface Fawkes BlackBoard Interface.
bool is_final() const
Get final value.
uint32_t error_code() const
Get error_code value.
uint32_t msgid() const
Get msgid value.
LockPtr< ros::NodeHandle > rosnode
Central ROS node handle.
Definition: ros.h:47
Thread class encapsulation of pthreads.
Definition: thread.h:46
Mutex * loop_mutex
Mutex that is used to protect a call to loop().
Definition: thread.h:152
const char * name() const
Get name of thread.
Definition: thread.h:100
A class for handling time.
Definition: time.h:93
long get_nsec() const
Get nanoseconds.
Definition: time.h:132
long get_sec() const
Get seconds.
Definition: time.h:117
Thread aspect to access the transform system.
Definition: tf.h:39
tf::Transformer * tf_listener
This is the transform listener which saves transforms published by other threads in the system.
Definition: tf.h:67
Wrapper class to add time stamp and frame ID to base types.
Definition: types.h:130
std::string frame_id
The frame_id associated this data.
Definition: types.h:133
fawkes::Time stamp
The timestamp associated with this data.
Definition: types.h:132
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.