ros导航框架-move_base

这里我们首先梳理一下，从发布导航目标后到机器人导航到达目标点这个过程中，move_base内部是如何运行的。
进入MoveBase类中，重点关注3个函数MoveBase::planThread,MoveBase::executeCb和MoveBase::executeCycle

发布的导航目标首先进入MoveBase::executeCb回调函数中，
第一部分关键代码：

 boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
 planner_goal_ = goal;
 runPlanner_ = true;
 planner_cond_.notify_one();    // planThread阻塞结束  开始执行规划 
 lock.unlock();

在这里runPlanner_ 标志置位，同时触发条件变量planner_cond_， 这时把目光转移到MoveBase::planThread线程中，可以看到MoveBase::planThread线程刚开始运行时，是阻塞在下面代码里的planner_cond_.wait中的：

void MoveBase::planThread(){
	...
    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;
      }
      ...
	}
}

planner_cond_触发后，阻塞结束跳出 while(wait_for_wake || !runPlanner_){} 开始继续执行，
接下来拿到在MoveBase::executeCb回调中设置的planner_goal_作为导航目标，然后执行makePlan，这里在这里完成全局路径搜索，搜索结果放置于planner_plan_，它的类型是std::vector<geometry_msgs::PoseStamped>*。

//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();
bool gotPlan = n.ok() && makePlan(temp_goal, *planner_plan_);  

接着，如果搜索成功的话，planner_plan_会赋值给latest_plan_，runPlanner_在设置目标点后触发的回调函数中被设置为true，因此状态转为CONTROLLING，意味着开始执行控制，代码如下。

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

接着回到MoveBase::executeCb回调函数中，通过executeCycle函数对全局路径搜索得到的路径执行跟踪，

bool done = executeCycle(goal, global_plan);

在executeCycle函数中，首先通过tf获取实时位姿：

geometry_msgs::PoseStamped global_pose;
getRobotPose(global_pose, planner_costmap_ros_);
const geometry_msgs::PoseStamped& current_position = global_pose;

接着，new_global_plan_ == true,于是执行：

if(new_global_plan_){
      //make sure to set the new plan flag to false
      new_global_plan_ = false;
      //do a pointer swap under mutex
      std::vector<geometry_msgs::PoseStamped>* temp_plan = controller_plan_;
      boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
      controller_plan_ = latest_plan_;
      latest_plan_ = temp_plan;
      lock.unlock();

      if(!tc_->setPlan(*controller_plan_)){
        ...略
      }
}

可以看到，全局规划的结果latest_plan_赋值给了controller_plan_，然后，将全局规划的路径传给了局部规划模块tc_->setPlan(*controller_plan_)
那么，局部规划模块便会依据这个全局路径以及反馈的机器人实时位姿，求解出速度，细节见---------------------------------------
接着进入到最核心的一段代码中，重点关注CONTROLLING状态时的操作：

switch(state_){
      //if we are in a planning state, then we'll attempt to make a plan
      case PLANNING:
		...略
        break;

      //if we're controlling, we'll attempt to find valid velocity commands
      case CONTROLLING:

        //check to see if we've reached our goal
        if(tc_->isGoalReached()){
		...略
        }

        //check for an oscillation condition
        if(oscillation_timeout_ > 0.0 &&
            last_oscillation_reset_ + ros::Duration(oscillation_timeout_) < ros::Time::now())
        {
			...略
        }
        
        {
         boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_ros_->getCostmap()->getMutex()));
        // 进行局部规划 
        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;
        }
        else {
		...略          
        }
        }

        break;

      //we'll try to clear out space with any user-provided recovery behaviors
      case CLEARING:
        ...略
        break;
      default:
        ...略
    }

这里的核心就是局部规划器求解出速度 tc_->computeVelocityCommands(cmd_vel)，并将速度发布出去。