22 #include "move_base_thread.h" 24 #include <core/threading/mutex_locker.h> 35 :
Thread(
"RosMoveBaseThread",
Thread::OPMODE_WAITFORWAKEUP),
44 exec_request_ =
false;
45 exec_running_ =
false;
47 exec_type_ = NAVGRAPH;
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());
68 e.
append(
"%s initialization failed, could not open navigator " 69 "interface for reading",
name());
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),
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),
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));
108 as_navgraph_->start();
114 RosMoveBaseThread::stop()
118 if (exec_type_ == NAVGRAPH) {
124 as_goal_.setAborted(create_result());
126 exec_running_ =
false;
130 RosMoveBaseThread::action_goal_cb(MoveBaseServer::GoalHandle goal, ExecType ext)
133 if (exec_running_ && exec_as_) {
134 as_goal_.setAborted(create_result(),
"Replaced by new goal");
137 goal_pose_ = goal.getGoal()->target_pose;
139 exec_request_ =
true;
146 RosMoveBaseThread::action_cancel_cb(MoveBaseServer::GoalHandle goal)
150 goal.setCanceled(create_result(),
"Abort on request");
154 RosMoveBaseThread::message_cb(geometry_msgs::PoseStampedConstPtr goal_pose, ExecType ext)
157 goal_pose_ = *goal_pose;
159 exec_request_ =
true;
163 move_base_msgs::MoveBaseResult
164 RosMoveBaseThread::create_result()
166 return move_base_msgs::MoveBaseResult();
169 move_base_msgs::MoveBaseFeedback
170 RosMoveBaseThread::create_feedback()
172 move_base_msgs::MoveBaseFeedback feedback;
176 transform_pose_in.
frame_id = cfg_base_frame_;
182 feedback.base_position.header.stamp = ros::Time(transform_pose_out.
stamp.
get_sec(),
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();
194 "Failed to determine fixed frame pose");
204 exec_request_ =
false;
206 if (exec_type_ == NAVGRAPH && ! nav_navgraph_if_->
has_writer()) {
210 }
else if (exec_type_ == COLLI && ! nav_colli_if_->
has_writer()) {
216 std::string target_frame;
217 if (exec_type_ == COLLI) {
219 target_frame = cfg_base_frame_;
220 nav_if_ = nav_colli_if_;
221 }
else if (exec_type_ == NAVGRAPH) {
223 target_frame = cfg_fixed_frame_;
224 nav_if_ = nav_navgraph_if_;
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);
249 "Failed to transform target pose (%s -> %s), cannot move",
250 target_pose_in.
frame_id.c_str(), target_frame.c_str());
256 tf::Vector3 &pos = target_pose_out.getOrigin();
260 tf::get_yaw(target_pose_out.getRotation()));
263 msg->
x(), msg->
y(), msg->
orientation(), target_frame.c_str());
267 exec_running_ =
true;
268 exec_msgid_ = msg->
id();
279 if (exec_as_) as_goal_.publishFeedback(create_feedback());
281 if (nav_if_->
msgid() == exec_msgid_ && nav_if_->
is_final()) {
282 exec_running_ =
false;
286 as_goal_.setSucceeded(create_result(),
"Target reached");
289 as_goal_.setAborted(create_result(),
"Failed to reach target");
RosMoveBaseThread()
Contructor.
std::string frame_id
The frame_id associated this data.
unsigned int id() const
Get message ID.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
virtual void init()
Initialize the thread.
float y() const
Get y value.
virtual void loop()
Code to execute in the thread.
Fawkes library namespace.
A class for handling time.
Thread class encapsulation of pthreads.
uint32_t error_code() const
Get error_code value.
Mutex * loop_mutex
Mutex that is used to protect a call to loop().
Logger * logger
This is the Logger member used to access the logger.
float orientation() const
Get orientation value.
Thread aspect to use blocked timing.
fawkes::Time stamp
The timestamp associated with this data.
bool is_final() const
Get final value.
Base class for exceptions in Fawkes.
void read()
Read from BlackBoard into local copy.
CartesianGotoMessage Fawkes BlackBoard Interface Message.
long get_nsec() const
Get nanoseconds.
bool has_writer() const
Check if there is a writer for the interface.
const char * name() const
Get name of thread.
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.
unsigned int msgq_enqueue(Message *message)
Enqueue message at end of queue.
long get_sec() const
Get seconds.
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.
float x() const
Get x value.
virtual void finalize()
Finalize the thread.
uint32_t msgid() const
Get msgid value.
Configuration * config
This is the Configuration member used to access the configuration.
void append(const char *format,...)
Append messages to the message list.
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.
StopMessage Fawkes BlackBoard Interface Message.
NavigatorInterface Fawkes BlackBoard Interface.
virtual void close(Interface *interface)=0
Close interface.