ROS Navigation的move_base类实现方法

move_base

navigation的系统组成如下，move_base的地位很重要。

MoveBase的构造函数中，使用了不同的NodeHandle，如上图看到订阅的是move_base_simaple/goal是使用simaple_nh之后分辨节点名称之下的topic。订阅和发布的消息:

ros::NodeHandle nh;
vel_pub_ = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
current_goal_pub_ = private_nh.advertise<geometry_msgs::PoseStamped>("current_goal", 0 );
ros::NodeHandle action_nh("move_base");
action_goal_pub_ = action_nh.advertise<move_base_msgs::MoveBaseActionGoal>("goal", 1);
ros::NodeHandle simple_nh("move_base_simple");
goal_sub_ = simple_nh.subscribe<geometry_msgs::PoseStamped>("goal", 1, boost::bind(&MoveBase::goalCB, this, _1));

系统状态根据两个变量表示：{state_， recovery_trigger_}

enum MoveBaseState{PLANNING, CONTROLLING, CLEARING};
enum RecoveryTrigger{PLANNING_R, CONTROLLING_R,OSCILLATION_R};

初始化了用来存储全局规划的路径序列，最终会把全局规划得到的最新路径latest_plan_赋值给controller_plan_传递给局部规划器。

//set up plan triple buffer
planner_plan_ = new std::vector();
latest_plan_ = new std::vector();
controller_plan_ = new std::vector();

开启costmap的更新。

// Start actively updating costmaps based on sensor data
planner_costmap_ros_->start();
controller_costmap_ros_->start();

global_planner

在构造函数中同样加载了global_planner的类，以及执行全局规划的线程。

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

规划线程在一个while的循环中，需要保证wait_for_wake==false,runPlanner_==true，同时planner_cond_没有被lock的情况下通过调用makePlan完成全局路径的规划。因此可以看出，在MoveBase::executeCb中可以根据系统的状态改变标志位的值，控制规划线程。在这里有个planner_frequency_的参数，用来表示规划的周期，如果规划用时少于规划周期，则timer=n.createTime(sleep_time,$MoveBase::wakePlanner,this);通过使用ROS定时器来唤醒规划线程。

MoveBaseActionServer

as_维护MoveBase的MoveBaseActionServer状态机，并且新建了一个executeCb回调线程。

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

先检查函数参数目标输入的合法性，主要是pose.orientation检查。然后调用geometry_msgs::PoseStamped goal = goalToGlobalFrame(move_base_goal->target_pose) 将goal转换为在全局地图下的goal。通过赋值planner_goal_=goal将目标点传入planThread, 然后开启planThread:runPlanner_=true; planner_cond_.notify_one()。
然后进入while(n.ok())循环：首先判断as_是否是可抢占的，以及检查是否有新的goal，如果有则处理新的goal。继续往下，判断goal的坐标系和planner_costmap_ros_坐标系是否是一致的，否则调用goal = goalToGlobalFrame(goal)；转换到planner_costmap_ros_的坐标系下，然后重新开启planThread。最终，我们拿到了全局规划的路径latest_plan_， 接下来调用核心代码：

首先会锁住MoveBase::reconfigureCB(……)，然后会处理震荡问题。以及controller_costmap_ros_是否是最新的局部规划地图，然后通过new_global_plan_判断是否全局规划得到可以执行的全局路径(会通过标志位runPlanner_=false;等使得planThread暂时不再执行makePlan`)

然后通过系统状态判断执行的代码，如果正在规划则继续规划……。 如果系统状态处于CONTROLLING状态，首先检查是否到达目的地if(tc_->isGoalReached()) ，如果到达目标点，则resetState()， 并暂停planThread: runPlanner_ = false; 如果满足震荡条件，则:机器人运动停止publishZeroVelocity();系统状态转入state_ = CLEARING; recovery_trigger_ = OSCILLATION_R; 系统的速度发布计算:tc_->computeVelocityCommands(cmd_vel)

        //check to see if we've reached our goal
        if(tc_->isGoalReached()){
          ROS_DEBUG_NAMED("move_base","Goal reached!");
          resetState();
          //disable the planner thread
          boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
          runPlanner_ = false;
          lock.unlock();
          as_->setSucceeded(move_base_msgs::MoveBaseResult(), "Goal reached.");
          return true;
        }
        //check for an oscillation condition
        if(oscillation_timeout_ > 0.0 &&
            last_oscillation_reset_ + ros::Duration(oscillation_timeout_) < ros::Time::now())
        {
          publishZeroVelocity();
          state_ = CLEARING;
          recovery_trigger_ = OSCILLATION_R;
        }

        {
         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 {
          ROS_DEBUG_NAMED("move_base", "The local planner could not find a valid plan.");
          ros::Time attempt_end = last_valid_control_ + ros::Duration(controller_patience_);

          //check if we've tried to find a valid control for longer than our time limit
          if(ros::Time::now() > attempt_end){
            //we'll move into our obstacle clearing mode
            publishZeroVelocity();
            state_ = CLEARING;
            recovery_trigger_ = CONTROLLING_R;
          }
          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();
          }
        }
        }

构造函数的依赖关系