Navigation源码阅读之move_base

move_base是小车控制的核心模块，这一块从宏观上调控了整个导航过程。先瞄一眼头文件：

typedef actionlib::SimpleActionServer<move_base_msgs::MoveBaseAction> MoveBaseActionServer;

  enum MoveBaseState
  {
    PLANNING,
    CONTROLLING,
    CLEARING
  };

  enum RecoveryTrigger
  {
    PLANNING_R,
    CONTROLLING_R,
    OSCILLATION_R
  };

它声明了一个actionlib，关于它的详细介绍点击https://www.cnblogs.com/feixiao5566/p/4757916.html的博文，另外，状态机告诉我们小车运行的状态分为规划路径中，控制中（运行中），清理中（recovery behaviors）三种。

一、类的主文件，MoveBase的构造函数只接受tf变换（base_link->map），在初始化列表中利用pluginlib加载了nav_core下面的三个基类，并初始化了全局代价地图和局部代价地图，这几个参数对整个运行过程至关重要。

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

actionlib服务器绑定了成员函数 MoveBase::executeCb，当有client对server寻求服务时，该成员函数将进行反应。

另外，注意这个新建的线程，它负责新的全局路径的规划，无论是一段导航刚开始时、中途换目标、避障绕行运行，都会调用该线程。

planner_thread_ = new boost::thread(boost::bind(&MoveBase::planThread, this));

构造函数的末尾，状态机被初始化为planning，并开启了dynamic_reconfigure，这样可以在rqt中动态调整句柄ros::NodeHandle("~")下的参数。

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

二、当一个goal发出时（例如在rviz中点击nav goal），它首先被MoveBase::goalCB函数捕获，这个函数很简单，它将geometry_msgs::PoseStamped格式的goal转化为actionlib格式的move_base_msgs::MoveBaseActionGoal发布，便于actionlib处理。

三、当actionlib格式发布后，MoveBase::executeCb函数开始运行，它先检查这个goal是否合法，再通过条件变量通知挂起的MoveBase::planThread干活。

planner_cond_.notify_one();

 １．看看MoveBase::planThread是怎么规划出全局路径的。这个函数乍看之下有些费解，需要对线程进行复习～

boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
    while(n.ok())
{
　　//check if we should run the planner (the mutex is locked)
　　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;
　　}
…………

这一段代表着锁对象传给了条件变量的wait函数中，该线程会被加入等待序列中，互斥量可以被其他线程申请获得。当条件变量被满足时该线程会再次被唤醒并获得互斥量。

当然一开始规划全局路径时线程被唤醒了，调用MoveBase::makePlan函数：

bool gotPlan = n.ok() && makePlan(temp_goal, *planner_plan_);

当然一般这个布尔量会是true的，这样会保存该次产生的全局路径，以及上一次产生的路径。当然如果路径没有产生，说明遇到问题了，例如小车陷进代价地图中，若没有及时解救会触发recovery behaviors。

２．MoveBase::makePlan是通过当前的全局代价地图产生全局路径的，它首先将小车当前位置设为start：

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

再调用global_planner的makePlan函数去计算自主导航路径，在类初始化时，默认使用的global_planner是navfn_ros，即NavFnROS::makePlan。这样便获得了新鲜出炉的全局路径。

３．再回到MoveBase::executeCb函数被岔开的地方，设置好时间戳后，开始了漫长的大循环，这个循环直到导航结束或者目的点被抢占。当出现了新目的点时，planThread被再次叫醒干活，重复一遍之前讲述的过程。当然，会再次检查目的点的合法性（格式、frame_id）。如果没有出现打断循环的信息，循环将调用MoveBase::executeCycle函数检查每个周期是否正常。

４．MoveBase::executeCycle这个函数，它不断确定小车当前位置交给actionlib（虽然这一点不是太明白）。

tf::Stamped<tf::Pose> global_pose;
getRobotPose(global_pose, planner_costmap_ros_);
geometry_msgs::PoseStamped current_position;
tf::poseStampedTFToMsg(global_pose, current_position);

接着，需要确认小车不会来回震荡，这通过判断是否运行了超过一定距离实现；并且，需要确认代价地图是否时刻在更新：

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

讲道理这个警告我看过很多次，有可能是代价地图更新的频率比较低，也有可能是改写时其他函数劫持了传感器信息导致。总之不保证代价地图的实时性小车将无法运行。

在第一次执行executeCycle或者遇到新的plan之后，需要调用DWAPlannerROS::setPlan（假设使用DWA产生速度）。当在正常执行导航过程中，进入状态机的判断：

（１）状态机是PLANNING，这可能会出现在避障过程不顺利的情况下，一个经典场景就是小车被很多移动的人围住，自然无法在一个控制周期内产生新路径。在这个情况，planThread会再次被唤醒。

（２）状态机是CONTROLLING，这属于正常情况，它先判断是否已到达终点，再进入本函数的关键部分——速度计算部分。

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

它调用base_local_planner（默认为DWA）的计算速度函数，成功则下发给base_controller。如果失败，说明遇到了障碍物需要重新规划。当然重新规划有个次数限制，超过了就宣告失败，进入清扫过程了，按源码的逻辑，这次导航就快没救了，需要关掉节点再重来。。。

if(ros::Time::now() > attempt_end)
{
  getRobotPose(global_pose, controller_costmap_ros_);
  lastControl_x=global_pose.getOrigin().x();
  lastControl_y=global_pose.getOrigin().y();
  lastControl_orien_z=global_pose.getRotation().getZ();

  //we'll move into our obstacle clearing mode
  publishZeroVelocity();
  state_ = CLEARING;
  recovery_trigger_ = CONTROLLING_R;
}

下面这一段是避障绕行的关键，这里直接粗暴地发布一个零速度让车停下，再重新唤醒planThread，实操发现这样对车的电机加速度要求很高，很容易抽风～

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

（３）状态机是CLEARING，它默认执行温柔清空——暴力清空——放弃的过程，在这里改写的地方很多，可以通过自定义清扫过程来改变小车的clearing状态，例如倒退一小段距离等等。。。

这就是MoveBase::executeCycle的周期过程，如果小车没有到终点，是会返回false的，我们再回到executeCb被岔开的地方。

５．

bool done = executeCycle(goal, global_plan);

//if we're done, then we'll return from execute
if(done)
  return;

//check if execution of the goal has completed in some way

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

当executeCycle返回true，executeCb的大循环就被跳出了。否则，计算一下本次循环的执行时间，超过了控制频率（可设置），会报警告。这个警告我也经常看见，多半是由于自主寻迹的算法耗费的时间有点多，或者ＤＷＡ计算速度的打分筛选过程太久，我们也不一定要用源码设计的A*，Dijkstra算法，大胆尝试一些别的算法～

四、这就是一次完整导航过程的调用流程，由于我以前的一些操作失误，经常让小车陷入clear与abort的陷阱中，对recovery behavior都有心理阴影了，这一块先不详细分析了。。。总之，move_base是导航的关键模块，调用了其他模块的很多接口，任何用navigation源码做导航算法的朋友都应该对其非常了解，这一块自由创作的空间也很大，让小车如臂使指般运行吧～