ROS navigation分析：move_base分析
move_base package可以说是整个navigation stack里最重要的部分了。它的功能是给定一个目标点，尝试产生一系列能让机器人到达这个目标点的控制指令。简单来说，就是输入是一个goal(geometry_msgs/PoseStamped)，输出是一系列command(geometry_msgs/Twist)。详细一点来说，就是在已知环境地图和机器人在地图中位置的情况下，利用地图和传感器数据处理得到的 global costmap 和 local costmap，调用全局路径规划器得到一条全局路径，调用局部路径规划器使机器人在移动过程中能在大致沿着路径前进并动态规避障碍物，整个行进过程中还要handle一部分异常情况即recovery_behaviors。架构描述如下图。
接下来，分析一下源码（move_base.cpp）
先看构造函数：
sub topic：move_base只sub了一个topic，即"/move_base_simple/goal，类型是"geometry_msgs/PoseStamped"。在rviz中"2D Nav Goal"点击下，可以产生一条这种消息。
goal_sub_ = simple_nh.subscribe("goal", 1, boost::bind(&MoveBase::goalCB, this, _1)); pub topic：pub了三个消息，/cmd_vel是路径规划出来的控制机器人运动的速度指令；/current_goal是一个中间变量；/goal是actionlib相关的topic。
vel_pub_ = nh.advertise("cmd_vel", 1);
current_goal_pub_ = private_nh.advertise("current_goal", 0 );
action_goal_pub_ = action_nh.advertise("goal", 1); service server：提供了两个service，都是server端程序，/make_plan service提供路径规划的功能，call 这个service会调用planService()这个函数，service type是“nav_msgs/GetPlan”，传入起点坐标和终点坐标，传出一条路径；/clear_costmaps service提供清除一次costmap的功能，call这个service会调用clearCostmapService() 这个函数，没有返回值。
make_plan_srv_ = private_nh.advertiseService("make_plan", &MoveBase::planService, this);
clear_costmaps_srv_ = private_nh.advertiseService("clear_costmaps", &MoveBase::clearCostmapsService, this); as_ = new MoveBaseActionServer(ros::NodeHandle(), "move_base", boost::bind(&MoveBase::executeCb, this, _1), false); planner_thread_ = new boost::thread(boost::bind(&MoveBase::planThread, this));
bgp_loader_("nav_core", "nav_core::BaseGlobalPlanner")
blp_loader_("nav_core", "nav_core::BaseLocalPlanner")
recovery_loader_("nav_core", "nav_core::RecoveryBehavior")others：构造函数里剩下的基本都是一些从参数服务器抓参数、初始化一些变量、初始化各种其它乱七八糟的等等。
再看一些主要函数：
goalCB函数：该函数是/move_base_simple/goal 的回调函数，也是我们在rviz里戳一个目标点触发move_base模块工作的入口函数。这个函数很简单，只是入口，并不是实际执行的核心函数。它的做的事情是将传入的目标点坐标封装成actionlib的goal。真正的执行函数是在构造函数里，actionlib server注册的executeCb()这个函数。
void MoveBase::goalCB(const geometry_msgs::PoseStamped::ConstPtr& goal){
ROS_DEBUG_NAMED("move_base","In ROS goal callback, wrapping the PoseStamped in the action message and re-sending to the server.");
move_base_msgs::MoveBaseActionGoal action_goal;
action_goal.header.stamp = ros::Time::now();
action_goal.goal.target_pose = *goal;
action_goal_pub_.publish(action_goal);
}executeCb函数：进入函数首先对传入的坐标做了一些预处理，确认点的坐标是有效之后，执行下面的操作，唤醒另外一个线程(planThread)。目的是触发全局路径规划，先得到一条路径。
boost::unique_lock lock(planner_mutex_);
planner_goal_ = goal;
runPlanner_ = true;
planner_cond_.notify_one();
lock.unlock(); 接下来会进入一个while()循环。
里面先做了一个判断，判断actionlib是否收到抢占信号。如果收到抢占信号，且是一个新的导航点进来的话，那么原来的规划的路径肯定是无效了，所以需要重新唤醒planThread再规划一条路径。也就是上面五行代码在这个if里又执行了一遍。
if(as_->isPreemptRequested()){
if(as_->isNewGoalAvailable()){然后又做了一个判断，如果目标点不是global坐标系下面的点，就把它转过去。
if(goal.header.frame_id != planner_costmap_ros_->getGlobalFrameID()){
goal = goalToGlobalFrame(goal);都判断完之后，就把目标点和规划得到的路径塞给executeCycle()函数，做具体执行。
bool done = executeCycle(goal, global_plan);
executeCycle函数：函数前面还是做了一堆预处理，略过了。比较重要的是这个if里，如果判断得到一个新路径，就调用tc->setPlan()函数传给局部路径规划器。setPlan函数的实现在base_local_planner文件夹下的local_planner_util.cpp里。
if (new_global_plan_){
......
if (!tc_->setPlan(*controller_plan_)){接着会进入move_base的状态机。
switch (state_){一共有三个状态：PLANNING、CONTROLLING、CLEARING。
PLANNING状态即规划状态，会把runPlanner_置为true，继续通知planThread干活。
CONTROLLING状态即控制状态。每次先查询是否到达终点，到达终点就runPlanner_置为false，同时给actionlib返回success。如果没到终点的话，就调用computeVelocityCommands继续让局部路径规划器工作，生成速度指令，控制底盘移动。
if (tc_->isGoalReached()){
......
boost::unique_lock lock(planner_mutex_);
runPlanner_ = false;
lock.unlock();
as_->setSucceeded(move_base_msgs::MoveBaseResult(), "Goal reached.");
return true;
}
if (tc_->computeVelocityCommands(cmd_vel)){
...... CLEARING状态是用来出发recovery的。会根据我们之前导入的plugin，执行一些动作。比如清除costmap、原地转圈什么的。
接下来看一下上面提到很多次，不停被唤醒的planThread。里面的逻辑也不复杂，删减一下，主要代码是下面几行。
如果不需要规划的时候，planner_cond_.wait(lock)会使线程挂起，避免浪费。唤醒状态下说明需要规划路径，调用makePlan函数使全局路径规划器工作生成一条路径。得到路径之后，更改move_base状态机的状态变量，使状态机从PLANNING状态切换到CONTROLLING状态。如果在配置文件里设置需要不停规划路径，就调用createTimer设置一个定时器不停唤醒这个线程。
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;
}
......
bool gotPlan = n.ok() && makePlan(temp_goal, *planner_plan_);
if (gotPlan){
......
if (runPlanner_)
state_ = CONTROLLING;
if (planner_frequency_ <= 0)
runPlanner_ = false;
lock.unlock();
}
......
if (planner_frequency_ > 0){
......
timer = n.createTimer(sleep_time, &MoveBase::wakePlanner, this);
}
}说到这里，其实move_base核心的函数就是这几个，executeCb()、executeCycle()、planThread()。把这几个看明白了基本就能掌握整个move_base的执行流程。剩下几个函数例如reconfigureCB()、clearCostmapWindows()、makePlan()等等这些功能都比较简单明了，就不在这里展开了。