【ROS-Navigation】Movebase主体源码解读

记录学习阅读ROS Navigation源码的理解，本文为Movebase主体源码学习记录，以文字总结、绘制结构图说明、代码注释为主。仍在学习过程中，有错误欢迎指正，共同进步。

Movebase的主干部分是一个Action服务器，接收用户发送的目标位置，并调用全局规划器和局部规划器，基于各层代价地图的信息进行路径规划，得到最优路径，向用户反馈机器人速度指令，驱动机器人按照指令运动，最终到达目标位置。这里不涉及到规划路径和更新地图的具体算法和实现，主要是完成了一个大的调用框架，具体的实现在各子过程的ROS封装类中。

【结构示意图】

【相关文件】

• move_base/src/move_base_node.cpp
• move_base/src/move_base.cpp
  
  

【代码分析】

1. move_base_node.cpp

这里初始化了节点“move_base_node”，Action服务的定义、全局规划器、局部规划器的调用都将在这个节点中进行。
然后实例化了MoveBase这个类，上述工作以类成员函数的形式定义在这个类中。
实例化之后，Action开始监听服务请求，并通过ros::spin()传递到Action的回调函数中进行处理。

#include <move_base/move_base.h>

int main(int argc, char** argv){
  ros::init(argc, argv, "move_base_node");
  tf::TransformListener tf(ros::Duration(10));

  move_base::MoveBase move_base( tf );

  ros::spin();

  return(0);
}

2. move_base_node.cpp

这里对MoveBase类的类成员进行了定义，以下为比较重要的几个函数。

–目录–

构造函数 MoveBase::MoveBase | 初始化Action

控制主体 MoveBase::executeCb | 收到目标，触发全局规划线程，循环执行局部规划

全局规划线程 void MoveBase::planThread | 调用全局规划

全局规划 MoveBase::makePlan | 调用全局规划器类方法，得到全局规划路线

局部规划 MoveBase::executeCycle | 传入全局路线，调用局部规划器类方法，得到速度控制指令

<1> 构造函数 MoveBase::MoveBase

MoveBase类的构造函数进行了初始化工作，获取了服务器上的参数值。

  MoveBase::MoveBase(tf::TransformListener& tf) :
    tf_(tf),                                                     
    as_(NULL),                                                    
    planner_costmap_ros_(NULL), controller_costmap_ros_(NULL),    
    bgp_loader_("nav_core", "nav_core::BaseGlobalPlanner"),       //加载了baseGlobalPlanner的类库
    blp_loader_("nav_core", "nav_core::BaseLocalPlanner"),        //加载了baseLocalPlanner的类库
    recovery_loader_("nav_core", "nav_core::RecoveryBehavior"),   //加载了recoveryBehaviour的类库
    planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL),
    runPlanner_(false), setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false) {

新建Action服务器，指针as_，绑定回调函数MoveBase::executeCb，这个函数是move_base的核心。

    //as_指向action服务器，当执行as_->start()时调用MoveBase::executeCb函数
    as_ = new MoveBaseActionServer(ros::NodeHandle(), "move_base", boost::bind(&MoveBase::executeCb, this, _1), false);

    ros::NodeHandle private_nh("~");                          
    ros::NodeHandle nh;                                         
    recovery_trigger_ = PLANNING_R;                              

从参数服务器设置用到的全局规划器、局部规划器、机器人底盘坐标系等，初始化三个用于存放plan的buffer。

    //从参数服务器加载用户输入的参数，如果没有，设置为默认值（第三个参数）
    std::string global_planner, local_planner;
    //全局规划器，默认navfn/NavfnROS
    private_nh.param("base_global_planner", global_planner, std::string("navfn/NavfnROS"));
    //局部规划器，默认TrajectoryPlannerROS
    private_nh.param("base_local_planner", local_planner, std::string("base_local_planner/TrajectoryPlannerROS"));
    //robot_base_frame，默认base_link
    private_nh.param("global_costmap/robot_base_frame", robot_base_frame_, std::string("base_link"));
    //global_frame，默认/map坐标系
    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);//默认不开启
    private_nh.param("oscillation_timeout", oscillation_timeout_, 0.0);
    private_nh.param("oscillation_distance", oscillation_distance_, 0.5);

    //初始化三个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>();

planner_thread_线程所绑定的planThread函数是move_base的又一个重点，即全局规划线程。
接下来声明速度话题和即时目标的发布。

    //创建规划器线程，在该线程里运行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 );

    //发布MoveBaseActionGoal消息到/move_base/goal话题上
    ros::NodeHandle action_nh("move_base");//movebase命名空间下的句柄
    action_goal_pub_ = action_nh.advertise<move_base_msgs::MoveBaseActionGoal>("goal", 1);
    
    //发布geometry_msgs::PoseStamped消息到/move_base_simple/goal话题上，回调函数goalCB会处理在/move_base_simple/goal话题上接收到的消息
    //供nav_view和rviz等仿真工具使用
    ros::NodeHandle simple_nh("move_base_simple");//move_base_simple下的句柄，订阅goal话题
    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，这里两个规划器用到的地图实质上是Costmap2DROS类的实例，这个类是ROS对costmap的封装，类函数start()会调用各层地图的active()函数，开始订阅传感器话题，对地图进行更新，这部分在代价地图部分详述。

    //全局规划器代价地图global_costmap_ros_
    planner_costmap_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tf_);
    planner_costmap_ros_->pause();

    //初始化全局规划器，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_
    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();

略去一部分跟主体关系不大的代码，接下来启动movebase的action服务器，并开启参数动态配置。

    //调用action server的start函数，服务器启动
    as_->start();

    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);
  }

<2> 控制主体 MoveBase::executeCb

executeCb是Action的回调函数，它是MoveBase控制流的主体，它调用了MoveBase内另外几个作为子部分的重要成员函数，先后完成了全局规划和局部规划。
在函数的开始部分，它对Action收到的目标进行四元数检测、坐标系转换，然后将其设置为全局规划的目标，并设置了一些标志位。

  void MoveBase::executeCb(const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal)
  {
    //检测收到的目标位置的旋转四元数是否有效，若无效，直接返回
    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);
    //启动全局规划
    boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
    //用接收到的目标goal来更新全局变量，即全局规划目标，这个值在planThread中会被用来做全局规划的当前目标
    planner_goal_ = goal;
    //全局规划标志位设为真
    runPlanner_ = true;
    //开始全局规划并于此处阻塞

接下来，由于全局规划器线程绑定的函数plannerThread()里有planner_cond_对象的wait函数，在这里调用notify会直接启动全局规划器线程，进行全局路径规划，函数plannerThread的内容后述。

    planner_cond_.notify_one();
    lock.unlock();

    //全局规划完成后，发布目标到current_goal话题上
    current_goal_pub_.publish(goal);
    //创建一个全局规划容器
    std::vector<geometry_msgs::PoseStamped> global_plan;

    //局部规划频率
    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();
    }

    //上一次有效的局部规划时间设为现在
    last_valid_control_ = ros::Time::now();
    //上一次有效的全局规划时间设为现在
    last_valid_plan_ = ros::Time::now();
    //上一次震荡重置时间设为现在
    last_oscillation_reset_ = ros::Time::now();
    //对同一目标的全局规划次数记录归为0
    planning_retries_ = 0;

    ros::NodeHandle n;

全局规划完成，接下来循环调用executeCycle函数来控制机器人进行局部规划，完成相应跟随。

    while(n.ok())
    {
      //c_freq_change_被初始化为false
      //如果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的服务器被抢占，可能是“局部规划进行过程中收到新的目标”，也可能是“收到取消行动的命令”。

• 如果是收到新目标，那么放弃当前目标，重复上面对目标进行的操作，使用新目标。并重新全局规划；
• 如果是收到取消行动命令，直接结束返回。

② 如果服务器未被抢占，或被抢占的if结构已执行完毕，接下来开始局部规划，调用executeCycle函数，并记录局部控制起始时间。

      //如果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);
          //重设恢复行为索引位为0
          recovery_index_ = 0; 
          //重设MoveBase状态为全局规划中
          state_ = PLANNING;

          //重新调用planThread进行全局规划
          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);

          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;
        }
      }

      //服务器接收到目标后，没有被新目标或取消命令抢占
      //检查目标是否被转换到全局坐标系（/map）下
      if(goal.header.frame_id != planner_costmap_ros_->getGlobalFrameID()){
        goal = goalToGlobalFrame(goal);

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

        lock.lock();
        planner_goal_ = goal;
        runPlanner_ = true;
        planner_cond_.notify_one();
        lock.unlock();

        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);

        last_valid_control_ = ros::Time::now();
        last_valid_plan_ = ros::Time::now();
        last_oscillation_reset_ = ros::Time::now();
        planning_retries_ = 0;
      }

      //记录开始局部规划的时刻为当前时间
      ros::WallTime start = ros::WallTime::now();

      //调用executeCycle函数进行局部规划，传入目标和全局规划路线
      bool done = executeCycle(goal, global_plan);

若局部规划完成，结束。

      if(done)
        return;

      //记录从局部规划开始到这时的时间差
      ros::WallDuration t_diff = ros::WallTime::now() - start;
      //打印用了多长时间完成操作
      ROS_DEBUG_NAMED("move_base","Full control cycle time: %.9f\n", t_diff.toSec());
      //用局部规划频率进行休眠
      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());
    }

    //唤醒全局规划线程，以使它能够“干净地退出”
    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;
  }

<3> 全局规划线程 void MoveBase::planThread()

planThread()的核心是调用makePlan函数，该函数中实际进行全局规划。
全局规划线程时刻等待被executeCB函数唤醒，当executeCB函数中唤醒planThread并将标志位runPlanner_设置为真，跳出内部的循环，继续进行下面部分。

  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_lock<boost::recursive_mutex> lock(planner_mutex_);
    while(n.ok()){
      //不断循环，直到wait_for_wake（上面行已置为假）和runPlanner_为真，跳出循环
      while(wait_for_wake || !runPlanner_){
        //如果waitforwake是真或者runplanner是假，不断执行循环，wait()等待
        ROS_DEBUG_NAMED("move_base_plan_thread","Planner thread is suspending");
        //调用wait函数时，函数会自动调用lock.unlock()释放锁，使得其他被阻塞在锁竞争上的线程得以继续执行
        //比如executeCB中先把标志位runPlanner设为true，然后用notify，使这里的锁由unlock变成lock，竞争到了资源，并且这里的规划线程开始执行
        planner_cond_.wait(lock);
        wait_for_wake = false;
      }
      //start_time设为当前时间
      ros::Time start_time = ros::Time::now();

调用makePlan函数，进行全局规划，如果成功，结果将储存进planner_plan_中。接下来对makePlan函数的结果进行判断：

① 若全局规划成功，则交换planner_plan_和latest_plan的值，即令latest_plan中存储的是本次全局规划的结果（最新），planner_plan_中存储的是上次全局规划的结果（次新）。设置标志位new_global_plan_ = true，表示得到了新的全局规划路线，并设置Movebase状态标志位state_为CONTROLLING，即全局规划完成，开始进行局部控制。

② 如果全局规划失败，MoveBase还在planning状态，即机器人没有移动，则进入自转模式。

      //把全局中被更新的全局目标planner_goal存储为临时目标
      geometry_msgs::PoseStamped temp_goal = planner_goal_;
      lock.unlock();//
      ROS_DEBUG_NAMED("move_base_plan_thread","Planning...");

      //全局规划初始化，清空
      planner_plan_->clear();
      //调用MoveBase类的makePlan函数，如果成功为临时目标制定全局规划planner_plan_，则返回true
      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());
        //用指针交换planner_plan_和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");

        //确保只有在我们还没到达目标时才启动controller以局部规划
        //如果runPlanner_在调用此函数时被置为真，将MoveBase状态设置为CONTROLLING（局部规划中）
        if(runPlanner_) 
          state_ = CONTROLLING;
        //如果规划频率小于0，runPlanner_置为假
        if(planner_frequency_ <= 0)
          runPlanner_ = false;
        lock.unlock();
      }
      //如果全局规划失败并且MoveBase还在planning状态，即机器人没有移动，则进入自转模式
      else if(state_==PLANNING){
        ROS_DEBUG_NAMED("move_base_plan_thread","No Plan...");
        //最迟制定出本次全局规划的时间=上次成功规划的时间+容忍时间
        ros::Time attempt_end = last_valid_plan_ + ros::Duration(planner_patience_);

        //检查时间和次数是否超过限制，若其中一项不满足限制，停止全局规划
        lock.lock();
        //对同一目标的全局规划的次数记录+1
        planning_retries_++;
        //如果runplanner被置为真，且目前超时或超次数，进入恢复行为模式
        if(runPlanner_ &&
           (ros::Time::now() > attempt_end || planning_retries_ > uint32_t(max_planning_retries_))){
          //将MoveBase状态设置为恢复行为
          state_ = CLEARING;
          //全局规划标志位置为假
          runPlanner_ = false;
          //发布0速度
          publishZeroVelocity();
          //恢复行为触发器状态设置为全局规划失败
          recovery_trigger_ = PLANNING_R;
        }

        lock.unlock();
      }
    }
  }

该函数最后部分的代码与主体关系不大，这里略去。

<4> 全局规划 MoveBase::makePlan

该函数先进行一些预备工作，如检查全局代价地图、起始位姿，然后将起始位姿的数据格式做转换。

  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()));

    //初始化空plan
    plan.clear();

    //如果没有全局代价地图，返回false，因为全局规划必须基于全局代价地图
    if(planner_costmap_ros_ == NULL) {
      ROS_ERROR("Planner costmap ROS is NULL, unable to create global plan");
      return false;
    }

    //如果得不到机器人的起始位姿，返回false
    tf::Stamped<tf::Pose> global_pose;
    if(!planner_costmap_ros_->getRobotPose(global_pose)) {
      ROS_WARN("Unable to get starting pose of robot, unable to create global plan");
      return false;
    }

    geometry_msgs::PoseStamped start;
    tf::poseStampedTFToMsg(global_pose, start);

接下来是实际进行全局规划的函数，调用全局规划器的makePlan函数planner_-> makePlan(start, goal, plan)，传入机器人当前位姿和目标，得到plan，若规划失败或得到的plan为空，返回false，否则返回true。关于全局规划的具体算法实现，在NavFn部分具体学习理解。

    //调用BaseGlobalPlanner类的makePlan函数做全局规划
    //如果全局规划失败或者全局规划为空，返回false
    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;
  }

<5> 局部规划 MoveBase::executeCycle

executeCycle函数的作用是进行局部规划，函数先声明了将要发布的速度，然后获取当前位姿并格式转换。

  bool MoveBase::executeCycle(geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& global_plan){
    boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);

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

    //声明全局姿态
    tf::Stamped<tf::Pose> global_pose;

    //从全局代价地图上获取当前位姿
    planner_costmap_ros_->getRobotPose(global_pose);

    //把当前位姿转换为geometry_msgs::PoseStamped格式，存储在current_position中
    geometry_msgs::PoseStamped current_position;
    tf::poseStampedTFToMsg(global_pose, current_position);

    //feedback指的是从服务端周期反馈回客户端的信息，把当前位姿反馈给客户端
    move_base_msgs::MoveBaseFeedback feedback;
    feedback.base_position = current_position;
    as_->publishFeedback(feedback);

做几个判断，判断机器人是否被困住（若是，则进入恢复行为，即针对机器人运动异常情况做出的指令，具体内容再该部分学习理解），并检查局部规划的地图是否是当前的，否则发布零速，停止规划，制停机器人。

    //如果长时间内移动距离没有超过震荡距离，那么认为机器人在震荡（长时间被困在一片小区域），进入恢复行为
    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;
    }

    //检查局部代价地图是否是当前的
    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_){ 

      //重置标志位
      new_global_plan_ = false;

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

      std::vector<geometry_msgs::PoseStamped>* temp_plan = controller_plan_;

      //在指针的保护下，交换latest_plan和controller_plan的值
      boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
      //controller_plan_存储【当前最新制定好的待到达的全局规划】
      controller_plan_ = latest_plan_;
      //使得全局规划制定好的planner_plan经由latest_plan一路传递到controller_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();
        //停止全局规划线程
        lock.lock();
        runPlanner_ = false;
        lock.unlock();
        //停止Action服务器，打印“将全局规划传递至局部规划器控制失败”
        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：全局规划还没完成，还没得到一个全局路线，那么唤醒一个全局规划线程去制定全局路线

    switch(state_){
      //如果我们还在全局规划状态
      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：全局规划成功，得到全局路线，这里进行真正的局部规划：

• 如果已经位于终点，结束局部规划；
• 如果没到终点，检查机器人是否被困住，如果是，则进入恢复行为；
• 如果没到终点，且状态正常，调用局部规划器实例tc_->computeVelocityCommands(cmd_vel)函数，这个函数结合传入的全局规划路线和其他因素得出局部规划结果，即速度指令，存放在cmd_vel中，将其发布，控制机器人运行。computeVelocityCommands(cmd_vel)函数内容具体在base_local_planner中，它通过在给定速度范围内模拟各个方向的速度得到当前可能的小范围行驶路线，进一步对得到的路线进行评估，得分最高的路线对应的速度就是函数得到的结果。所以局部规划更像是不断循环进行的“短期规划”，实现在该部分详述。

      //如果全局规划成功，进入CONTROLLING状态，就去找有效的速度控制
      case CONTROLLING:
        ROS_DEBUG_NAMED("move_base","In controlling state.");

        //查询是否到达终点，如果到达终点，结束
        if(tc_->isGoalReached()){
          ROS_DEBUG_NAMED("move_base","Goal reached!");
          resetState();

          //结束全局规划线程
          boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
          //标志位置false
          runPlanner_ = false;
          lock.unlock();
          //Action返回成功
          as_->setSucceeded(move_base_msgs::MoveBaseResult(), "Goal reached.");
          return true;
        }

        //如果未到达终点，检查是否处于振荡状态
        if(oscillation_timeout_ > 0.0 &&
            last_oscillation_reset_ + ros::Duration(oscillation_timeout_) < ros::Time::now())
        {
          //如果振荡状态超时了，发布0速度
          publishZeroVelocity();
          //MoveBase状态置为恢复行为
          state_ = CLEARING;
          //恢复行为触发器置为，长时间困在一片小区域
          recovery_trigger_ = OSCILLATION_R;
        }
        
        {
         boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_ros_->getCostmap()->getMutex()));
        //局部规划器实例tc_被传入了全局规划后，调用computeVelocityCommands函数计算速度存储在cmd_vel中
        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);
          //如果恢复行为触发器值是局部规划失败，把索引置0
          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();

            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");
        //如果允许使用恢复行为，且恢复行为索引值小于恢复行为数组的大小
        if(recovery_behavior_enabled_ && recovery_index_ < recovery_behaviors_.size()){
          ROS_DEBUG_NAMED("move_base_recovery","Executing behavior %u of %zu", recovery_index_, recovery_behaviors_.size());
          //开始恢复行为，在executeCycle的循环中一次次迭代恢复行为
          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.");
          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();
        //关闭全局规划器
        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;
    }

【总结】

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

Neo 2020.2