move_base逻辑梳理

本文是作者在学习ROS时对move_base的理解，留下此文以便以后温习，读者如果有兴致阅读，发现了什么错误的话，希望能够不吝给予正确的理解。或者有不解的地方均可以提出疑问，在我能力和精力范围内我尽量解答。

一、基本要求

阅读之前需要涉及至少以下两点知识。
1 掌握相关的ros概念。 如怎么编写一个插件。
2 熟悉c++,掌握基本的多线程以及锁的概念。

二、navigation包下载及编译

参考如下文章：
ROS-Navigation教程–源码安装

三、什么是move_base?

在作者目前的理解里面 move_base是ROS中导航包navigation中的一个比较重要的包，完成了从机器人当前位置到目标位置的路径规划以及运动控制的功能。总的导航框架就在move_base中。该包中有两个文件：move_base_node.cpp和move_base.cpp。

四、move_base运行逻辑（重要）

1
代码运行以后会开启一个线程:planThread()，该线程功能是完成路径规划并把生成的路径保存到planner_plan_容器中。但是该线程第一次运行时会进入一个while循环中等待。直到executeCB函数中 runPlanner_ 被置为true并且唤醒该线程时，该循环才不会再进入。此后由于runPlanner_在没有其他意外条件发生时都不会被置为false，所以该线程会一直循环规划路径。线程每次循环规划出的路径都会保存至planner_plan_。

2
planThread线程运行的同时executeCB也在执行。他的主要功能就是根据获得的路径进行相关的局部路径规划，然后下发对应的控制速度: cmd_vel。直到机器人到达目标点才退出该函数中的大循环。每次循环都是采用planner_plan_中的数据。但是由于planThread()线程一直在规划出新的路径保存至planner_plan_中，所以在executeCB的大循环中planner_plan_中的路径数据并不是一成不变的。

3
planThread()和executeCB相互合作共同完成了整个路径规划以及导航操作。

五、move_base中非常重要的几个函数

//  该函数完成了局部路径规划以及运动控制
void MoveBase::executeCb(const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal)
// 该线程作用就是生成全局路径
void MoveBase::planThread()
//全局路径规划函数
bool makePlan(const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan);
// 完成局部路径规划和运动控制，executeCb中调用
bool MoveBase::executeCycle(geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& global_plan)
  

六、部分代码及逻辑讲解

为了防止大篇幅的代码和注解让人眼花缭乱，在接下来讲解的几个函数中只讲我认为比较关键的点。

MoveBase构造函数

// MoveBase构造函数注解
	//位于movebase构造函数中第60行
    //只是在此实例化一个对象，分配储存空间而已。因为MoveBaseActionServer中的最后一个参数被设置为false所以该对象实例化后并不会进入回调函数执行而是在最后调用as_->start()后开始执行。
    as_ = new MoveBaseActionServer(ros::NodeHandle(), "move_base", boost::bind(&MoveBase::executeCb, this, _1), false);

 
	//位于movebase构造函数中第86行
    //set up plan triple buffer 三个用于保存规划出的路径的容器 路径规划时使用的是 planner_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>();
	
	//位于movebase构造函数中第92行
    //创建一个线程，线程创建后就会开始运行不过在线程中第一次运行时会暂时挂起等待唤醒
    planner_thread_ = new boost::thread(boost::bind(&MoveBase::planThread, this));

    
	//位于movebase构造函数中第170行左右
    //此处开始运行executeCB回调函数
    //官方文档指出 actionlib::SimpleActionServer 的成员函数 start() 用于启动 actionserver 因为as_
    //实例化时 最后一个参数设置成了false (auto_start)
    as_->start();

    //以下动态调参配置 方便调试用 一般运行在导航包正常运行以后 可开启一个新终端输入 rosrun dynamic_reconfigure dynamic_reconfigure 进行动态调参
    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::planThread()

// 该线程创建以后会因为 runPlanner_ 值为false而挂起。当executeCB中唤醒线程前一刻时runPlanner_被设置为true
// 在这之后就正常情况下面没有被设置为fale过所以线程被唤醒以后就不在进入等待的循环中而是一直在大循环执行，
// 每个大循环都会调用一次 makeplan并重新把新路径装入planner_plan_容器中。(更新了路径)
// 在executeCB中没有意外发生时只会唤醒一次该线程，以后不再唤醒。会一致执行大循环每个循环下发一次控制速度，但是每次
// 循环中的路径可能会由于该线程一直在跑的缘故而发生改变
  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()){
      //check if we should run the planner (the mutex is locked)
      //就是这两个变量控制着线程的挂起与否，最主要的还是要看 runPlanner_
      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();
      //路径规划 这里面调用了路径规划函数 不过这些算法都是以插件的形式存在。而且a*和迪杰斯特拉算法都是实现了的，具体采用那个算法 需要在参数文件中将 use_dijkstra 设为true（采用dijkstra） false（采用a*）
      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);
        }
      }
    }
  }

executeCb函数 和 executeCycle函数

executeCycle在executeCb中被调用所以我连着一起讲解。 executeCb函数的主要逻辑是一个大循环在跑。唤醒线程以后，后面就会进入大循环，而每次循环主要做以下几个事情

1 检测是否有抢占式指令下达。

该指令有两个作用 ，一个是由客户端发布删除命令cancel掉当前goal并结束当前循环退出任务。二是新的goal下达了，接收goal ，唤醒一次线程重新规划路径，舍弃前一次的goal和其路径。
所涉及函数：as_->isPreemptRequested()

2 控制机器人运动

在路径规划完成以后会调用executeCycle函数，主要就是为了控制机器人到目标点。executeCycle中涉及到了局部路径规划，控制速度计算的相关算法。我认为比较重要的几个函数如下：

1) tc_->setPlan(*controller_plan_)

作用是将规划出的全局路径保存到相关的局部规划器中。以便局部规划器规划路径时采用。

2) tc_->isGoalReached()

显然是判断是否到达目标点的函数。

3) tc_->computeVelocityCommands(cmd_vel)

这个函数最终的结果是得到要下发的速度 cmd_vel，但是该函数调用了相关的局部最优路径求解函数。如teb。teb这类局部最优路径求解是比较复杂的所以本文不讲解(我还没弄透！)

七、总结

本文是站在对控制逻辑的理解基础上完成的，没有详细的去介绍代码的运行流程，但是作者认为已经解读了整个movebase主要的控制逻辑。对应navigation包中其他的内容打算以后学一个发一篇，哈哈哈。