move_base源码解析

序言
好久没写博客了，最近把move_base源码包研究了一下，顺便以写博客的形式总结一下，同时里面掺杂了自己的心得体会，各路大神如果有不同的简介可以私下评论。

1 move_base_node.cpp
整个文件的开始以move_base_node.cpp开始。
从MoveBase::MoveBase的初始化开始。
as_作为维护动作服务的变量。

as_ = new MoveBaseActionServer(ros::NodeHandle(), "move_base", boost::bind(&MoveBase::executeCb, this, _1), false);

以上代码重点在MoveBase::executeCb线程上。

 1.进入MoveBase::executeCb线程。** 
 2. 1.1首先执行 

isQuaternionValid(move_base_goal->target_pose.pose.orientation)

用于判断目标点的格式对不对。
1.2然后执行

geometry_msgs::PoseStamped goal = goalToGlobalFrame(move_base_goal->target_pose)

用于将目标点的格式转换成通用格式
接下来发布目标点和设置发布频率。

current_goal_pub_.publish(goal); 
ros::Rate r(controller_frequency_);

然后是更新cost_map,以及设置时间戳

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循环函数，其中的if语句比较复杂。

//action的抢断函数
if(as_->isPreemptRequested())
{
      //有没有新的目标点出现
   if(as_->isNewGoalAvailable())
    {
     //如果有新的目标点出现，就判断目标点的格式对不对，跟上面的那个函数一样
      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);
       
       recovery_index_ = 0;
       state_ = PLANNING;
       
       //唤醒路径规划线程
       lock.lock();
       planner_goal_ = goal;
       runPlanner_ = true;
       planner_cond_.notify_one();
       lock.unlock();
       
       //把目标发给可视化平台
       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//如果没接收到新目标
    {
     
        //14.重置状态,设置为抢占式任务
          resetState();
          ROS_DEBUG_NAMED("move_base","Move base preempting the current goal");
          as_->setPreempted();
          return;
    }
//15.如果目标点的坐标系和全局地图的坐标系不相同，那就转换到世界坐标系，然后唤醒全局路径规划
      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;
      }
     
     //19. 到达目标点的真正工作，控制机器人进行跟随
      bool done = executeCycle(goal);
}

在executeCycle(goal)后面省略一些东西，主要是要进到这个函数了，判断机器人有没有到达目标位置。

1.2.1 进入ecuteCycle(goal)函数


```cpp
bool MoveBase::executeCycle(geometry_msgs::PoseStamped& goal)
{
    boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
    //we need to be able to publish velocity commands
    geometry_msgs::Twist cmd_vel;
    
    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);
    
    //检测机器人当前的位置是否是在震荡
    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;
    }

    //地图数据超时，即观测传感器数据不够新，停止机器人，返回false，其中publishZeroVelocity（）函数把geometry_msgs::Twist类型的cmd_vel设置为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;
    }

    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* temp_plan = controller_plan_;

      boost::unique_lock lock(planner_mutex_);
      controller_plan_ = latest_plan_;
      latest_plan_ = temp_plan;
      lock.unlock();
      ROS_DEBUG_NAMED("move_base","pointers swapped!");
      //tc是格局部路径
      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;
      
        if(recovery_trigger_ == PLANNING_R)
        recovery_index_ = 0;
      }
  //接下来进入的是一个庞大的switch中，只是根据不同的状态值决定接下来该怎么做
    //然后判断move_base状态，一般默认状态或者接收到一个有效goal时是PLANNING，
    //在规划出全局路径后state_会由PLANNING变为CONTROLLING，如果规划失败则由PLANNING变为CLEARING
    switch(state_){
      //if we are in a planning state, then we'll attempt to make a plan
      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;

      //if we're controlling, we'll attempt to find valid velocity commands
      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 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();//发送速度为0
          state_ = CLEARING;
          recovery_trigger_ = OSCILLATION_R;
        }

        
        boost::unique_lock lock(*(controller_costmap_ros_->getCostmap()->getMutex()));
        //获取有效速度,如果获取成功，直接发布到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();
          //make sure that we send the velocity command to the base
          vel_pub_.publish(cmd_vel);
          if(recovery_trigger_ == CONTROLLING_R)
            recovery_index_ = 0;
        }
        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 lock(planner_mutex_);
            runPlanner_ = true;
            planner_cond_.notify_one();
            lock.unlock();
          }
        }
        }

        break;

      //we'll try to clear out space with any user-provided recovery behaviors
      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
        //如果有可用恢复器，执行恢复动作，并设置状态为PLANNING：
        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{//如果没有可用恢复器，结束动作，返回true：
          ROS_DEBUG_NAMED("move_base_recovery","All recovery behaviors have failed, locking the planner and disabling it.");
          //disable the planner thread
          boost::unique_lock 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 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;
}

讲了这么多其实才讲完初始化move_base中的第一句。

2. 接下来就是一些基本的参数配置的问题。比如用那种全局路径规划器，局部路径规划器。

ros::NodeHandle private_nh("~");
    ros::NodeHandle nh;
    //触发模式（三种模式：规划、控制、振荡）设置为“规划中”：
    recovery_trigger_ = PLANNING_R;

    //get some parameters that will be global to the move base node，参数设置
    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);`

接下来设置了三个容器。

//为三种规划器设置内存缓冲区：（planner_plan_保存最新规划的路径，传递给latest_plan_，
    //然后latest_plan_通过executeCycle中传给controller_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_ = new boost::thread(boost::bind(&MoveBase::planThread, this));

2.1 接下来进这个线程里转转

void MoveBase::planThread()
{
     
    ROS_DEBUG_NAMED("move_base_plan_thread","Starting planner thread...");
    ros::NodeHandle n;
    ros::Timer timer;
    bool wait_for_wake = false;
    //1. 创建递归锁
    boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
    //干完前面的那些看也看不懂的东西后，进入while循环
    while(n.ok())
    {
     
       //检查是否需要路径规划，不需要的话就在这里循环了
        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();

        geometry_msgs::PoseStamped temp_goal = planner_goal_;
        lock.unlock();
        ROS_DEBUG_NAMED("move_base_plan_thread","Planning...");
        //路径规划
        planner_plan_->clear();
        //需要强调的是全局路径规划要进makeplan这个函数里面这里就不进去了
        bool gotPlan = n.ok() && makePlan(temp_goal, *planner_plan_);
        
        //如果获得了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");

        //目标的路径已经有了，就是控制过程的事情
        if(runPlanner_)
          state_ = CONTROLLING;
        if(planner_frequency_ <= 0)
          runPlanner_ = false;
        lock.unlock();
      }
      //5. 达到一定条件后停止路径规划,进入清障模式
       //如果没有规划出路径，并且处于PLANNING状态，则判断是否超过最大规划周期或者规划次数
       //如果是 则进入自转模式，否则应该会等待MoveBase::executeCycle的唤醒再次规划
      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();
        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();
      }
      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);
        }
      }
    }
}

3.咱们走完路径规划线程，回到move_base初始化.。接下来的工作是创建发布者

//创建发布者，话题名一个是cmd_vel，一个是cunrrent_goal，一个是goal：
    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");
    ac题tion_goal_pub_ = action_nh.advertise<move_base_msgs::MoveBaseActionGoal>("goal", 1);

然后是设定跟rviz相关的目标点

//提供消息类型为geometry_msgs::PoseStamped的发送goals的接口，比如cb为MoveBase::goalCB，
    //在rviz中输入的目标点就是通过这个函数来响应的：
    ros::NodeHandle simple_nh("move_base_simple");
    goal_sub_ = simple_nh.subscribe<geometry_msgs::PoseStamped>("goal", 1, boost::bind(&MoveBase::goalCB, this, _1));

然后是costmap参数的设定，像地图膨胀层这些参数。

//设置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);

接下来是设定全局规划器，局部规划器

//设置全局路径规划器，planner_costmap_ros_是costmap_2d::Costmap2DROS*类型的实例化指针
    planner_costmap_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tf_);
    planner_costmap_ros_->pause();

    //initialize the global 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);
    }

    //create the ros wrapper for the controller's costmap... and initializer a pointer we'll use with the underlying map
    //局部路径规划器
    controller_costmap_ros_ = new costmap_2d::Costmap2DROS("local_costmap", tf_);
    controller_costmap_ros_->pause();

    //create a local planner
    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);
    }

剩下的基本就差不多了。

//开始更新costmap：
    planner_costmap_ros_->start();
    controller_costmap_ros_->start();

    //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
    //开始清除地图服务：其中，调用了MoveBase::clearCostmapsService（）函数，提供清除一次costmap的功能
    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
    //如果不小心关闭了costmap， 则停用：
    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
    //加载指定的恢复器，加载不出来则使用默认的，这里包括了找不到路自转360：
    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);
  }

4.以上有的地方也没有弄太明白，在摸索的时候也发现自己有很多地方还需要不断学习，至于不懂的小伙伴可以下面留言。同时也提供一个链接，觉得写的不错：[https://www.cnblogs.com/JuiceCat/p/13040552.html]

注：本文是巧克力～唯心原创文章。创作不易，引用的话请注明出处！！！