ROS-Navigation学习(3)——move_base(2)源码cpp学习

源码+注释

Move_base主要代码核心是 executeCb(); executeCycle(); planThread()三个函数。

首先是move_base的构造函数，主要是设置一些配置参数和数据成员的初始化，有很多NodeHandle。分别是nh, action_nh, simple_nh,源码如下

 
 MoveBase::MoveBase(tf2_ros::Buffer& tf) :
    // 初始化一些参数
    tf_(tf), 
    as_(NULL),
    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),
    runPlanner_(false), setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false) 
    
    ros::NodeHandle private_nh("~"); //private_nh的命名空间应该是 move_base/node_name(move_base?)
    ros::NodeHandle nh;  //nh在的命名空间就是 /move_base
    
    //创建move_baseActionServer,回调函数是executeCb
    as_ = new MoveBaseActionServer(ros::NodeHandle(), "move_base", boost::bind(&MoveBase::executeCb, this, _1), false);
	//读取配置的一些参数
	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);
    
    //set up plan triple buffer  三个路径规划器产生的向量分别保存在planner_plan,
    //latest_plan,controller_plan中。传递顺序是planner->latest->controller;
    planner_plan_ = new std::vector<geometry_msgs::PoseStamped>();
    latest_plan_ = new std::vector<geometry_msgs::PoseStamped>();
    controller_plan_ = new std::vector<geometry_msgs::PoseStamped>();
    
    //set up the planner's thread 开启路径规划线程
    planner_thread_ = new boost::thread(boost::bind(&MoveBase::planThread, this));

    //for commanding the base 发布控制速度到cmd_vel
    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);
	//这个simple_goal代码注释是说可以通过posestamp发送goal，对rviz等工具有用，
	//可能是rviz中点击那个2D Navigation也可以进行导航的原因（猜测）
	ros::NodeHandle simple_nh("move_base_simple");
    goal_sub_ = simple_nh.subscribe<geometry_msgs::PoseStamped>("goal", 1, boost::bind(&MoveBase::goalCB, this, _1));

	//设置代价地图的一些参数，有默认值
	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);

路径规划代价地图话题，并且初始化（不过还不太懂这是干啥的。。。）
查下ros wiki的costmap_2DROS函数（现在在家进wiki进不去。。）

 planner_costmap_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tf_);
 planner_costmap_ros_->pause();
 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);
 }

除了planner_costmap_ros之外还有controller_costmap_ros,和其一样。
然后就是开始更新这两种代价地图：

 planner_costmap_ros_->start();
 controller_costmap_ros_->start();

分别定义路径规划的服务和清除代价地图的服务，回调函数分别是planservice和clearcostmapService.

//advertise a service for getting a plan
 make_plan_srv_ = private_nh.advertiseService("make_plan", &MoveBase::planService, this);
 //advertise a service for clearing the costmaps
 clear_costmaps_srv_ = private_nh.advertiseService("clear_costmaps", &MoveBase::clearCostmapsService, this);

构造函数中还有一些状态的设置和恢复行为等。

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

    //load any user specified recovery behaviors, and if that fails load the defaults 恢复行为
    if(!loadRecoveryBehaviors(private_nh)){
     
      loadDefaultRecoveryBehaviors();
    }

    //initially, we'll need to make a plan
    state_ = PLANNING;

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

    //we're all set up now so we can start the action server 开启服务器
    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);

以上就是move_base的构造函数。

主要函数executeCb();

创建的MoveBaseActionServer的回调函数是executeCb,因此首先看一下executeCb 具体如下：
首先是检查四元数是否合法，非法直接返回

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

其中isQuaternionValid函数如下：

  bool MoveBase::isQuaternionValid(const geometry_msgs::Quaternion& q){
     
    //检查四元数中是否有nans或者是infs，如果有返回false
    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 检查四元数是否趋近于0
    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
    //对四元数进行规范化，检测z轴是否垂直，因为是小车，只能绕着z轴旋转。
    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;
  }

然后是进行坐标系的变换，将目标的坐标系转换到全局坐标系。函数是goalToGlobalFrame. 使用的是tf中的transform。

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

回到executeCb函数，在进行完坐标系转换后，进行目标的设定，并且唤醒路径规划器。

 	publishZeroVelocity(); //初始化速度，均设置成0，发布。
    //we have a goal so start the planner
    boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
    planner_goal_ = goal;
    runPlanner_ = true;
    planner_cond_.notify_one();
    lock.unlock();

然后发布目标点，并且设置了控制频率

    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();
    planning_retries_ = 0;

然后开始循环,首先判断是否修改控制频率

 while(n.ok())
    {
     
    //首先判断是否要更改控制频率
      if(c_freq_change_)
      {
     
        ROS_INFO("Setting controller frequency to %.2f", controller_frequency_);
        r = ros::Rate(controller_frequency_);
        c_freq_change_ = false;
      }

然后是判断actionserver是否激活以及是否有新的导航点。如果有新的导航点，则表示之前的导航点无效，所以需要重新进行判断四元数，转换全局坐标系，重新唤醒路径规划器。代码在上文中都有所提及。

if(as_->isPreemptRequested()){
     
        if(as_->isNewGoalAvailable()){
     
          //if we're active and a new goal is available, we'll accept it, but we won't shut anything down
          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;
          }

          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;

          //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","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;

如果没有新的目标，那就应该对这个action进行中断，重置状态，继续之前的导航的状态。（这里的理解可能有些问题，重置状态不会对之前导航状态进行重置？）

      else {
     
          //if we've been preempted explicitly we need to shut things down
          resetState();

          //notify the ActionServer that we've successfully preempted
          ROS_DEBUG_NAMED("move_base","Move base preempting the current goal");
          as_->setPreempted();

          //we'll actually return from execute after preempting
          return;
        }
      }

接下来会对目标点检测是否是在全局坐标系下的表示，如果不是，则需要进行转换，然后继续进行重新路径规划等一些操作。代码也是重复的：

if(goal.header.frame_id != planner_costmap_ros_->getGlobalFrameID()){
     
        goal = goalToGlobalFrame(goal);

        //we want to go back to the planning state for the next execution cycle
        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;
      }

在进行了上述一些检测之后，才会真正进入到执行到达目标点工作的函数：executeCycle();

	  ros::WallTime start = ros::WallTime::now();

      //the real work on pursuing a goal is done here
      bool done = executeCycle(goal, global_plan);

如果完成了就会返回。
最后还有检测是那种方式完成的以及休眠动作。

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

      r.sleep();
      //make sure to sleep for the remainder of our cycle time
      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());
    }
    //wake up the planner thread so that it can exit cleanly
    lock.lock();
    runPlanner_ = true;
    planner_cond_.notify_one();
    lock.unlock();

    //if the node is killed then we'll abort and return
    as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on the goal because the node has been killed");
    return;
  }

主要函数executeCycle()

在executeCb();函数中，又会涉及到executeCycle();函数，是执行导航的工作函数。返回导航的结果，是否到达目标点。
首先是获取机器人当前位置并且发布（actionlib中的publishfeedback存在疑问？）；(像是一个缓冲器，存储多个逐个发送那种。)

    geometry_msgs::PoseStamped global_pose;
    getRobotPose(global_pose, planner_costmap_ros_);
    const geometry_msgs::PoseStamped& current_position = global_pose;

    //push the feedback out
    move_base_msgs::MoveBaseFeedback feedback;
    feedback.base_position = current_position;
    as_->publishFeedback(feedback);

其中getRobotPose函数是得到机器人当前位置的函数，主要就是注意坐标系转换以及时间戳需要一致。

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

在进行完获取位置后，会对机器人位置是否震荡（？？），以及代价地图更新是否超时，超时则表示得到数据和实际不匹配。导航结果会错误，因此会进行重置。

if(distance(current_position, oscillation_pose_) >= oscillation_distance_)
    {
     
      last_oscillation_reset_ = ros::Time::now();
      oscillation_pose_ = current_position;

      //if our last recovery was caused by oscillation, we want to reset the recovery index 
      if(recovery_trigger_ == OSCILLATION_R)
        recovery_index_ = 0;
    }

    //check that the observation buffers for the costmap are current, we don't want to drive blind
    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;
    }

然后判断是否有新路径，如果存在则调用setPlan函数，传递给路径规划器（一个外部包的函数）。而后确认重置了恢复的一个recovery_index，一个计数，保证不越界。

if(!tc_->setPlan(*controller_plan_)){
     
        //ABORT and SHUTDOWN COSTMAPS
        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;
      }

      //make sure to reset recovery_index_ since we were able to find a valid plan
      if(recovery_trigger_ == PLANNING_R)
        recovery_index_ = 0;
    }

然后进入到一个状态机，有PLANNING， CONTROLLING， CLEARING，主要的是其中的CONTROLLING，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;

CLEARING 是机器人如果控制或者规划失败进入的一个状态，会进行恢复行为，并且设置为导航状态。如果没有恢复行为，或者是恢复已经越界(else里内容），则会结束动作。然后进行resetState();这里runBehavior()函数是另外一个包的。

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_, recovery_behaviors_.size());
          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;

主要的是controlling状态。
首先是检测是否到达目标点，如果到达，就重置状态。其中isGoalReached函数也是外部。

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

其中resetState函数功能是将规划器关闭，然后重新进行到Planning状态，并且将机器人速度设置0。还有就是代价地图是否停止更新。

void MoveBase::resetState(){
     
    // Disable the planner thread
    boost::unique_lock<boost::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();
    }
  }

回到PLANNING状态，在检测完是否到达目标点之后，检测是否超过震荡时间，如果超过，就停止机器人，并且将状态改为CLEARING；

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

接下来就是获取导航过程中有效的速度，然后发布。computeVelocityCommands函数也是外部的一个函数。（？？）

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();
          //make sure that we send the velocity command to the base
          vel_pub_.publish(cmd_vel);
          if(recovery_trigger_ == CONTROLLING_R)
            recovery_index_ = 0;
        }

如果没有获取到有效速度，就让机器人停止然后进入到CLEARING状态或者是重新进行路径的规划。

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

          //check if we've tried to find a valid control for longer than our time limit
          if(ros::Time::now() > attempt_end){
     
            //we'll move into our obstacle clearing mode
            publishZeroVelocity();
            state_ = CLEARING;
            recovery_trigger_ = CONTROLLING_R;
          }
          else{
     
            //otherwise, if we can't find a valid control, we'll go back to planning
            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();
          }
      }

如果不是这三个状态，就进行重置。

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

          //check if we've tried to find a valid control for longer than our time limit
          if(ros::Time::now() > attempt_end){
     
            //we'll move into our obstacle clearing mode
            publishZeroVelocity();
            state_ = CLEARING;
            recovery_trigger_ = CONTROLLING_R;
          }
          else{
     
            //otherwise, if we can't find a valid control, we'll go back to planning
            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();
          }

planThread()函数

这个函数是经常被唤起的一个线程中进行的，自己对多线程不太懂，只是对函数内容学习，但是怎么进到这个函数不太清楚。（等待executeCb函数唤醒）
首先是判断是否阻塞线程。

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

然后获取全局路径

    //3. 获取规划的全局路径
    //这里的makePlan作用是获取机器人的位姿作为起点，然后调用全局规划器的makePlan返回规划路径，存储在planner_plan_
    planner_plan_->clear(); 
    bool gotPlan = n.ok() && makePlan(temp_goal, *planner_plan_);

如果获得规划的路径，就将其传递到latest_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();
      }

如果当前已经是规划路径的状态了，但是又没有得到新的路径，就判断一下有没有超出时间限制以及或者规划次数

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

如果还没到规划周期则定时器睡眠，在定时器中断中通过planner_cond_唤醒，这里规划周期为0

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

主要函数就是以上三个，但是之前提到的move_base_simple话题，订阅goalCB函数。make_plan服务的回调函数是planService。还有clear_costmaps服务回调函数是clearCostmapsService。这三个函数比较简单。功能也就是在之前hpp文件中提到的。

整个move_base源码就是注释和看完了，然后接下来应该会对整个逻辑画一个类似结构图的东西。

参考博客
https://www.cnblogs.com/JuiceCat/p/13040552.html
https://blog.csdn.net/Nksjc/article/details/78871528