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<geometry_msgs::PoseStamped>();
latest_plan_ = new std::vector<geometry_msgs::PoseStamped>();
controller_plan_ = new std::vector<geometry_msgs::PoseStamped>();

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

構造函數的依賴關係