39 #include <move_base_msgs/RecoveryStatus.h> 42 #include <boost/algorithm/string.hpp> 43 #include <boost/thread.hpp> 45 #include <geometry_msgs/Twist.h> 54 planner_costmap_ros_(NULL), controller_costmap_ros_(NULL),
55 bgp_loader_(
"nav_core",
"nav_core::BaseGlobalPlanner"),
56 blp_loader_(
"nav_core",
"nav_core::BaseLocalPlanner"),
57 recovery_loader_(
"nav_core",
"nav_core::RecoveryBehavior"),
58 planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL),
59 runPlanner_(false), setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false) {
69 std::string global_planner, local_planner;
70 private_nh.
param(
"base_global_planner", global_planner, std::string(
"navfn/NavfnROS"));
71 private_nh.
param(
"base_local_planner", local_planner, std::string(
"base_local_planner/TrajectoryPlannerROS"));
88 planner_plan_ =
new std::vector<geometry_msgs::PoseStamped>();
89 latest_plan_ =
new std::vector<geometry_msgs::PoseStamped>();
100 action_goal_pub_ = action_nh.advertise<move_base_msgs::MoveBaseActionGoal>(
"goal", 1);
101 recovery_status_pub_= action_nh.advertise<move_base_msgs::RecoveryStatus>(
"recovery_status", 1);
128 ROS_FATAL(
"Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", global_planner.c_str(), ex.what());
139 ROS_INFO(
"Created local_planner %s", local_planner.c_str());
142 ROS_FATAL(
"Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", local_planner.c_str(), ex.what());
178 dynamic_reconfigure::Server<move_base::MoveBaseConfig>::CallbackType cb = boost::bind(&
MoveBase::reconfigureCB,
this, _1, _2);
179 dsrv_->setCallback(cb);
195 if(config.restore_defaults) {
198 config.restore_defaults =
false;
224 if(config.base_global_planner !=
last_config_.base_global_planner) {
227 ROS_INFO(
"Loading global planner %s", config.base_global_planner.c_str());
243 ROS_FATAL(
"Failed to create the %s planner, are you sure it is properly registered and that the \ 244 containing library is built? Exception: %s", config.base_global_planner.c_str(), ex.what());
246 config.base_global_planner =
last_config_.base_global_planner;
250 if(config.base_local_planner !=
last_config_.base_local_planner){
262 ROS_FATAL(
"Failed to create the %s planner, are you sure it is properly registered and that the \ 263 containing library is built? Exception: %s", config.base_local_planner.c_str(), ex.what());
265 config.base_local_planner =
last_config_.base_local_planner;
276 ROS_DEBUG_NAMED(
"move_base",
"In ROS goal callback, wrapping the PoseStamped in the action message and re-sending to the server.");
277 move_base_msgs::MoveBaseActionGoal action_goal;
279 action_goal.goal.target_pose = *goal;
285 geometry_msgs::PoseStamped global_pose;
290 std::vector<geometry_msgs::Point> clear_poly;
291 double x = global_pose.pose.position.x;
292 double y = global_pose.pose.position.y;
293 geometry_msgs::Point pt;
295 pt.x = x - size_x / 2;
296 pt.y = y - size_y / 2;
297 clear_poly.push_back(pt);
299 pt.x = x + size_x / 2;
300 pt.y = y - size_y / 2;
301 clear_poly.push_back(pt);
303 pt.x = x + size_x / 2;
304 pt.y = y + size_y / 2;
305 clear_poly.push_back(pt);
307 pt.x = x - size_x / 2;
308 pt.y = y + size_y / 2;
309 clear_poly.push_back(pt);
317 x = global_pose.pose.position.x;
318 y = global_pose.pose.position.y;
320 pt.x = x - size_x / 2;
321 pt.y = y - size_y / 2;
322 clear_poly.push_back(pt);
324 pt.x = x + size_x / 2;
325 pt.y = y - size_y / 2;
326 clear_poly.push_back(pt);
328 pt.x = x + size_x / 2;
329 pt.y = y + size_y / 2;
330 clear_poly.push_back(pt);
332 pt.x = x - size_x / 2;
333 pt.y = y + size_y / 2;
334 clear_poly.push_back(pt);
352 ROS_ERROR(
"move_base must be in an inactive state to make a plan for an external user");
357 ROS_ERROR(
"move_base cannot make a plan for you because it doesn't have a costmap");
361 geometry_msgs::PoseStamped
start;
363 if(req.start.header.frame_id.empty())
365 geometry_msgs::PoseStamped global_pose;
367 ROS_ERROR(
"move_base cannot make a plan for you because it could not get the start pose of the robot");
383 std::vector<geometry_msgs::PoseStamped> global_plan;
384 if(!
planner_->makePlan(start, req.goal, global_plan) || global_plan.empty()){
385 ROS_DEBUG_NAMED(
"move_base",
"Failed to find a plan to exact goal of (%.2f, %.2f), searching for a feasible goal within tolerance",
386 req.goal.pose.position.x, req.goal.pose.position.y);
389 geometry_msgs::PoseStamped p;
391 bool found_legal =
false;
393 float search_increment = resolution*3.0;
394 if(req.tolerance > 0.0 && req.tolerance < search_increment) search_increment = req.tolerance;
395 for(
float max_offset = search_increment; max_offset <= req.tolerance && !found_legal; max_offset += search_increment) {
396 for(
float y_offset = 0; y_offset <= max_offset && !found_legal; y_offset += search_increment) {
397 for(
float x_offset = 0; x_offset <= max_offset && !found_legal; x_offset += search_increment) {
400 if(x_offset < max_offset-1e-9 && y_offset < max_offset-1e-9)
continue;
403 for(
float y_mult = -1.0; y_mult <= 1.0 + 1e-9 && !found_legal; y_mult += 2.0) {
406 if(y_offset < 1e-9 && y_mult < -1.0 + 1e-9)
continue;
408 for(
float x_mult = -1.0; x_mult <= 1.0 + 1e-9 && !found_legal; x_mult += 2.0) {
409 if(x_offset < 1e-9 && x_mult < -1.0 + 1e-9)
continue;
411 p.pose.position.y = req.goal.pose.position.y + y_offset * y_mult;
412 p.pose.position.x = req.goal.pose.position.x + x_offset * x_mult;
414 if(
planner_->makePlan(start, p, global_plan)){
415 if(!global_plan.empty()){
420 global_plan.push_back(req.goal);
424 ROS_DEBUG_NAMED(
"move_base",
"Found a plan to point (%.2f, %.2f)", p.pose.position.x, p.pose.position.y);
429 ROS_DEBUG_NAMED(
"move_base",
"Failed to find a plan to point (%.2f, %.2f)", p.pose.position.x, p.pose.position.y);
439 resp.plan.poses.resize(global_plan.size());
440 for(
unsigned int i = 0; i < global_plan.size(); ++i){
441 resp.plan.poses[i] = global_plan[i];
474 bool MoveBase::makePlan(
const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan){
482 ROS_ERROR(
"Planner costmap ROS is NULL, unable to create global plan");
487 geometry_msgs::PoseStamped global_pose;
489 ROS_WARN(
"Unable to get starting pose of robot, unable to create global plan");
493 const geometry_msgs::PoseStamped&
start = global_pose;
496 if(!
planner_->makePlan(start, goal, plan) || plan.empty()){
497 ROS_DEBUG_NAMED(
"move_base",
"Failed to find a plan to point (%.2f, %.2f)", goal.pose.position.x, goal.pose.position.y);
505 geometry_msgs::Twist cmd_vel;
506 cmd_vel.linear.x = 0.0;
507 cmd_vel.linear.y = 0.0;
508 cmd_vel.angular.z = 0.0;
514 if(!std::isfinite(q.x) || !std::isfinite(q.y) || !std::isfinite(q.z) || !std::isfinite(q.w)){
515 ROS_ERROR(
"Quaternion has nans or infs... discarding as a navigation goal");
523 ROS_ERROR(
"Quaternion has length close to zero... discarding as navigation goal");
530 tf2::Vector3 up(0, 0, 1);
534 if(fabs(dot - 1) > 1e-3){
535 ROS_ERROR(
"Quaternion is invalid... for navigation the z-axis of the quaternion must be close to vertical.");
544 geometry_msgs::PoseStamped goal_pose, global_pose;
545 goal_pose = goal_pose_msg;
552 tf_.
transform(goal_pose_msg, global_pose, global_frame);
555 ROS_WARN(
"Failed to transform the goal pose from %s into the %s frame: %s",
556 goal_pose.header.frame_id.c_str(), global_frame.c_str(), ex.what());
557 return goal_pose_msg;
573 bool wait_for_wake =
false;
579 ROS_DEBUG_NAMED(
"move_base_plan_thread",
"Planner thread is suspending");
581 wait_for_wake =
false;
597 std::vector<geometry_msgs::PoseStamped>* temp_plan =
planner_plan_;
606 ROS_DEBUG_NAMED(
"move_base_plan_thread",
"Generated a plan from the base_global_planner");
644 wait_for_wake =
true;
654 as_->
setAborted(move_base_msgs::MoveBaseResult(),
"Aborting on goal because it was sent with an invalid quaternion");
669 std::vector<geometry_msgs::PoseStamped> global_plan;
673 ROS_DEBUG_NAMED(
"move_base",
"Starting up costmaps that were shut down previously");
700 as_->
setAborted(move_base_msgs::MoveBaseResult(),
"Aborting on goal because it was sent with an invalid quaternion");
718 ROS_DEBUG_NAMED(
"move_base",
"move_base has received a goal of x: %.2f, y: %.2f", goal.pose.position.x, goal.pose.position.y);
756 ROS_DEBUG_NAMED(
"move_base",
"The global frame for move_base has changed, new frame: %s, new goal position x: %.2f, y: %.2f", goal.header.frame_id.c_str(), goal.pose.position.x, goal.pose.position.y);
794 as_->
setAborted(move_base_msgs::MoveBaseResult(),
"Aborting on the goal because the node has been killed");
798 double MoveBase::distance(
const geometry_msgs::PoseStamped& p1,
const geometry_msgs::PoseStamped& p2)
800 return hypot(p1.pose.position.x - p2.pose.position.x, p1.pose.position.y - p2.pose.position.y);
806 geometry_msgs::Twist cmd_vel;
809 geometry_msgs::PoseStamped global_pose;
811 const geometry_msgs::PoseStamped& current_position = global_pose;
814 move_base_msgs::MoveBaseFeedback feedback;
815 feedback.base_position = current_position;
854 ROS_ERROR(
"Failed to pass global plan to the controller, aborting.");
862 as_->
setAborted(move_base_msgs::MoveBaseResult(),
"Failed to pass global plan to the controller.");
880 ROS_DEBUG_NAMED(
"move_base",
"Waiting for plan, in the planning state.");
888 if(
tc_->isGoalReached()){
913 if(
tc_->computeVelocityCommands(cmd_vel)){
914 ROS_DEBUG_NAMED(
"move_base",
"Got a valid command from the local planner: %.3lf, %.3lf, %.3lf",
915 cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z );
923 ROS_DEBUG_NAMED(
"move_base",
"The local planner could not find a valid plan.");
958 move_base_msgs::RecoveryStatus msg;
959 msg.pose_stamped = current_position;
981 ROS_DEBUG_NAMED(
"move_base_recovery",
"All recovery behaviors have failed, locking the planner and disabling it.");
987 ROS_DEBUG_NAMED(
"move_base_recovery",
"Something should abort after this.");
990 ROS_ERROR(
"Aborting because a valid control could not be found. Even after executing all recovery behaviors");
991 as_->
setAborted(move_base_msgs::MoveBaseResult(),
"Failed to find a valid control. Even after executing recovery behaviors.");
994 ROS_ERROR(
"Aborting because a valid plan could not be found. Even after executing all recovery behaviors");
995 as_->
setAborted(move_base_msgs::MoveBaseResult(),
"Failed to find a valid plan. Even after executing recovery behaviors.");
998 ROS_ERROR(
"Aborting because the robot appears to be oscillating over and over. Even after executing all recovery behaviors");
999 as_->
setAborted(move_base_msgs::MoveBaseResult(),
"Robot is oscillating. Even after executing recovery behaviors.");
1006 ROS_ERROR(
"This case should never be reached, something is wrong, aborting");
1012 as_->
setAborted(move_base_msgs::MoveBaseResult(),
"Reached a case that should not be hit in move_base. This is a bug, please report it.");
1022 if(node.
getParam(
"recovery_behaviors", behavior_list)){
1024 for(
int i = 0; i < behavior_list.
size(); ++i){
1026 if(behavior_list[i].hasMember(
"name") && behavior_list[i].
hasMember(
"type")){
1028 for(
int j = i + 1; j < behavior_list.
size(); j++){
1030 if(behavior_list[j].hasMember(
"name") && behavior_list[j].
hasMember(
"type")){
1031 std::string name_i = behavior_list[i][
"name"];
1032 std::string name_j = behavior_list[j][
"name"];
1033 if(name_i == name_j){
1034 ROS_ERROR(
"A recovery behavior with the name %s already exists, this is not allowed. Using the default recovery behaviors instead.",
1043 ROS_ERROR(
"Recovery behaviors must have a name and a type and this does not. Using the default recovery behaviors instead.");
1048 ROS_ERROR(
"Recovery behaviors must be specified as maps, but they are XmlRpcType %d. We'll use the default recovery behaviors instead.",
1049 behavior_list[i].getType());
1055 for(
int i = 0; i < behavior_list.
size(); ++i){
1060 for(
unsigned int i = 0; i < classes.size(); ++i){
1063 ROS_WARN(
"Recovery behavior specifications should now include the package name. You are using a deprecated API. Please switch from %s to %s in your yaml file.",
1064 std::string(behavior_list[i][
"type"]).c_str(), classes[i].c_str());
1065 behavior_list[i][
"type"] = classes[i];
1074 if(behavior.get() == NULL){
1075 ROS_ERROR(
"The ClassLoader returned a null pointer without throwing an exception. This should not happen");
1085 ROS_ERROR(
"Failed to load a plugin. Using default recovery behaviors. Error: %s", ex.what());
1091 ROS_ERROR(
"The recovery behavior specification must be a list, but is of XmlRpcType %d. We'll use the default recovery behaviors instead.",
1141 ROS_FATAL(
"Failed to load a plugin. This should not happen on default recovery behaviors. Error: %s", ex.what());
1170 geometry_msgs::PoseStamped robot_pose;
1183 ROS_ERROR_THROTTLE(1.0,
"No Transform available Error looking up robot pose: %s\n", ex.what());
1188 ROS_ERROR_THROTTLE(1.0,
"Connectivity Error looking up robot pose: %s\n", ex.what());
1193 ROS_ERROR_THROTTLE(1.0,
"Extrapolation Error looking up robot pose: %s\n", ex.what());
1201 "Current time: %.4f, pose stamp: %.4f, tolerance: %.4f", costmap->
getName().c_str(),
void executeCb(const move_base_msgs::MoveBaseGoalConstPtr &move_base_goal)
boost::condition_variable_any planner_cond_
boost::thread * planner_thread_
void wakePlanner(const ros::TimerEvent &event)
This is used to wake the planner at periodic intervals.
ros::ServiceServer make_plan_srv_
double distance(const geometry_msgs::PoseStamped &p1, const geometry_msgs::PoseStamped &p2)
bool getRobotPose(geometry_msgs::PoseStamped &global_pose, costmap_2d::Costmap2DROS *costmap)
boost::shared_ptr< const Goal > acceptNewGoal()
void publishFeedback(const FeedbackConstPtr &feedback)
pluginlib::ClassLoader< nav_core::BaseLocalPlanner > blp_loader_
MoveBaseActionServer * as_
T & transform(const T &in, T &out, const std::string &target_frame, ros::Duration timeout=ros::Duration(0.0)) const
bool make_plan_clear_costmap_
bool isQuaternionValid(const geometry_msgs::Quaternion &q)
costmap_2d::Costmap2DROS * controller_costmap_ros_
int32_t max_planning_retries_
bool planService(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &resp)
A service call that can be made when the action is inactive that will return a plan.
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
bool make_plan_add_unreachable_goal_
pluginlib::ClassLoader< nav_core::BaseGlobalPlanner > bgp_loader_
boost::recursive_mutex configuration_mutex_
double planner_frequency_
boost::shared_ptr< T > createInstance(const std::string &lookup_name)
std::string getGlobalFrameID()
ros::Publisher action_goal_pub_
virtual ~MoveBase()
Destructor - Cleans up.
ROSCPP_DECL const std::string & getName()
INLINE Rall1d< T, V, S > hypot(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
ros::Time last_oscillation_reset_
bool recovery_behavior_enabled_
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
actionlib::SimpleActionServer< move_base_msgs::MoveBaseAction > MoveBaseActionServer
std::string global_frame_
static const unsigned char FREE_SPACE
std::string getName() const
doubleAcc dot(const VectorAcc &lhs, const VectorAcc &rhs)
std::vector< geometry_msgs::PoseStamped > * planner_plan_
void setSucceeded(const Result &result=Result(), const std::string &text=std::string(""))
boost::shared_ptr< nav_core::BaseGlobalPlanner > planner_
tf2Scalar getAngle() const
bool clearing_rotation_allowed_
#define ROS_DEBUG_NAMED(name,...)
unsigned int recovery_index_
dynamic_reconfigure::Server< move_base::MoveBaseConfig > * dsrv_
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
Type const & getType() const
void publish(const boost::shared_ptr< M > &message) const
RecoveryTrigger recovery_trigger_
void setPreempted(const Result &result=Result(), const std::string &text=std::string(""))
ros::Publisher recovery_status_pub_
ros::ServiceServer clear_costmaps_srv_
#define ROS_WARN_THROTTLE(period,...)
move_base::MoveBaseConfig default_config_
bool getParam(const std::string &key, std::string &s) const
std::vector< std::string > getDeclaredClasses()
#define ROS_ERROR_THROTTLE(period,...)
ros::Time last_valid_plan_
void loadDefaultRecoveryBehaviors()
Loads the default recovery behaviors for the navigation stack.
std::vector< boost::shared_ptr< nav_core::RecoveryBehavior > > recovery_behaviors_
bool loadRecoveryBehaviors(ros::NodeHandle node)
Load the recovery behaviors for the navigation stack from the parameter server.
void reconfigureCB(move_base::MoveBaseConfig &config, uint32_t level)
double controller_patience_
void goalCB(const geometry_msgs::PoseStamped::ConstPtr &goal)
pluginlib::ClassLoader< nav_core::RecoveryBehavior > recovery_loader_
double controller_frequency_
bool isPreemptRequested()
bool setConvexPolygonCost(const std::vector< geometry_msgs::Point > &polygon, unsigned char cost_value)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
costmap_2d::Costmap2DROS * planner_costmap_ros_
Duration cycleTime() const
double getTransformTolerance() const
boost::recursive_mutex planner_mutex_
virtual std::string getName(const std::string &lookup_name)
void publishZeroVelocity()
Publishes a velocity command of zero to the base.
geometry_msgs::PoseStamped goalToGlobalFrame(const geometry_msgs::PoseStamped &goal_pose_msg)
double oscillation_distance_
boost::shared_ptr< nav_core::BaseLocalPlanner > tc_
void resetState()
Reset the state of the move_base action and send a zero velocity command to the base.
void setAborted(const Result &result=Result(), const std::string &text=std::string(""))
ros::Publisher current_goal_pub_
geometry_msgs::PoseStamped planner_goal_
ros::Subscriber goal_sub_
void setParam(const std::string &key, const XmlRpc::XmlRpcValue &v) const
MoveBase(tf2_ros::Buffer &tf)
Constructor for the actions.
std::vector< geometry_msgs::PoseStamped > * latest_plan_
move_base::MoveBaseConfig last_config_
double getResolution() const
tf2Scalar length2() const
std::vector< std::string > recovery_behavior_names_
bool clearCostmapsService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp)
A service call that clears the costmaps of obstacles.
uint32_t planning_retries_
double circumscribed_radius_
std::string robot_base_frame_
bool makePlan(const geometry_msgs::PoseStamped &goal, std::vector< geometry_msgs::PoseStamped > &plan)
Make a new global plan.
double conservative_reset_dist_
bool executeCycle(geometry_msgs::PoseStamped &goal, std::vector< geometry_msgs::PoseStamped > &global_plan)
Performs a control cycle.
virtual bool isClassAvailable(const std::string &lookup_name)
ros::Time last_valid_control_
double oscillation_timeout_
geometry_msgs::PoseStamped oscillation_pose_
void clearCostmapWindows(double size_x, double size_y)
Clears obstacles within a window around the robot.
bool isNewGoalAvailable()
bool hasMember(const std::string &name) const
std::vector< geometry_msgs::PoseStamped > * controller_plan_