move_base代码解析（一）MoveBase::executeCb

move_base是ROS中的经典路径规划算法，move_base 功能包提供了基于动作(action)的路径规划实现，move_base 可以根据给定的目标点，控制机器人底盘运动至目标位置，并且在运动过程中会连续反馈机器人自身的姿态与目标点的状态信息。

1、goalCB

move_base的主函数定义在move_base_node.cpp中，具体的对外publish以及subscriber实现在move_base.cpp中。对于move_base而言，它的输入一般只需要一个目标点就可以了:

goal_sub_ = simple_nh.subscribe<geometry_msgs::PoseStamped>("goal", 1, boost::bind(&MoveBase::goalCB, this, _1));

这是一个订阅函数，传入的是goal，主要作用是为rviz等提供调用借口，将geometry_msgs::PoseStamped形式的goal转换成move_base_msgs::MoveBaseActionGoal，再发布到对应类型的goal话题中。这个函数很简单，只是入口，并不是实际执行的核心函数。它的做的事情是将传入的目标点坐标封装成actionlib的goal。真正的执行函数是在构造函数里，actionlib server注册的executeCb()这个函数。

2、executeCB

executeCB函数定义在这里：

    //actionserver实例化一个对象，一直执行回调函数executeCb。
    //Action servrer接收外部的目标请求，驱动整个的路径规划和移动过程。
    //actionlib会启动一个线程，当外部请求到来时，调用MoveBase::executeCb回调函数处理
    as_ = new MoveBaseActionServer(ros::NodeHandle(), "move_base", boost::bind(&MoveBase::executeCb, this, _1), false);

接下来实例化函数主要分为以下部分：

2.1、判断四元数的合法性

executeCB函数首先会判断传入位姿的四元数的合法性，如果四元数不对就退出。

    //位姿的四元数合法性检查，如果四元数不对就返回
    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;
    }

四元数的判断函数如下：

bool MoveBase::isQuaternionValid(const geometry_msgs::Quaternion& q){
    //first we need to check if the quaternion has nan's or infs
    //1、首先检查四元数是否元素完整
    //isfinite()用于检查并返回给定数是否为有限数的函数,有限数是既不是无限也不是NaN(不是数字)的任何浮点数。 
    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;
    }
    //2、检查四元数是否趋近于0
    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){//四元数的长度不能为0
      ROS_ERROR("Quaternion has length close to zero... discarding as navigation goal");
      return false;
    }

    //next, we will normalize the quaternion and check that it transforms the vertical vector correctly
    //3、对四元数规范化，转化为vector
    tf_q.normalize();

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

    double dot = up.dot(up.rotate(tf_q.getAxis(), tf_q.getAngle()));
    //四元数的Z轴必须是垂直的
    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;
  }

它的判断主要分为了三个部分：
1、首先检查四元数是否元素完整
2、检查四元数是否趋近于0
3、对四元数规范化，转化为vector，判断四元数的Z轴必须是垂直的

2.2、将目标的坐标系统一转换为全局坐标系

确认四元数没有问题以后，需要将当前坐标转换到世界坐标系下：

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

主要思路是调用tf变换，根据当前的pose的frame_id以及世界坐标系的frame_id将当前坐标系下的坐标转换成世界坐标系下的坐标。

2.3、将速度置零

这里简单的通过调用一个publishZeroVelocity函数将机器人输出的速度设置为0

2.4、设置目标点并唤醒路径规划线程

前面123步是准备，第4步开始就是正式的处理路径规划事宜了：

    boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);//给该线程上锁
    planner_goal_ = goal;//传入目标并开启线程
    runPlanner_ = true;
    planner_cond_.notify_one();
    lock.unlock();//打开互斥锁

2.5、然后发布goal，设置控制频率

    //发布当前的目标坐标
    current_goal_pub_.publish(goal);
    //设置了一个频率，后面会用到
    ros::Rate r(controller_frequency_);

2.6、开启costmap更新

    if(shutdown_costmaps_){//如果代价地图被关闭，似乎在move_base中如果机器人长时间不运动的话代价地图会被关闭以节省资源
      ROS_DEBUG_NAMED("move_base","Starting up costmaps that were shut down previously");
      planner_costmap_ros_->start();
      controller_costmap_ros_->start();
    }

在move_base中，为了节省资源，代价地图似乎不是一直开着的。如果被关闭的话，每次路径规划前需要重新判断以下是否为开启状态，没有开启的话需要开启一下。

2.7、重置时间标志位

    //记录几个时间，最新的控制时间，上一次有效的规划，上一次最后的碰撞重置
    last_valid_control_ = ros::Time::now();
    last_valid_plan_ = ros::Time::now();
    last_oscillation_reset_ = ros::Time::now();
    planning_retries_ = 0;

这几个时间可能后面会用到

在完成以上几个步骤之后，程序会进入一个while循环，在这个循环中执行了以下步骤：

2.8、修改循环频率

      //如果频率被改变的话
      if(c_freq_change_)
      {
        ROS_INFO("Setting controller frequency to %.2f", controller_frequency_);
        r = ros::Rate(controller_frequency_);//修改频率
        c_freq_change_ = false;
      }

2.9、判断是否有抢占式目标以及目标点是否可用

if(as_->isPreemptRequested())
  if(as_->isNewGoalAvailable()){//是否有新的目标点
    //如果有新的目标，会接受的，但不会关闭其他进程
    move_base_msgs::MoveBaseGoal new_goal = *as_->acceptNewGoal();//接受新的目标点，这个同样是simple_action_server.h的函数
          

isPreemptRequested由actionlib/server/simple_action_server.h实现。ROS中对于action的抢占是否允许这个是怎么判定的？这个函数的返回可能是true也可能是false

2.10、判断四元数的合法性

同2.1，但是由于有新的goal所以需要重新再判断一次

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

2.11、将目标的坐标系统一转换为全局坐标系

同2.2，但是由于有新的goal所以需要重新再判断一次

          //将目标位置转换到global坐标系下（geometry_msgs形式）
          goal = goalToGlobalFrame(new_goal.target_pose);

2.12、设置状态为PLANNING

move_base的状态机有三种，PLANNING、CONTROLLING以及CLEARING。规划时的状态为PLANNING

          //enum MoveBaseState {
          //  PLANNING,
          //  CONTROLLING,
          //  CLEARING
          //};move_base的状态，状态改变为PLANNING全局路径规划状态
          state_ = PLANNING;

2.13、设置目标点并唤醒路径规划线程

同2.4，但是由于有新的goal所以需要重新再执行一次

2.14、把goal发布给可视化工具

同2.5，但是由于有新的goal所以需要重新再执行一次

          //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);//发布新的目标点的坐标

2.15、重置规划时间

同2.7，但是由于有新的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;

2.16、重置状态,设置为抢占式任务

这里的判断是对应2.9中的第二个if判断：if(as_->isNewGoalAvailable())。如果当前没有新的目标点可用时，需要重置当前的状态机，以及允许任务抢占：

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

2.17、如果目标点的坐标系和全局地图的坐标系不相同

虽然前面进行过一次tf变换，但是在进行路径规划之前还是需要重新判断一下goal是否已经转换成功了，如果之前的tf变换没有成功的话需要重新处理，这里重新处理的流程前面都有同样的函数就不展开讲了。

      //再判断一次目标点的frame_id，需要确认目标点是在世界坐标系下的。如果目标点不在世界坐标系下
      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;//recovery重试次数设置为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;
      }

2.18、调用和计算全局路径,并进行局部规划计算

这里才是真正路径规划执行的地方，executeCB是路径规划的启动函数，而executeCycle是路径规划的执行函数，后面这个单独展开讲。它的输入是目标点，输出是bool变量代表是否到达目标点。

      bool done = executeCycle(goal);
      //if we're done, then we'll return from execute
      //如果完成任务则返回
      if(done)
        return;

2.19、控制时间以及控制频率

这里会循环输出当前的控制周期，如果控制频率过高的话会输出一个警告信息，之前自己做仿真的时候就遇到过这个问题，当时控制频率高于实际运算频率的时候终端会一直报警告：

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

2.20、如果while循环被中断

在executeCB中，新的goal都在while循环中处理，但是如果ros发生故障，算法跳出while(n.ok())语句的时候，需要对当前的状态进行一定的处理：

    //唤醒规划线程并退出
    lock.lock();
    runPlanner_ = true;
    planner_cond_.notify_one();
    lock.unlock();

    //if the node is killed then we'll abort and return
    //如果节点被中断
    //将move_base的结果状态status置为Aborted，并且设置text内容为Aborting on goal because it was sent with an invalid quaternion，反馈Aborted的原因；
    as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on the goal because the node has been killed");
    return;

2.21、executeCb小结executeCb

executeCb函数是move_base中路径规划的起始函数。在这里传入一个goal目标点，该函数首先需要判断传入目标点的四元数是否合理，然后将该坐标转换到世界坐标系下同时通过publisher发布该坐标用于可视化显示。随后开启线程以及代价地图costmap，记录当前的开始时间以及规划的状态机state，最后完成这些后函数开始调用executeCycle(goal)函数执行正式的路径规划过程。