对MoveBase源码的阅读理解

对MoveBase源码的阅读理解

#include <move_base/move_base.h>
#include <move_base_msgs/RecoveryStatus.h>
#include

#include <boost/algorithm/string.hpp>
#include <boost/thread.hpp>

#include <geometry_msgs/Twist.h>

#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

//在Movebase主体中，各层地图的更新被启动，Action的回调函数触发全局规划线程，若成功，则将全局规划结果传入局部规划器，
//循环进行局部规划，得到速度指令，控制机器人前进，直到到达目标。其间，需要判断机器人是否到达终点（若是则规划停止）、
//机器人是否状态异常如发生震荡行为（若是则进入恢复行为）、机器人是否超时（若是则停止规划发布零速，否则重新规划）等等。
//这个主体是一个大的调用框架，保证了运动规划的正常运行，具体算法在各子过程中分别实

//每次有goal发布的时候，都会去调用executeCb,executeCb会去调用executeCycle
//进行local plan,发布cmd_vel的topic，根据小车处于的状态，进行相应的实现(会进行plan,control,clear obstacle)

namespace move_base {

MoveBase::MoveBase(tf2_ros::Buffer& tf) :
tf_(tf),//坐标变换TF2的接口对象
as_(NULL),//Action服务器;
//全局规划器的代价地图对象 局部规划器所用的代价地图对象
planner_costmap_ros_(NULL), controller_costmap_ros_(NULL),
bgp_loader_(“nav_core”, “nav_core::BaseGlobalPlanner”),//装载全局规划器插件的工具，
blp_loader_(“nav_core”, “nav_core::BaseLocalPlanner”),//装载局部规划器插件的工具
recovery_loader_(“nav_core”, “nav_core::RecoveryBehavior”),
//三个记录规划结果的缓存
planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL),
//一些控制和反映MoveBase系统状态的布尔变量
runPlanner_(false), setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false) {

//----------------------------------------------------------------------------------------------------
//新建Action服务器，指针as_，绑定回调函数MoveBase::executeCb
//as_指向action服务器，当执行as_->start()时调用MoveBase::executeCb函数
//                                                                                                            当前action名                             client端访问server端的回调函数   是否自动开启server
as_ = new MoveBaseActionServer(ros::NodeHandle(), "move_base", boost::bind(&MoveBase::executeCb, this, _1), false);
//----------------------------------------------------------------------------------------------------

//定义了两个ROS的句柄
ros::NodeHandle private_nh("~");//用于获取节点参数
ros::NodeHandle nh;//用于订阅和发布主题

recovery_trigger_ = PLANNING_R;//全局规划失败

//从参数服务器设置用到的全局规划器、局部规划器、机器人底盘坐标系等，初始化三个用于存放plan的buffer
//从参数服务器加载用户输入的参数，如果没有，设置为默认值（第三个参数）
std::string global_planner, local_planner;
private_nh.param("base_global_planner", global_planner, std::string("navfn/NavfnROS"));
private_nh.param("base_local_planner", local_planner, std::string("base_local_planner/TrajectoryPlannerROS"));
private_nh.param("global_costmap/robot_base_frame", robot_base_frame_, std::string("base_link"));
private_nh.param("global_costmap/global_frame", global_frame_, std::string("map"));
private_nh.param("planner_frequency", planner_frequency_, 0.0);
private_nh.param("controller_frequency", controller_frequency_, 20.0);
private_nh.param("planner_patience", planner_patience_, 5.0);
private_nh.param("controller_patience", controller_patience_, 15.0);
private_nh.param("max_planning_retries", max_planning_retries_, -1);  // disabled by default

private_nh.param("oscillation_timeout", oscillation_timeout_, 0.0);
private_nh.param("oscillation_distance", oscillation_distance_, 0.5);

// parameters of make_plan service
private_nh.param("make_plan_clear_costmap", make_plan_clear_costmap_, true);
private_nh.param("make_plan_add_unreachable_goal", make_plan_add_unreachable_goal_, true);

//初始化三个plan的“缓冲池”数组，轨迹规划结果缓存
planner_plan_ = new std::vector<geometry_msgs::PoseStamped>();
latest_plan_ = new std::vector<geometry_msgs::PoseStamped>();
controller_plan_ = new std::vector<geometry_msgs::PoseStamped>();

//创建规划器线程，在该线程里运行planThread函数
planner_thread_ = new boost::thread(boost::bind(&MoveBase::planThread, this));

//底盘的速度发布者
vel_pub_ = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
current_goal_pub_ = private_nh.advertise<geometry_msgs::PoseStamped>("current_goal", 0 );

ros::NodeHandle action_nh("move_base");
action_goal_pub_ = action_nh.advertise<move_base_msgs::MoveBaseActionGoal>("goal", 1);
recovery_status_pub_= action_nh.advertise<move_base_msgs::RecoveryStatus>("recovery_status", 1);

//接收目标点，并在回调函数goalCB中处理在RVIZ中发布的/move_base_simple/goal话题消息，转换成move_base服务端回调函数的参数类型//
//如果是用的定点导航的action客户端的话就不需要这个回调函数进行处理/
ros::NodeHandle simple_nh("move_base_simple");
goal_sub_ = simple_nh.subscribe<geometry_msgs::PoseStamped>("goal", 1, boost::bind(&MoveBase::goalCB, this, _1));


//加载代价地图的参数（内切、外接、清理半径等），假设机器人的半径和costmap规定的一致
private_nh.param("local_costmap/inscribed_radius", inscribed_radius_, 0.325);
private_nh.param("local_costmap/circumscribed_radius", circumscribed_radius_, 0.46);
private_nh.param("clearing_radius", clearing_radius_, circumscribed_radius_);
private_nh.param("conservative_reset_dist", conservative_reset_dist_, 3.0);
//一些标志位的设置
private_nh.param("shutdown_costmaps", shutdown_costmaps_, false);
private_nh.param("clearing_rotation_allowed", clearing_rotation_allowed_, true);
private_nh.param("recovery_behavior_enabled", recovery_behavior_enabled_, true);


//----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
//通过模块costmap_2d构建了全局代价地图对象，并通过其成员函数pause()暂停运行，将在必要的功能模块初始化结束之后通过成员接口start()开启
planner_costmap_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tf_);
planner_costmap_ros_->pause();

//通过在一开始获取的全局规划器名称global_planner构造全局规划器，并用刚刚构建的全局代价地图完成对其的初始化操作。

  //初始化全局规划器，planner_指针
try {
  planner_ = bgp_loader_.createInstance(global_planner);
  planner_->initialize(bgp_loader_.getName(global_planner), planner_costmap_ros_);
} catch (const pluginlib::PluginlibException& ex) {
  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());
  exit(1);
}

//以类似的套路，构建局部代价地图和局部规划器的对象
controller_costmap_ros_ = new costmap_2d::Costmap2DROS("local_costmap", tf_);
controller_costmap_ros_->pause();

//初始化本地规划器，tc_指针
try {
  tc_ = blp_loader_.createInstance(local_planner);
  ROS_INFO("Created local_planner %s", local_planner.c_str());
  //初始化局部路径
  tc_->initialize(blp_loader_.getName(local_planner), &tf_, controller_costmap_ros_);
} catch (const pluginlib::PluginlibException& ex) {
  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());
  exit(1);
}

//构建了必要的功能模块之后，就可以开启代价地图的计算了，根据传感器数据动态更新全局和本地的代价地图
planner_costmap_ros_->start();
controller_costmap_ros_->start();
//------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------

//开启"make_plan"和"clear_costmaps"的服务
make_plan_srv_ = private_nh.advertiseService("make_plan", &MoveBase::planService, this);
clear_costmaps_srv_ = private_nh.advertiseService("clear_costmaps", &MoveBase::clearCostmapsService, this);

//下面这两段代码是MoveBase的一些运行特性，涉及到是否使用代价地图，以及恢复行为
if(shutdown_costmaps_){
  ROS_DEBUG_NAMED("move_base","Stopping costmaps initially");
  planner_costmap_ros_->stop();
  controller_costmap_ros_->stop();
}

//没有设置具体的恢复行为，就加载默认恢复行为
if(!loadRecoveryBehaviors(private_nh)){
  loadDefaultRecoveryBehaviors();
}

//至此，初始化工作已经基本完成了，下面对系统的状态进行初始化，然后Action服务器对象as_开启服务
state_ = PLANNING;

//we'll start executing recovery behaviors at the beginning of our list
recovery_index_ = 0;

//构建对象as_的时候提供了一个回调函数executeCb，每当有新的Action请求的时候， 都会调用这一回调函数，进行导航控制
//调用action server的start函数，服务器开始运行
as_->start();

//作为一个松耦合的框架，MoveBase还希望能够动态的修改系统运行参数，所以就有了下面这些动态配置器的初始化过程
dsrv_ = new dynamic_reconfigure::Server<move_base::MoveBaseConfig>(ros::NodeHandle("~"));
dynamic_reconfigure::Server<move_base::MoveBaseConfig>::CallbackType cb = boost::bind(&MoveBase::reconfigureCB, this, _1, _2);
dsrv_->setCallback(cb);

}

void MoveBase::reconfigureCB(move_base::MoveBaseConfig &config, uint32_t level){
boost::recursive_mutex::scoped_lock l(configuration_mutex_);

//The first time we're called, we just want to make sure we have the
//original configuration
if(!setup_)
{
  last_config_ = config;
  default_config_ = config;
  setup_ = true;
  return;
}

if(config.restore_defaults) {
  config = default_config_;
  //if someone sets restore defaults on the parameter server, prevent looping
  config.restore_defaults = false;
}

if(planner_frequency_ != config.planner_frequency)
{
  planner_frequency_ = config.planner_frequency;
  p_freq_change_ = true;
}

if(controller_frequency_ != config.controller_frequency)
{
  controller_frequency_ = config.controller_frequency;
  c_freq_change_ = true;
}

planner_patience_ = config.planner_patience;
controller_patience_ = config.controller_patience;
max_planning_retries_ = config.max_planning_retries;
conservative_reset_dist_ = config.conservative_reset_dist;

recovery_behavior_enabled_ = config.recovery_behavior_enabled;
clearing_rotation_allowed_ = config.clearing_rotation_allowed;
shutdown_costmaps_ = config.shutdown_costmaps;

oscillation_timeout_ = config.oscillation_timeout;
oscillation_distance_ = config.oscillation_distance;
if(config.base_global_planner != last_config_.base_global_planner) {
  boost::shared_ptr<nav_core::BaseGlobalPlanner> old_planner = planner_;
  //initialize the global planner
  ROS_INFO("Loading global planner %s", config.base_global_planner.c_str());
  try {
    planner_ = bgp_loader_.createInstance(config.base_global_planner);

    // wait for the current planner to finish planning
    boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);

    // Clean up before initializing the new planner
    planner_plan_->clear();
    latest_plan_->clear();
    controller_plan_->clear();
    resetState();
    planner_->initialize(bgp_loader_.getName(config.base_global_planner), planner_costmap_ros_);

    lock.unlock();
  } catch (const pluginlib::PluginlibException& ex) {
    ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the \
               containing library is built? Exception: %s", config.base_global_planner.c_str(), ex.what());
    planner_ = old_planner;
    config.base_global_planner = last_config_.base_global_planner;
  }
}

if(config.base_local_planner != last_config_.base_local_planner){
  boost::shared_ptr<nav_core::BaseLocalPlanner> old_planner = tc_;
  //create a local planner
  try {
    tc_ = blp_loader_.createInstance(config.base_local_planner);
    // Clean up before initializing the new planner
    planner_plan_->clear();
    latest_plan_->clear();
    controller_plan_->clear();
    resetState();
    tc_->initialize(blp_loader_.getName(config.base_local_planner), &tf_, controller_costmap_ros_);
  } catch (const pluginlib::PluginlibException& ex) {
    ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the \
               containing library is built? Exception: %s", config.base_local_planner.c_str(), ex.what());
    tc_ = old_planner;
    config.base_local_planner = last_config_.base_local_planner;
  }
}

make_plan_clear_costmap_ = config.make_plan_clear_costmap;
make_plan_add_unreachable_goal_ = config.make_plan_add_unreachable_goal;

last_config_ = config;

}

//将goal转换为move_base_msgs::MoveBaseActionGoal类型
void MoveBase::goalCB(const geometry_msgs::PoseStamped::ConstPtr& goal){
ROS_DEBUG_NAMED(“move_base”,“In ROS goal callback, wrapping the PoseStamped in the action message and re-sending to the server.”);
move_base_msgs::MoveBaseActionGoal action_goal;
action_goal.header.stamp = ros::Time::now();
action_goal.goal.target_pose = *goal;

action_goal_pub_.publish(action_goal);

}

void MoveBase::clearCostmapWindows(double size_x, double size_y){
geometry_msgs::PoseStamped global_pose;

//clear the planner's costmap
getRobotPose(global_pose, planner_costmap_ros_);

std::vector<geometry_msgs::Point> clear_poly;
double x = global_pose.pose.position.x;
double y = global_pose.pose.position.y;
geometry_msgs::Point pt;

pt.x = x - size_x / 2;
pt.y = y - size_y / 2;
clear_poly.push_back(pt);

pt.x = x + size_x / 2;
pt.y = y - size_y / 2;
clear_poly.push_back(pt);

pt.x = x + size_x / 2;
pt.y = y + size_y / 2;
clear_poly.push_back(pt);

pt.x = x - size_x / 2;
pt.y = y + size_y / 2;
clear_poly.push_back(pt);

planner_costmap_ros_->getCostmap()->setConvexPolygonCost(clear_poly, costmap_2d::FREE_SPACE);

//clear the controller's costmap
getRobotPose(global_pose, controller_costmap_ros_);

clear_poly.clear();
x = global_pose.pose.position.x;
y = global_pose.pose.position.y;

pt.x = x - size_x / 2;
pt.y = y - size_y / 2;
clear_poly.push_back(pt);

pt.x = x + size_x / 2;
pt.y = y - size_y / 2;
clear_poly.push_back(pt);

pt.x = x + size_x / 2;
pt.y = y + size_y / 2;
clear_poly.push_back(pt);

pt.x = x - size_x / 2;
pt.y = y + size_y / 2;
clear_poly.push_back(pt);

controller_costmap_ros_->getCostmap()->setConvexPolygonCost(clear_poly, costmap_2d::FREE_SPACE);

}

bool MoveBase::clearCostmapsService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp){
//clear the costmaps
boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock_controller(*(controller_costmap_ros_->getCostmap()->getMutex()));
controller_costmap_ros_->resetLayers();

boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock_planner(*(planner_costmap_ros_->getCostmap()->getMutex()));
planner_costmap_ros_->resetLayers();
return true;

}

bool MoveBase::planService(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &resp){
if(as_->isActive()){
ROS_ERROR(“move_base must be in an inactive state to make a plan for an external user”);
return false;
}
//make sure we have a costmap for our planner
if(planner_costmap_ros_ == NULL){
ROS_ERROR(“move_base cannot make a plan for you because it doesn’t have a costmap”);
return false;
}

geometry_msgs::PoseStamped start;
//if the user does not specify a start pose, identified by an empty frame id, then use the robot's pose
if(req.start.header.frame_id.empty())
{
    geometry_msgs::PoseStamped global_pose;
    if(!getRobotPose(global_pose, planner_costmap_ros_)){
      ROS_ERROR("move_base cannot make a plan for you because it could not get the start pose of the robot");
      return false;
    }
    start = global_pose;
}
else
{
    start = req.start;
}

if (make_plan_clear_costmap_) {
  //update the copy of the costmap the planner uses
  clearCostmapWindows(2 * clearing_radius_, 2 * clearing_radius_);
}

//first try to make a plan to the exact desired goal
std::vector<geometry_msgs::PoseStamped> global_plan;
if(!planner_->makePlan(start, req.goal, global_plan) || global_plan.empty()){
  ROS_DEBUG_NAMED("move_base","Failed to find a plan to exact goal of (%.2f, %.2f), searching for a feasible goal within tolerance",
      req.goal.pose.position.x, req.goal.pose.position.y);

  //search outwards for a feasible goal within the specified tolerance
  geometry_msgs::PoseStamped p;
  p = req.goal;
  bool found_legal = false;
  float resolution = planner_costmap_ros_->getCostmap()->getResolution();
  float search_increment = resolution*3.0;
  if(req.tolerance > 0.0 && req.tolerance < search_increment) search_increment = req.tolerance;
  for(float max_offset = search_increment; max_offset <= req.tolerance && !found_legal; max_offset += search_increment) {
    for(float y_offset = 0; y_offset <= max_offset && !found_legal; y_offset += search_increment) {
      for(float x_offset = 0; x_offset <= max_offset && !found_legal; x_offset += search_increment) {

        //don't search again inside the current outer layer
        if(x_offset < max_offset-1e-9 && y_offset < max_offset-1e-9) continue;

        //search to both sides of the desired goal
        for(float y_mult = -1.0; y_mult <= 1.0 + 1e-9 && !found_legal; y_mult += 2.0) {

          //if one of the offsets is 0, -1*0 is still 0 (so get rid of one of the two)
          if(y_offset < 1e-9 && y_mult < -1.0 + 1e-9) continue;

          for(float x_mult = -1.0; x_mult <= 1.0 + 1e-9 && !found_legal; x_mult += 2.0) {
            if(x_offset < 1e-9 && x_mult < -1.0 + 1e-9) continue;

            p.pose.position.y = req.goal.pose.position.y + y_offset * y_mult;
            p.pose.position.x = req.goal.pose.position.x + x_offset * x_mult;

            if(planner_->makePlan(start, p, global_plan)){
              if(!global_plan.empty()){

                if (make_plan_add_unreachable_goal_) {
                  //adding the (unreachable) original goal to the end of the global plan, in case the local planner can get you there
                  //(the reachable goal should have been added by the global planner)
                  global_plan.push_back(req.goal);
                }

                found_legal = true;
                ROS_DEBUG_NAMED("move_base", "Found a plan to point (%.2f, %.2f)", p.pose.position.x, p.pose.position.y);
                break;
              }
            }
            else{
              ROS_DEBUG_NAMED("move_base","Failed to find a plan to point (%.2f, %.2f)", p.pose.position.x, p.pose.position.y);
            }
          }
        }
      }
    }
  }
}

//copy the plan into a message to send out
resp.plan.poses.resize(global_plan.size());
for(unsigned int i = 0; i < global_plan.size(); ++i){
  resp.plan.poses[i] = global_plan[i];
}

return true;

}

MoveBase::~MoveBase(){
recovery_behaviors_.clear();

delete dsrv_;

if(as_ != NULL)
  delete as_;

if(planner_costmap_ros_ != NULL)
  delete planner_costmap_ros_;

if(controller_costmap_ros_ != NULL)
  delete controller_costmap_ros_;

planner_thread_->interrupt();
planner_thread_->join();

delete planner_thread_;

delete planner_plan_;
delete latest_plan_;
delete controller_plan_;

planner_.reset();
tc_.reset();

}

bool MoveBase::makePlan(const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan){
boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(planner_costmap_ros_->getCostmap()->getMutex()));

//make sure to set the plan to be empty initially
plan.clear();

//since this gets called on handle activate
if(planner_costmap_ros_ == NULL) {
  ROS_ERROR("Planner costmap ROS is NULL, unable to create global plan");
  return false;
}

//get the starting pose of the robot
geometry_msgs::PoseStamped global_pose;
if(!getRobotPose(global_pose, planner_costmap_ros_)) {
  ROS_WARN("Unable to get starting pose of robot, unable to create global plan");
  return false;
}

const geometry_msgs::PoseStamped& start = global_pose;

//if the planner fails or returns a zero length plan, planning failed
if(!planner_->makePlan(start, goal, plan) || plan.empty()){
  ROS_DEBUG_NAMED("move_base","Failed to find a  plan to point (%.2f, %.2f)", goal.pose.position.x, goal.pose.position.y);
  return false;
}

return true;

}

void MoveBase::publishZeroVelocity(){
geometry_msgs::Twist cmd_vel;
cmd_vel.linear.x = 0.0;
cmd_vel.linear.y = 0.0;
cmd_vel.angular.z = 0.0;
vel_pub_.publish(cmd_vel);
}

bool MoveBase::isQuaternionValid(const geometry_msgs::Quaternion& q){
//first we need to check if the quaternion has nan’s or infs
if(!std::isfinite(q.x) || !std::isfinite(q.y) || !std::isfinite(q.z) || !std::isfinite(q.w)){
ROS_ERROR(“Quaternion has nans or infs… discarding as a navigation goal”);
return false;
}

tf2::Quaternion tf_q(q.x, q.y, q.z, q.w);

//next, we need to check if the length of the quaternion is close to zero
if(tf_q.length2() < 1e-6){
  ROS_ERROR("Quaternion has length close to zero... discarding as navigation goal");
  return false;
}

//next, we'll normalize the quaternion and check that it transforms the vertical vector correctly
tf_q.normalize();

tf2::Vector3 up(0, 0, 1);

double dot = up.dot(up.rotate(tf_q.getAxis(), tf_q.getAngle()));

if(fabs(dot - 1) > 1e-3){
  ROS_ERROR("Quaternion is invalid... for navigation the z-axis of the quaternion must be close to vertical.");
  return false;
}

return true;

}

geometry_msgs::PoseStamped MoveBase::goalToGlobalFrame(const geometry_msgs::PoseStamped& goal_pose_msg){
std::string global_frame = planner_costmap_ros_->getGlobalFrameID();
geometry_msgs::PoseStamped goal_pose, global_pose;
goal_pose = goal_pose_msg;

//just get the latest available transform... for accuracy they should send
//goals in the frame of the planner
goal_pose.header.stamp = ros::Time();

try{
  tf_.transform(goal_pose_msg, global_pose, global_frame);
}
catch(tf2::TransformException& ex){
  ROS_WARN("Failed to transform the goal pose from %s into the %s frame: %s",
      goal_pose.header.frame_id.c_str(), global_frame.c_str(), ex.what());
  return goal_pose_msg;
}

return global_pose;

}

void MoveBase::wakePlanner(const ros::TimerEvent& event)
{
// we have slept long enough for rate
planner_cond_.notify_one();
}

void MoveBase::planThread(){
ROS_DEBUG_NAMED(“move_base_plan_thread”,“Starting planner thread…”);
ros::NodeHandle n;
ros::Timer timer;
bool wait_for_wake = false;
boost::unique_lockboost::recursive_mutex lock(planner_mutex_);
while(n.ok()){
//check if we should run the planner (the mutex is locked)
while(wait_for_wake || !runPlanner_){
//if we should not be running the planner then suspend this thread
ROS_DEBUG_NAMED(“move_base_plan_thread”,“Planner thread is suspending”);
planner_cond_.wait(lock);
wait_for_wake = false;
}
ros::Time start_time = ros::Time::now();

  //time to plan! get a copy of the goal and unlock the mutex
  geometry_msgs::PoseStamped temp_goal = planner_goal_;
  lock.unlock();
  ROS_DEBUG_NAMED("move_base_plan_thread","Planning...");

  //run planner
  planner_plan_->clear();
  bool gotPlan = n.ok() && makePlan(temp_goal, *planner_plan_);

  if(gotPlan){
    ROS_DEBUG_NAMED("move_base_plan_thread","Got Plan with %zu points!", planner_plan_->size());
    //pointer swap the plans under mutex (the controller will pull from latest_plan_)
    std::vector<geometry_msgs::PoseStamped>* temp_plan = planner_plan_;

    lock.lock();
    planner_plan_ = latest_plan_;
    latest_plan_ = temp_plan;
    last_valid_plan_ = ros::Time::now();
    planning_retries_ = 0;
    new_global_plan_ = true;

    ROS_DEBUG_NAMED("move_base_plan_thread","Generated a plan from the base_global_planner");

    //make sure we only start the controller if we still haven't reached the goal
    if(runPlanner_)
      state_ = CONTROLLING;
    if(planner_frequency_ <= 0)
      runPlanner_ = false;
    lock.unlock();
  }
  //if we didn't get a plan and we are in the planning state (the robot isn't moving)
  else if(state_==PLANNING){
    ROS_DEBUG_NAMED("move_base_plan_thread","No Plan...");
    ros::Time attempt_end = last_valid_plan_ + ros::Duration(planner_patience_);

    //check if we've tried to make a plan for over our time limit or our maximum number of retries
    //issue #496: we stop planning when one of the conditions is true, but if max_planning_retries_
    //is negative (the default), it is just ignored and we have the same behavior as ever
    lock.lock();
    planning_retries_++;
    if(runPlanner_ &&
       (ros::Time::now() > attempt_end || planning_retries_ > uint32_t(max_planning_retries_))){
      //we'll move into our obstacle clearing mode
      state_ = CLEARING;
      runPlanner_ = false;  // proper solution for issue #523
      publishZeroVelocity();
      recovery_trigger_ = PLANNING_R;
    }

    lock.unlock();
  }

  //take the mutex for the next iteration
  lock.lock();

  //setup sleep interface if needed
  if(planner_frequency_ > 0){
    ros::Duration sleep_time = (start_time + ros::Duration(1.0/planner_frequency_)) - ros::Time::now();
    if (sleep_time > ros::Duration(0.0)){
      wait_for_wake = true;
      timer = n.createTimer(sleep_time, &MoveBase::wakePlanner, this);
    }
  }
}

}

//在构造函数中，构造Action服务器对象as_的时候，为之绑定了一个回调函数executeCb。每当接收到一个Action目标(goal)的时候，都会调用该函数，它可以看作是整个move_base系统的业务逻辑入口
//executeCb是Action的回调函数，它是MoveBase控制流的主体，它调用了MoveBase内另外几个作为子部分的重要成员函数，
//输入参数为经过goalCB回调函数处理后的 /move_base/goal或者是action客户端直接发的goal

控制主体 MoveBase::executeCb | 收到目标，触发全局规划线程，循环执行局部规划 ///
void MoveBase::executeCb(const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal)
{
//如下面的代码片段所示，executeCb以goal消息指针为输入参数。在函数的一开始，首先检查目标点的位姿是否合法。
//只有当合法之后， 才通过MoveBase的成员函数goalToGlobalFram获取目标点在全局坐标系下的坐标
if(!isQuaternionValid(move_base_goal->target_pose.pose.orientation)){
as_->setAborted(move_base_msgs::MoveBaseResult(), “Aborting on goal because it was sent with an invalid quaternion”);
return;
}

//将目标位置转换到global坐标系下（geometry_msgs形式）
geometry_msgs::PoseStamped goal = goalToGlobalFrame(move_base_goal->target_pose);

publishZeroVelocity();

//成功接收到目标点后，就更新系统的运行状态，规划器的目标。因为MoveBase涉及到多个线程，存在一定的并行特性，
//所以在更新系统状态的时候，需要对其加锁
boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);

//用接收到的目标goal来更新全局变量，即全局规划目标，这个值在planThread中会被用来做全局规划的当前目标
planner_goal_ = goal;
runPlanner_ = true;
planner_cond_.notify_one();
lock.unlock();

//然后在话题"/move_base/current_goal"上发布当前目标点，同时构建一个局部变量global_plan用于进行规划
current_goal_pub_.publish(goal);
std::vector<geometry_msgs::PoseStamped> global_plan;

//构建一个定时器r用于按照指定的控制频率controller_frequency_进行控制
ros::Rate r(controller_frequency_);
if(shutdown_costmaps_){
  ROS_DEBUG_NAMED("move_base","Starting up costmaps that were shut down previously");
  planner_costmap_ros_->start();
  controller_costmap_ros_->start();
}

//we want to make sure that we reset the last time we had a valid plan and control
last_valid_control_ = ros::Time::now();
last_valid_plan_ = ros::Time::now();
last_oscillation_reset_ = ros::Time::now();
planning_retries_ = 0;

//---------------------------------------------------------------------------------------------------------------------------------------------
//全局规划完成，接下来循环调用executeCycle函数来控制机器人进行局部规划，完成相应跟随
ros::NodeHandle n;
while(n.ok())
{
  //c_freq_change_被初始化为false
  //在循环的一开始先通过变量c_freq_change_检查是否需要修改控制频率，并最终将之转换为ros::Rate的频率r
  //如果c_freq_change_即局部规划频率需要中途更改为真，用更改后的controller_frequency_来更新r值
  if(c_freq_change_)
  {
    ROS_INFO("Setting controller frequency to %.2f", controller_frequency_);
    r = ros::Rate(controller_frequency_);
    c_freq_change_ = false;
  }

  //然后，查看Action服务器的状态机。如果触发了抢占，需要相应作出处理，并更新as_的状态机。
  //如果是因为接收到新的目标而触发了抢占行为，我们将接受新目标并开始新的规划和控制。 对于其它抢占行为，我们认为取消了Action任务，将挂起退出
  if(as_->isPreemptRequested()){
    if(as_->isNewGoalAvailable()){
      //如果获得了新目标，接收并存储新目标，并将上述过程重新进行一遍
      move_base_msgs::MoveBaseGoal new_goal = *as_->acceptNewGoal();

      //检测四元数是否有效
      if(!isQuaternionValid(new_goal.target_pose.pose.orientation)){
        as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on goal because it was sent with an invalid quaternion");
        return;
      }

      //将新目标坐标转换到全局坐标系（默认/map）下
      goal = goalToGlobalFrame(new_goal.target_pose);

      //we'll make sure that we reset our state for the next execution cycle
      recovery_index_ = 0;
      state_ = PLANNING; //重设MoveBase状态为全局规划中

      //we have a new goal so make sure the planner is awake
      lock.lock();
      planner_goal_ = goal;
      runPlanner_ = true;
      planner_cond_.notify_one();
      lock.unlock();

      //全局规划成功后，发布新目标到current_goal话题上
      ROS_DEBUG_NAMED("move_base","move_base has received a goal of x: %.2f, y: %.2f", goal.pose.position.x, goal.pose.position.y);
      current_goal_pub_.publish(goal);

      //make sure to reset our timeouts and counters
      last_valid_control_ = ros::Time::now();
      last_valid_plan_ = ros::Time::now();
      last_oscillation_reset_ = ros::Time::now();
      planning_retries_ = 0;
    }
    //如果服务器的抢占是由于收到了取消行动的命令，重置服务器状态
    else {
      resetState();
      //action服务器清除相关内容，并调用setPreempted()函数
      ROS_DEBUG_NAMED("move_base","Move base preempting the current goal");
      as_->setPreempted();
      return;
    }
  }

  //MoveBase的框架还允许我们在控制过程中更新地图和全局坐标系，下面的代码片段检查到全局坐标系发生改变之后，
  //重新计算目标点在新坐标系下的位姿，并重置规划状态
  if(goal.header.frame_id != planner_costmap_ros_->getGlobalFrameID()){
    goal = goalToGlobalFrame(goal);

    //恢复行为索引重置为0，MoveBase状态置为全局规划中
    recovery_index_ = 0;
    state_ = PLANNING;

    //we have a new goal so make sure the planner is awake
    lock.lock();
    planner_goal_ = goal;
    runPlanner_ = true;
    planner_cond_.notify_one();
    lock.unlock();

    //publish the goal point to the visualizer
    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);
    current_goal_pub_.publish(goal);

    //make sure to reset our timeouts and counters
    last_valid_control_ = ros::Time::now();
    last_valid_plan_ = ros::Time::now();
    last_oscillation_reset_ = ros::Time::now();
    planning_retries_ = 0;
  }

  //for timing that gives real time even in simulation
  ros::WallTime start = ros::WallTime::now();

  //---------------------------------------------------------------------------------------------
  //在完成了各种情况的检测之后，MoveBase才开始执行真正的导航控制业务executeCycle。该函数返回一个布尔变量用于标志是否是否完成了Action任务，
  //调用executeCycle函数进行局部规划，传入目标和全局规划路线
  bool done = executeCycle(goal, global_plan);

  //若局部规划完成，结束
  if(done)
    return;

  //check if execution of the goal has completed in some way

  ros::WallDuration t_diff = ros::WallTime::now() - start;
  ROS_DEBUG_NAMED("move_base","Full control cycle time: %.9f\n", t_diff.toSec());

   //在业务循环的最后，通过ros::Rate对象消耗掉一个控制周期中剩余的时间，如果计时发生了偏差，将给出警告
  r.sleep();
  //cycleTime用来获取从r实例初始化到r实例调用sleep函数的时间间隔
  //时间间隔超过了1/局部规划频率，且还在局部规划，打印“未达到实际要求，实际上时间是r.cycleTime().toSec()”
  if(r.cycleTime() > ros::Duration(1 / controller_frequency_) && state_ == CONTROLLING)
    ROS_WARN("Control loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds", controller_frequency_, r.cycleTime().toSec());
}
//-----------------------------------------------------------------------------------------------------

//在业务循环的while语句中，以ros句柄的ok()接口作为终止业务循环的条件。如果因为该条件而终止了业务，意味着move_base节点被杀死了，
//需要更新Action服务器对象as_的状态
lock.lock();
runPlanner_ = true;
planner_cond_.notify_one();
lock.unlock();

//如果节点被关闭了，那么Action服务器也关闭并返回
as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on the goal because the node has been killed");
return;

}

double MoveBase::distance(const geometry_msgs::PoseStamped& p1, const geometry_msgs::PoseStamped& p2)
{
return hypot(p1.pose.position.x - p2.pose.position.x, p1.pose.position.y - p2.pose.position.y);
}

/// 局部规划 MoveBase::executeCycle | 传入goal，调用局部规划器类方法，得到速度控制指令///
bool MoveBase::executeCycle(geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& global_plan){

//为了保证执行业务逻辑的过程中，系统的参数不会发生改变，在业务主体的一开始就定义了一个锁对象ecl。在ecl的构造函数中，
//完成了对信号量configuration_mutex_的加锁操作。 当该函数退出时，局部变量ecl将被自动释放，在它的析构函数中自动的释放信号量
boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);

//声明速度消息
geometry_msgs::Twist cmd_vel;

//通过MoveBase的成员函数getRobotPose获取全局代价地图上的机器人位姿
geometry_msgs::PoseStamped global_pose;
getRobotPose(global_pose, planner_costmap_ros_);
//把当前位姿转换为geometry_msgs::PoseStamped格式，存储在current_position中
const geometry_msgs::PoseStamped& current_position = global_pose;

//发布反馈消息到Action的客户端，feedback（结构体）指的是从服务端周期反馈回客户端的信息，把当前位姿反馈给客户端
move_base_msgs::MoveBaseFeedback feedback;
feedback.base_position = current_position;
as_->publishFeedback(feedback);

//check to see if we've moved far enough to reset our oscillation timeout
if(distance(current_position, oscillation_pose_) >= oscillation_distance_)
{
  last_oscillation_reset_ = ros::Time::now();
  oscillation_pose_ = current_position;

  //如果我们上一次的恢复行为是由振荡引起，我们就重新设置恢复行为的索引
  if(recovery_trigger_ == OSCILLATION_R)
    recovery_index_ = 0;
}

//controller_costmap_ros_是MoveBase的局部代价地图，通过其接口isCurrent()，我们可以查看代价地图近期是否得到了更新。
//如果长时间没有更新，意味着机器人失去了周围的感知能力， 此时移动机器人是危险的，所以我们通过publishZeroVelocity控制机器人停下来并退出业务逻辑
if(!controller_costmap_ros_->isCurrent()){
  ROS_WARN("[%s]:Sensor data is out of date, we're not going to allow commanding of the base for safety",ros::this_node::getName().c_str());
  publishZeroVelocity();
  return false;
}

//通过标志位判定全局规划是否得出新的路线，然后通过指针交换，将latest_plan_（最新的全局规划结果）的值传递给controller_plan_即局部规划使用，
//然后将上一次的局部规划路线传递给latest_plan
//new_global_plan_标志位在planThread中被置为真，表示生成了全局规划
if(new_global_plan_){
  //make sure to set the new plan flag to false
  new_global_plan_ = false;

  ROS_DEBUG_NAMED("move_base","Got a new plan...swap pointers");

  //do a pointer swap under mutex
  std::vector<geometry_msgs::PoseStamped>* temp_plan = controller_plan_;

  boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
  controller_plan_ = latest_plan_;
  latest_plan_ = temp_plan;
  lock.unlock();
  ROS_DEBUG_NAMED("move_base","pointers swapped!");

  //在实例tc_上调用局部规划器BaseLocalPlanner的类函数setPlan()， 把全局规划的结果传递给局部规划器，如果传递失败，退出并返回
  if(!tc_->setPlan(*controller_plan_)){
    ROS_ERROR("Failed to pass global plan to the controller, aborting.");
    resetState();

    //disable the planner thread
    lock.lock();
    runPlanner_ = false;
    lock.unlock();

    as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to pass global plan to the controller.");
    return true;
  }

  //如果我们找到有效的局部规划路线，且恢复行为是“全局规划失败”，重置恢复行为索引
  if(recovery_trigger_ == PLANNING_R)
    recovery_index_ = 0;
}

//对MoveBase状态进行判断，由于局部规划在全局规划结束后才调用，所以有以下几种结果：
//① PLANNING：全局规划还没完成，还没得到一个全局路线，那么唤醒一个全局规划线程去制定全局路线
//② CONTROLLING：全局规划成功，得到全局路线，这里进行真正的局部规划，实际控制机器人沿着轨迹运动的状态
//③ CLEARING：全局规划失败，进入恢复行为，它调用用户定义的恢复逻辑尝试清除状态空间

switch(state_){
  //① PLANNING：全局规划还没完成，还没得到一个全局路线，那么唤醒一个全局规划线程去制定全局路线
  case PLANNING:
    {
      boost::recursive_mutex::scoped_lock lock(planner_mutex_);
      runPlanner_ = true;
      planner_cond_.notify_one();
    }
    ROS_DEBUG_NAMED("move_base","Waiting for plan, in the planning state.");
    break;

  //② CONTROLLING：全局规划成功，得到全局路线，这里进行真正的局部规划，实际控制机器人沿着轨迹运动的状态-----------------------------------------------------
  case CONTROLLING:
    ROS_DEBUG_NAMED("move_base","In controlling state.");

    //check to see if we've reached our goal
    if(tc_->isGoalReached()){
      ROS_DEBUG_NAMED("move_base","Goal reached!");
      resetState();

      //disable the planner thread
      boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
      runPlanner_ = false;
      lock.unlock();

      as_->setSucceeded(move_base_msgs::MoveBaseResult(), "Goal reached.");
      return true;
    }

    //check for an oscillation condition
    if(oscillation_timeout_ > 0.0 &&
        last_oscillation_reset_ + ros::Duration(oscillation_timeout_) < ros::Time::now())
    {
      publishZeroVelocity();
      state_ = CLEARING;
      recovery_trigger_ = OSCILLATION_R;
    }

    {
     boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_ros_->getCostmap()->getMutex()));

     ///局部规划器实例tc_被传入了全局规划后，调用computeVelocityCommands函数计算速度存储在cmd_vel中
     //其中变量 tc_ 是teb_local_planner_ros插件类的实例,是一个local_planner对象的共享指针(boost::shared_ptr<nav_core::BaseLocalPlanner>)，
     //在move_base类的构造函数中创建和初始化,调用local_planner的函数主要是tc_->computeVelocityCommands(cmd_vel)，就是这个函数开始调用teb_local_planner算法
    if(tc_->computeVelocityCommands(cmd_vel)){
      ROS_DEBUG_NAMED( "move_base", "Got a valid command from the local planner: %.3lf, %.3lf, %.3lf",
                       cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z );
      //若成功计算速度，上一次有效局部控制的时间设为当前
      last_valid_control_ = ros::Time::now();
      //向底盘发送速度控制消息，一个循环只发一次速度命令
      vel_pub_.publish(cmd_vel);
      if(recovery_trigger_ == CONTROLLING_R)
        recovery_index_ = 0;
    }
    //若computeVelocityCommands(cmd_vel)函数计算失败，且超时，则进入对应恢复行为，若未超时，则发布零速制停机器人，重新全局规划，再进行下一次局部规划
    else {
      ROS_DEBUG_NAMED("move_base", "The local planner could not find a valid plan.");
      ros::Time attempt_end = last_valid_control_ + ros::Duration(controller_patience_);

      //若局部规划用时超过限制
      if(ros::Time::now() > attempt_end){
        //发布0速度，进入恢复行为，触发器置为局部规划失败
        publishZeroVelocity();
        state_ = CLEARING;
        recovery_trigger_ = CONTROLLING_R;
      }
      //若局部规划用时没超过限制
      else{
        //发布0速度，在机器人当前位置再次回到全局规划
        last_valid_plan_ = ros::Time::now();
        planning_retries_ = 0;
        state_ = PLANNING;
        publishZeroVelocity();

        //enable the planner thread in case it isn't running on a clock
        boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
        runPlanner_ = true;
        planner_cond_.notify_one();
        lock.unlock();
      }
    }
    }

    break;

  //③ CLEARING：全局规划失败，进入恢复行为，它调用用户定义的恢复逻辑尝试清除状态空间--------------------------------------------------------------------------------
  case CLEARING:
    ROS_DEBUG_NAMED("move_base","In clearing/recovery state");
    //we'll invoke whatever recovery behavior we're currently on if they're enabled
    if(recovery_behavior_enabled_ && recovery_index_ < recovery_behaviors_.size()){
      ROS_DEBUG_NAMED("move_base_recovery","Executing behavior %u of %zu", recovery_index_+1, recovery_behaviors_.size());

      move_base_msgs::RecoveryStatus msg;
      msg.pose_stamped = current_position;
      msg.current_recovery_number = recovery_index_;
      msg.total_number_of_recoveries = recovery_behaviors_.size();
      msg.recovery_behavior_name =  recovery_behavior_names_[recovery_index_];

      recovery_status_pub_.publish(msg);

      recovery_behaviors_[recovery_index_]->runBehavior();

      //we at least want to give the robot some time to stop oscillating after executing the behavior
      last_oscillation_reset_ = ros::Time::now();

      //we'll check if the recovery behavior actually worked
      ROS_DEBUG_NAMED("move_base_recovery","Going back to planning state");
      last_valid_plan_ = ros::Time::now();
      planning_retries_ = 0;
      state_ = PLANNING;

      //update the index of the next recovery behavior that we'll try
      recovery_index_++;
    }
     //若恢复行为无效
    else{
      ROS_DEBUG_NAMED("move_base_recovery","All recovery behaviors have failed, locking the planner and disabling it.");
      //disable the planner thread
      boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
      runPlanner_ = false;
      lock.unlock();

      ROS_DEBUG_NAMED("move_base_recovery","Something should abort after this.");

       //反馈失败的具体信息
      if(recovery_trigger_ == CONTROLLING_R){
        ROS_ERROR("Aborting because a valid control could not be found. Even after executing all recovery behaviors");
        as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to find a valid control. Even after executing recovery behaviors.");
      }
      else if(recovery_trigger_ == PLANNING_R){
        ROS_ERROR("Aborting because a valid plan could not be found. Even after executing all recovery behaviors");
        as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to find a valid plan. Even after executing recovery behaviors.");
      }
      else if(recovery_trigger_ == OSCILLATION_R){
        ROS_ERROR("Aborting because the robot appears to be oscillating over and over. Even after executing all recovery behaviors");
        as_->setAborted(move_base_msgs::MoveBaseResult(), "Robot is oscillating. Even after executing recovery behaviors.");
      }
      resetState();
      return true;
    }
    break;
  default:
    ROS_ERROR("This case should never be reached, something is wrong, aborting");
    resetState();
    //disable the planner thread
    boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
    runPlanner_ = false;
    lock.unlock();
    as_->setAborted(move_base_msgs::MoveBaseResult(), "Reached a case that should not be hit in move_base. This is a bug, please report it.");
    return true;
}

//we aren't done yet
return false;

}

bool MoveBase::loadRecoveryBehaviors(ros::NodeHandle node){
XmlRpc::XmlRpcValue behavior_list;
if(node.getParam(“recovery_behaviors”, behavior_list)){
if(behavior_list.getType() == XmlRpc::XmlRpcValue::TypeArray){
for(int i = 0; i < behavior_list.size(); ++i){
if(behavior_list[i].getType() == XmlRpc::XmlRpcValue::TypeStruct){
if(behavior_list[i].hasMember(“name”) && behavior_list[i].hasMember(“type”)){
//check for recovery behaviors with the same name
for(int j = i + 1; j < behavior_list.size(); j++){
if(behavior_list[j].getType() == XmlRpc::XmlRpcValue::TypeStruct){
if(behavior_list[j].hasMember(“name”) && behavior_list[j].hasMember(“type”)){
std::string name_i = behavior_list[i][“name”];
std::string name_j = behavior_list[j][“name”];
if(name_i == name_j){
ROS_ERROR(“A recovery behavior with the name %s already exists, this is not allowed. Using the default recovery behaviors instead.”,
name_i.c_str());
return false;
}
}
}
}
}
else{
ROS_ERROR(“Recovery behaviors must have a name and a type and this does not. Using the default recovery behaviors instead.”);
return false;
}
}
else{
ROS_ERROR(“Recovery behaviors must be specified as maps, but they are XmlRpcType %d. We’ll use the default recovery behaviors instead.”,
behavior_list[i].getType());
return false;
}
}

    //if we've made it to this point, we know that the list is legal so we'll create all the recovery behaviors
    for(int i = 0; i < behavior_list.size(); ++i){
      try{
        //check if a non fully qualified name has potentially been passed in
        if(!recovery_loader_.isClassAvailable(behavior_list[i]["type"])){
          std::vector<std::string> classes = recovery_loader_.getDeclaredClasses();
          for(unsigned int i = 0; i < classes.size(); ++i){
            if(behavior_list[i]["type"] == recovery_loader_.getName(classes[i])){
              //if we've found a match... we'll get the fully qualified name and break out of the loop
              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.",
                  std::string(behavior_list[i]["type"]).c_str(), classes[i].c_str());
              behavior_list[i]["type"] = classes[i];
              break;
            }
          }
        }

        boost::shared_ptr<nav_core::RecoveryBehavior> behavior(recovery_loader_.createInstance(behavior_list[i]["type"]));

        //shouldn't be possible, but it won't hurt to check
        if(behavior.get() == NULL){
          ROS_ERROR("The ClassLoader returned a null pointer without throwing an exception. This should not happen");
          return false;
        }

        //initialize the recovery behavior with its name
        behavior->initialize(behavior_list[i]["name"], &tf_, planner_costmap_ros_, controller_costmap_ros_);
        recovery_behavior_names_.push_back(behavior_list[i]["name"]);
        recovery_behaviors_.push_back(behavior);
      }
      catch(pluginlib::PluginlibException& ex){
        ROS_ERROR("Failed to load a plugin. Using default recovery behaviors. Error: %s", ex.what());
        return false;
      }
    }
  }
  else{
    ROS_ERROR("The recovery behavior specification must be a list, but is of XmlRpcType %d. We'll use the default recovery behaviors instead.",
        behavior_list.getType());
    return false;
  }
}
else{
  //if no recovery_behaviors are specified, we'll just load the defaults
  return false;
}

//if we've made it here... we've constructed a recovery behavior list successfully
return true;

}

//we’ll load our default recovery behaviors here
void MoveBase::loadDefaultRecoveryBehaviors(){
recovery_behaviors_.clear();
try{
//we need to set some parameters based on what’s been passed in to us to maintain backwards compatibility
ros::NodeHandle n("~");
n.setParam(“conservative_reset/reset_distance”, conservative_reset_dist_);
n.setParam(“aggressive_reset/reset_distance”, circumscribed_radius_ * 4);

  //first, we'll load a recovery behavior to clear the costmap
  boost::shared_ptr<nav_core::RecoveryBehavior> cons_clear(recovery_loader_.createInstance("clear_costmap_recovery/ClearCostmapRecovery"));
  cons_clear->initialize("conservative_reset", &tf_, planner_costmap_ros_, controller_costmap_ros_);
  recovery_behavior_names_.push_back("conservative_reset");
  recovery_behaviors_.push_back(cons_clear);

  //next, we'll load a recovery behavior to rotate in place
  boost::shared_ptr<nav_core::RecoveryBehavior> rotate(recovery_loader_.createInstance("rotate_recovery/RotateRecovery"));
  if(clearing_rotation_allowed_){
    rotate->initialize("rotate_recovery", &tf_, planner_costmap_ros_, controller_costmap_ros_);
    recovery_behavior_names_.push_back("rotate_recovery");
    recovery_behaviors_.push_back(rotate);
  }

  //next, we'll load a recovery behavior that will do an aggressive reset of the costmap
  boost::shared_ptr<nav_core::RecoveryBehavior> ags_clear(recovery_loader_.createInstance("clear_costmap_recovery/ClearCostmapRecovery"));
  ags_clear->initialize("aggressive_reset", &tf_, planner_costmap_ros_, controller_costmap_ros_);
  recovery_behavior_names_.push_back("aggressive_reset");
  recovery_behaviors_.push_back(ags_clear);

  //we'll rotate in-place one more time
  if(clearing_rotation_allowed_){
    recovery_behaviors_.push_back(rotate);
    recovery_behavior_names_.push_back("rotate_recovery");
  }
}
catch(pluginlib::PluginlibException& ex){
  ROS_FATAL("Failed to load a plugin. This should not happen on default recovery behaviors. Error: %s", ex.what());
}

return;

}

void MoveBase::resetState(){
// Disable the planner thread
boost::unique_lockboost::recursive_mutex lock(planner_mutex_);
runPlanner_ = false;
lock.unlock();

// Reset statemachine
state_ = PLANNING;
recovery_index_ = 0;
recovery_trigger_ = PLANNING_R;
publishZeroVelocity();

//if we shutdown our costmaps when we're deactivated... we'll do that now
if(shutdown_costmaps_){
  ROS_DEBUG_NAMED("move_base","Stopping costmaps");
  planner_costmap_ros_->stop();
  controller_costmap_ros_->stop();
}

}

bool MoveBase::getRobotPose(geometry_msgs::PoseStamped& global_pose, costmap_2d::Costmap2DROS* costmap)
{
tf2::toMsg(tf2::Transform::getIdentity(), global_pose.pose);
geometry_msgs::PoseStamped robot_pose;
tf2::toMsg(tf2::Transform::getIdentity(), robot_pose.pose);
robot_pose.header.frame_id = robot_base_frame_;
robot_pose.header.stamp = ros::Time(); // latest available
ros::Time current_time = ros::Time::now(); // save time for checking tf delay later

// get robot pose on the given costmap frame
try
{
  tf_.transform(robot_pose, global_pose, costmap->getGlobalFrameID());
}
catch (tf2::LookupException& ex)
{
  ROS_ERROR_THROTTLE(1.0, "No Transform available Error looking up robot pose: %s\n", ex.what());
  return false;
}
catch (tf2::ConnectivityException& ex)
{
  ROS_ERROR_THROTTLE(1.0, "Connectivity Error looking up robot pose: %s\n", ex.what());
  return false;
}
catch (tf2::ExtrapolationException& ex)
{
  ROS_ERROR_THROTTLE(1.0, "Extrapolation Error looking up robot pose: %s\n", ex.what());
  return false;
}

// check if global_pose time stamp is within costmap transform tolerance
if (current_time.toSec() - global_pose.header.stamp.toSec() > costmap->getTransformTolerance())
{
  ROS_WARN_THROTTLE(1.0, "Transform timeout for %s. " \
                    "Current time: %.4f, pose stamp: %.4f, tolerance: %.4f", costmap->getName().c_str(),
                    current_time.toSec(), global_pose.header.stamp.toSec(), costmap->getTransformTolerance());
  return false;
}

return true;

}
};