ROS naviagtion analysis: move_base
这是navigation的第一篇文章，主要通过分析ROS代码级实现，了解navigation。本文从move_base入手。
机器人导航主要框架如图：
Navigation Stack主要组成部分：move_base:
用户调用movebase是通过传入带tf参数的构造函数：
move_base::MoveBase move_base( tf );
以下分析move_base的构造函数：
MoveBase::MoveBase(tf::TransformListener& tf)：
tf_(tf),
as_(NULL),
planner_costmap_ros_(NULL), controller_costmap_ros_(NULL),
bgp_loader_("nav_core", "nav_core::BaseGlobalPlanner"),
blp_loader_("nav_core", "nav_core::BaseLocalPlanner"),
recovery_loader_("nav_core", "nav_core::RecoveryBehavior"),
planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL),
runPlanner_(false), setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false) `这部分是构造函数的初始化列表，可以看到几个重要的参数：
planner_costmap_ros_(NULL), controller_costmap_ros_(NULL),
bgp_loader_("nav_core", "nav_core::BaseGlobalPlanner"),
blp_loader_("nav_core", "nav_core::BaseLocalPlanner"),
recovery_loader_("nav_core", "nav_core::RecoveryBehavior")planner_costmap_ros_是用于全局导航的地图，controller_costmap_ros_ 是局部导航用的地图，地图类型为经过ROS封装的costmap_2d::Costmap2DROS* 。关于地图类型的分析会在接下来的文章中进行。
bgp_loader_ 是global planner， blp_loader_ 是local planner。二者的声明为：
pluginlib::ClassLoader
pluginlib::ClassLoader
是属于一个模板类ClassLoader，实例化为BaseGlobalPlanner或者BaseLocalPlanner。关于pluginlib的分析也有在接下来的文章中进行。
bgp_loader_ 和 blp_loader_ 的作用是为以下类成员提供实例化：
boost::shared_ptr<nav_core::BaseLocalPlanner> tc_;
boost::shared_ptr<nav_core::BaseGlobalPlanner> planner_;
//initialize the global planner
try {
planner_ = bgp_loader_.createInstance(global_planner);
planner_->initialize(bgp_loader_.getName(global_planner), planner_costmap_ros_);
} `
` //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_);
}进入构造函数，第一句：
as_ = new MoveBaseActionServer(ros::NodeHandle(), "move_base", boost::bind(&MoveBase::executeCb, this, _1), false);as_维护movebase的actionServer状态机，并且新建了一个executeCb线程。
接下来从private_nh中获取参数设定值。
设置plan buffer：
planner_plan_ = new std::vector注意，这三个plan都是global_plan，最终由controller_plan_ 传给 local planner：
Line821 if(!tc_->setPlan(*controller_plan_)){
开启planner thread：
//set up the planner's thread
planner_thread_ = new boost::thread(boost::bind(&MoveBase::planThread, this));接下来设置一堆publisher， 包括cmd_vel，current_goal，goal。
然后是机器人的几何尺寸设定等。
然后实例化planner_costmap_ros_，controller_costmap_ros_
planner_costmap_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tf_);
controller_costmap_ros_ = new costmap_2d::Costmap2DROS("local_costmap", tf_);开启costmap基于传感器数据的更新：
// Start actively updating costmaps based on sensor data
planner_costmap_ros_->start();
controller_costmap_ros_->start();载入recovery behavior，这部分是状态机的设计问题，可以采用默认的设定，也可以用户指定：
if(!loadRecoveryBehaviors(private_nh)){
loadDefaultRecoveryBehaviors(); }然后开启actionserver: as_->start(); 开启参数动态配置服务，完了～这就是整个构造函数做的事情。
以下分析各个成员函数：
void MoveBase::clearCostmapWindows(double size_x, double size_y){
首先通过planner_costmap_ros_->getRobotPose(global_pose); 获取在全局地图的pose，然后以这个点为中心，找到以size_x和size_y 为边长的矩形的四个顶点，调用planner_costmap_ros_->getCostmap()->setConvexPolygonCost(clear_poly, costmap_2d::FREE_SPACE); 完成对机器人所在区域的clear工作。同样的操作在controller_costmap_ros_ 上也操作一遍，这样关于globa costmap 和local costmap都已经在机器人所在的区域完成clear工作。
bool MoveBase::planService(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &resp)
req参数包含了起点和目标信息。首先判断global planner 的costmap是否存在以及是否能够得到机器人所在位置，然后调用clearCostmapWindows 完成对机器人区域的clear，然后调用 if(!planner_->makePlan(start, req.goal, global_plan) || global_plan.empty()){ 这是完成plan计算的核心部分。逻辑判断这个调用是否成功，如果失败，则在目标区域附件搜索，更改req.goal的值，并重新调用makePlan，如果失败，则此次plan失败，无解。
bool MoveBase::makePlan(const geometry_msgs::PoseStamped& goal, std::vector
首先lock 住global costmap：
boost::unique_lock 然后判断global costmap是否存在以及是否能够获得机器人所在位置，如果失败，则直接返回失败。最后调用核心代码：
if(!planner_->makePlan(start, goal, plan) || plan.empty()){ 。
geometry_msgs::PoseStamped MoveBase::goalToGlobalFrame(const geometry_msgs::PoseStamped& goal_pose_msg){
这个函数的核心功能是获得机器人坐标系和全局地图坐标系的关系：
tf_.transformPose(global_frame, goal_pose, global_pose); 。
void MoveBase::wakePlanner(const ros::TimerEvent& event)
planner_cond_.notify_one(); 通过boost thread的条件变量planner_cond_ 唤醒线程 planThread。
MoveBase的核心函数是线程planThread：void MoveBase::planThread()
进入函数首先会将planner_mutex_ 锁住：boost::unique_lock
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;
}因此线程继续运行依赖于变量runPlanner_ ，唤醒线程依赖于条件变量planner_cond_ 这两个变量是后续其他函数需要唤醒并执行线程planThread的关键。
然后调用makePlan：
//run planner
planner_plan_->clear();
bool gotPlan = n.ok() && makePlan(temp_goal, *planner_plan_);
如果makePlan成功，则上锁，然后对planner_plan_ 拷贝，过程如下：
将最新的plan path给latest_plan_,然后将上一次的plan path给planner_plan_，这样这两个buffer就保存了最新的plan 和上一次的plan。
void MoveBase::executeCb(const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal)
首先检查传入的参数合法性，主要是pose.orientation检查。然后调用geometry_msgs::PoseStamped goal = goalToGlobalFrame(move_base_goal->target_pose) 将goal转换为在全局地图下的goal。
然后开启planner：
boost::unique_lock::mutex> lock(planner_mutex_);
planner_goal_ = goal;
runPlanner_ = true;
planner_cond_.notify_one();
lock.unlock(); 然后进入死循环：首先判断as_是否是可抢占的，检查是否有新的goal，如果有则处理新的goal，判断new goal的合法性，并开启planThread 为new goal 生成plan。继续往下，判断goal的坐标系和planner_costmap_ros_ 的坐标系是否是一致的，如果不是，则调用goal = goalToGlobalFrame(goal) 转换到 planner_costmap_ros_的坐标系下，然后重新开启planThread。最终，我们拿到了global plan， 接下来调用核心代码：
bool done = executeCycle(goal, global_plan);
在死循环最后sleep足够时间，以满足frequency的要求。
如果死循环被退出，则在本函数末尾开启planThread，似的线程能正常执行到末尾，当线程再次在死循环中运行时，检查while(n.ok()){ 会失败，然后线程正常清理退出。
bool MoveBase::executeCycle(geometry_msgs::PoseStamped& goal, std::vector
进入函数，第一行：boost::recursive_mutex::scoped_lock ecl(configuration_mutex_)会 锁住 函数void MoveBase::reconfigureCB(move_base::MoveBaseConfig &config, uint32_t level){
处理震荡问题：
if(distance(current_position, oscillation_pose_) >= oscillation_distance_)
{
last_oscillation_reset_ = ros::Time::now();
oscillation_pose_ = current_position;
if(recovery_trigger_ == OSCILLATION_R)
recovery_index_ = 0;
}然后检查controller_costmap_ros_ 是否是最新的if(!controller_costmap_ros_->isCurrent()){ 。然后检查是否有新的plan，如果有，则 上锁递推 controller_plan_和 latest_plan_。
然后将global的plan传递给tc_: if(!tc_->setPlan(*controller_plan_)){， 如果失败则关闭planThread并直接退出。
然后进入movebase的控制逻辑的状态机：
状态机根据两个变量实现状态表示：{state_， recovery_trigger_}
这两个变量的取值：
enum MoveBaseState{PLANNING, CONTROLLING, CLEARING};
enum RecoveryTrigger{PLANNING_R, CONTROLLING_R,OSCILLATION_R};RecoveryTrigger 对应的是在相应状态下出现失败。
注意，这个状态机和planThread 是在交互的。状态机一旦进入PLANNING状态，就唤醒planThread线程，在planThread线程中一旦找到plan就将state_设置为CONTROLLING，如果没有找到plan，则state_ = CLEARING;recovery_trigger_ = PLANNING_R;。
如果状态机在CONTROLLING状态，首先检查是否到达目的地if(tc_->isGoalReached()){ ，如果到达目的地，则resetState(); 并关闭planThread runPlanner_ = false; 如果震荡条件满足，则：publishZeroVelocity();state_ = CLEARING; recovery_trigger_ = OSCILLATION_R;；然后锁住local costmap ：
boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_ros_->getCostmap()->getMutex()));然后由baseLocalPlanner计算cmd_vel:
if(tc_->computeVelocityCommands(cmd_vel)){成功则可以控制地盘了，如果失败，检查是否超时，如果超时：publishZeroVelocity();state_ = CLEARING;recovery_trigger_ = CONTROLLING_R; 没有超时：state_ = PLANNING;publishZeroVelocity(); 并唤醒planThread线程。
如果状态机在CLEARING状态：
if(recovery_behavior_enabled_ && recovery_index_ < recovery_behaviors_.size())
{
recovery_behaviors_[recovery_index_]->runBehavior();**重点内容**
last_oscillation_reset_ = ros::Time::now();
state_ = PLANNING;
recovery_index_++;
}
else
{
//disable the planner thread
boost::unique_lock::mutex> lock(planner_mutex_);
runPlanner_ = false;
lock.unlock();
resetState();
return true;
} moveBase基本上主要内容就是这些，其他未分析的函数都比较简单。但是在moveBase的实现中，主要依赖于costmap_2d 以及global planner 和local planner。还有pluginlib的使用，简化了各种具体实现，因此在接下来的文章中着重分析这几部分。