博客轉載：https://blog.csdn.net/Neo11111/article/details/104583913
Movebase的主干部分是一個Action服務器，接收用戶發送的目標位置，並調用全局規划器和局部規划器，基於各層代價地圖的信息進行路徑規划，得到最優路徑，向用戶反饋機器人速度指令，驅動機器人按照指令運動，最終到達目標位置。這里不涉及到規划路徑和更新地圖的具體算法和實現，主要是完成了一個大的調用框架，具體的實現在各子過程的ROS封裝類中。
【相關文件】
- move_base/src/move_base_node.cpp
- move_base/src/move_base.cpp
【代碼分析】
1. move_base_node.cpp
這里初始化了節點“move_base_node”，Action服務的定義、全局規划器、局部規划器的調用都將在這個節點中進行。然后實例化了MoveBase這個類，上述工作以類成員函數的形式定義在這個類中。
實例化之后，Action開始監聽服務請求，並通過ros::spin()傳遞到Action的回調函數中進行處理。
#include <move_base/move_base.h>
int main(int argc, char** argv){
ros::init(argc, argv, "move_base_node");
tf::TransformListener tf(ros::Duration(10));
move_base::MoveBase move_base( tf );
ros::spin();
return(0);
}
2. move_base_node.cpp
這里對MoveBase類的類成員進行了定義，以下為比較重要的幾個函數。
–目錄–
構造函數 MoveBase::MoveBase | 初始化Action
控制主體 MoveBase::executeCb | 收到目標，觸發全局規划線程，循環執行局部規划
全局規划線程 void MoveBase::planThread | 調用全局規划
全局規划 MoveBase::makePlan | 調用全局規划器類方法，得到全局規划路線
局部規划 MoveBase::executeCycle | 傳入全局路線，調用局部規划器類方法，得到速度控制指令
<1> 構造函數 MoveBase::MoveBase
MoveBase類的構造函數進行了初始化工作，獲取了服務器上的參數值。
MoveBase::MoveBase(tf::TransformListener& tf) :
tf_(tf),
as_(NULL),
planner_costmap_ros_(NULL), controller_costmap_ros_(NULL),
bgp_loader_("nav_core", "nav_core::BaseGlobalPlanner"), //加載了baseGlobalPlanner的類庫
blp_loader_("nav_core", "nav_core::BaseLocalPlanner"), //加載了baseLocalPlanner的類庫
recovery_loader_("nav_core", "nav_core::RecoveryBehavior"), //加載了recoveryBehaviour的類庫
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) {
新建Action服務器，指針as_，綁定回調函數MoveBase::executeCb，這個函數是move_base的核心。
//as_指向action服務器，當執行as_->start()時調用MoveBase::executeCb函數
as_ = new MoveBaseActionServer(ros::NodeHandle(), "move_base", boost::bind(&MoveBase::executeCb, this, _1), false);
ros::NodeHandle private_nh("~");
ros::NodeHandle nh;
recovery_trigger_ = PLANNING_R;
從參數服務器設置用到的全局規划器、局部規划器、機器人底盤坐標系等，初始化三個用於存放plan的buffer。
//從參數服務器加載用戶輸入的參數，如果沒有，設置為默認值（第三個參數）
std::string global_planner, local_planner;
//全局規划器，默認navfn/NavfnROS
private_nh.param("base_global_planner", global_planner, std::string("navfn/NavfnROS"));
//局部規划器，默認TrajectoryPlannerROS
private_nh.param("base_local_planner", local_planner, std::string("base_local_planner/TrajectoryPlannerROS"));
//robot_base_frame，默認base_link
private_nh.param("global_costmap/robot_base_frame", robot_base_frame_, std::string("base_link"));
//global_frame，默認/map坐標系
private_nh.param("global_costmap/global_frame", global_frame_, std::string("/map"));
private_nh.param("planner_frequency", planner_frequency_, 0.0);
private_nh.param("controller_frequency", controller_frequency_, 20.0);
private_nh.param("planner_patience", planner_patience_, 5.0);
private_nh.param("controller_patience", controller_patience_, 15.0);
private_nh.param("max_planning_retries", max_planning_retries_, -1);//默認不開啟
private_nh.param("oscillation_timeout", oscillation_timeout_, 0.0);
private_nh.param("oscillation_distance", oscillation_distance_, 0.5);
//初始化三個plan的“緩沖池”數組
planner_plan_ = new std::vector<geometry_msgs::PoseStamped>();
latest_plan_ = new std::vector<geometry_msgs::PoseStamped>();
controller_plan_ = new std::vector<geometry_msgs::PoseStamped>();
planner_thread_線程所綁定的planThread函數是move_base的又一個重點，即全局規划線程。接下來聲明速度話題和即時目標的發布。
//創建規划器線程，在該線程里運行planThread函數
planner_thread_ = new boost::thread(boost::bind(&MoveBase::planThread, this));
vel_pub_ = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
current_goal_pub_ = private_nh.advertise<geometry_msgs::PoseStamped>("current_goal", 0 );
//發布MoveBaseActionGoal消息到/move_base/goal話題上
ros::NodeHandle action_nh("move_base");//movebase命名空間下的句柄
action_goal_pub_ = action_nh.advertise<move_base_msgs::MoveBaseActionGoal>("goal", 1);
//發布geometry_msgs::PoseStamped消息到/move_base_simple/goal話題上，回調函數goalCB會處理在/move_base_simple/goal話題上接收到的消息
//供nav_view和rviz等仿真工具使用
ros::NodeHandle simple_nh("move_base_simple");//move_base_simple下的句柄，訂閱goal話題
goal_sub_ = simple_nh.subscribe<geometry_msgs::PoseStamped>("goal", 1, boost::bind(&MoveBase::goalCB, this, _1));
//加載代價地圖的參數（內切、外接、清理半徑等），假設機器人的半徑和costmap規定的一致
private_nh.param("local_costmap/inscribed_radius", inscribed_radius_, 0.325);
private_nh.param("local_costmap/circumscribed_radius", circumscribed_radius_, 0.46);
private_nh.param("clearing_radius", clearing_radius_, circumscribed_radius_);
private_nh.param("conservative_reset_dist", conservative_reset_dist_, 3.0);
//一些標志位的設置
private_nh.param("shutdown_costmaps", shutdown_costmaps_, false);
private_nh.param("clearing_rotation_allowed", clearing_rotation_allowed_, true);
private_nh.param("recovery_behavior_enabled", recovery_behavior_enabled_, true);
初始化全局規划器和局部規划器的指針和各自的costmap，這里兩個規划器用到的地圖實質上是Costmap2DROS類的實例，這個類是ROS對costmap的封裝，類函數start()會調用各層地圖的active()函數，開始訂閱傳感器話題，對地圖進行更新，這部分在代價地圖部分詳述。
//全局規划器代價地圖global_costmap_ros_
planner_costmap_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tf_);
planner_costmap_ros_->pause();
//初始化全局規划器，planner_指針
try {
planner_ = bgp_loader_.createInstance(global_planner);
planner_->initialize(bgp_loader_.getName(global_planner), planner_costmap_ros_);
} catch (const pluginlib::PluginlibException& ex) {
ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", global_planner.c_str(), ex.what());
exit(1);
}
//本地規划器代價地圖controller_costmap_ros_
controller_costmap_ros_ = new costmap_2d::Costmap2DROS("local_costmap", tf_);
controller_costmap_ros_->pause();
//初始化本地規划器，tc_指針
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_);
} catch (const pluginlib::PluginlibException& ex) {
ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", local_planner.c_str(), ex.what());
exit(1);
}
//根據傳感器數據動態更新全局和本地的代價地圖
planner_costmap_ros_->start();
controller_costmap_ros_->start();
略去一部分跟主體關系不大的代碼，接下來啟動movebase的action服務器，並開啟參數動態配置。
//調用action server的start函數，服務器啟動 as_->start(); dsrv_ = new dynamic_reconfigure::Server<move_base::MoveBaseConfig>(ros::NodeHandle("~")); dynamic_reconfigure::Server<move_base::MoveBaseConfig>::CallbackType cb = boost::bind(&MoveBase::reconfigureCB, this, _1, _2); dsrv_->setCallback(cb); }
<2> 控制主體 MoveBase::executeCb
executeCb是Action的回調函數，它是MoveBase控制流的主體，它調用了MoveBase內另外幾個作為子部分的重要成員函數，先后完成了全局規划和局部規划。在函數的開始部分，它對Action收到的目標進行四元數檢測、坐標系轉換，然后將其設置為全局規划的目標，並設置了一些標志位。
void MoveBase::executeCb(const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal) { //檢測收到的目標位置的旋轉四元數是否有效，若無效，直接返回 if(!isQuaternionValid(move_base_goal->target_pose.pose.orientation)){ as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on goal because it was sent with an invalid quaternion"); return; } //將目標位置轉換到global坐標系下（geometry_msgs形式） geometry_msgs::PoseStamped goal = goalToGlobalFrame(move_base_goal->target_pose); //啟動全局規划 boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_); //用接收到的目標goal來更新全局變量，即全局規划目標，這個值在planThread中會被用來做全局規划的當前目標 planner_goal_ = goal; //全局規划標志位設為真 runPlanner_ = true; //開始全局規划並於此處阻塞
接下來，由於全局規划器線程綁定的函數plannerThread()里有planner_cond_對象的wait函數，在這里調用notify會直接啟動全局規划器線程，進行全局路徑規划，函數plannerThread的內容后述。
planner_cond_.notify_one(); lock.unlock(); //全局規划完成后，發布目標到current_goal話題上 current_goal_pub_.publish(goal); //創建一個全局規划容器 std::vector<geometry_msgs::PoseStamped> global_plan; //局部規划頻率 ros::Rate r(controller_frequency_); //如果代價地圖是被關閉的，這里重啟 if(shutdown_costmaps_){ ROS_DEBUG_NAMED("move_base","Starting up costmaps that were shut down previously"); planner_costmap_ros_->start(); controller_costmap_ros_->start(); } //上一次有效的局部規划時間設為現在 last_valid_control_ = ros::Time::now(); //上一次有效的全局規划時間設為現在 last_valid_plan_ = ros::Time::now(); //上一次震盪重置時間設為現在 last_oscillation_reset_ = ros::Time::now(); //對同一目標的全局規划次數記錄歸為0 planning_retries_ = 0; ros::NodeHandle n;
全局規划完成，接下來循環調用executeCycle函數來控制機器人進行局部規划，完成相應跟隨。
while(n.ok()) { //c_freq_change_被初始化為false //如果c_freq_change_即局部規划頻率需要中途更改為真，用更改后的controller_frequency_來更新r值 if(c_freq_change_) { ROS_INFO("Setting controller frequency to %.2f", controller_frequency_); r = ros::Rate(controller_frequency_); c_freq_change_ = false; }
這里需要進行判斷：
① 如果action的服務器被搶占，可能是“局部規划進行過程中收到新的目標”，也可能是“收到取消行動的命令”。
- 如果是收到新目標，那么放棄當前目標，重復上面對目標進行的操作，使用新目標。並重新全局規划；
- 如果是收到取消行動命令，直接結束返回。
② 如果服務器未被搶占，或被搶占的if結構已執行完畢，接下來開始局部規划，調用executeCycle函數，並記錄局部控制起始時間。
//如果action的服務器被搶占 if(as_->isPreemptRequested()){ if(as_->isNewGoalAvailable()){ //如果獲得了新目標，接收並存儲新目標，並將上述過程重新進行一遍 move_base_msgs::MoveBaseGoal new_goal = *as_->acceptNewGoal(); //檢測四元數是否有效 if(!isQuaternionValid(new_goal.target_pose.pose.orientation)){ as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on goal because it was sent with an invalid quaternion"); return; } //將新目標坐標轉換到全局坐標系（默認/map）下 goal = goalToGlobalFrame(new_goal.target_pose); //重設恢復行為索引位為0 recovery_index_ = 0; //重設MoveBase狀態為全局規划中 state_ = PLANNING; //重新調用planThread進行全局規划 lock.lock(); planner_goal_ = goal; runPlanner_ = true; planner_cond_.notify_one(); lock.unlock(); //全局規划成功后，發布新目標到current_goal話題上 ROS_DEBUG_NAMED("move_base","move_base has received a goal of x: %.2f, y: %.2f", goal.pose.position.x, goal.pose.position.y); current_goal_pub_.publish(goal); last_valid_control_ = ros::Time::now(); last_valid_plan_ = ros::Time::now(); last_oscillation_reset_ = ros::Time::now(); planning_retries_ = 0; } else { //否則，服務器的搶占是由於收到了取消行動的命令 //重置服務器狀態 resetState(); //action服務器清除相關內容，並調用setPreempted()函數 ROS_DEBUG_NAMED("move_base","Move base preempting the current goal"); as_->setPreempted(); //取消命令后，返回 return; } } //服務器接收到目標后，沒有被新目標或取消命令搶占 //檢查目標是否被轉換到全局坐標系（/map）下 if(goal.header.frame_id != planner_costmap_ros_->getGlobalFrameID()){ goal = goalToGlobalFrame(goal); //恢復行為索引重置為0，MoveBase狀態置為全局規划中 recovery_index_ = 0; state_ = PLANNING; lock.lock(); planner_goal_ = goal; runPlanner_ = true; planner_cond_.notify_one(); lock.unlock(); ROS_DEBUG_NAMED("move_base","The global frame for move_base has changed, new frame: %s, new goal position x: %.2f, y: %.2f", goal.header.frame_id.c_str(), goal.pose.position.x, goal.pose.position.y); current_goal_pub_.publish(goal); last_valid_control_ = ros::Time::now(); last_valid_plan_ = ros::Time::now(); last_oscillation_reset_ = ros::Time::now(); planning_retries_ = 0; } //記錄開始局部規划的時刻為當前時間 ros::WallTime start = ros::WallTime::now(); //調用executeCycle函數進行局部規划，傳入目標和全局規划路線 bool done = executeCycle(goal, global_plan);
若局部規划完成，結束。
if(done) return; //記錄從局部規划開始到這時的時間差 ros::WallDuration t_diff = ros::WallTime::now() - start; //打印用了多長時間完成操作 ROS_DEBUG_NAMED("move_base","Full control cycle time: %.9f\n", t_diff.toSec()); //用局部規划頻率進行休眠 r.sleep(); //cycleTime用來獲取從r實例初始化到r實例被調用sleep函數的時間間隔 //時間間隔超過了1/局部規划頻率，且還在局部規划，打印“未達到實際要求，實際上時間是r.cycleTime().toSec()” if(r.cycleTime() > ros::Duration(1 / controller_frequency_) && state_ == CONTROLLING) ROS_WARN("Control loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds", controller_frequency_, r.cycleTime().toSec()); } //喚醒全局規划線程，以使它能夠“干凈地退出” lock.lock(); runPlanner_ = true; planner_cond_.notify_one(); lock.unlock(); //如果節點被關閉了，那么Action服務器也關閉並返回 as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on the goal because the node has been killed"); return; }
<3> 全局規划線程 void MoveBase::planThread()
planThread()的核心是調用makePlan函數，該函數中實際進行全局規划。全局規划線程時刻等待被executeCB函數喚醒，當executeCB函數中喚醒planThread並將標志位runPlanner_設置為真，跳出內部的循環，繼續進行下面部分。
void MoveBase::planThread(){ ROS_DEBUG_NAMED("move_base_plan_thread","Starting planner thread..."); ros::NodeHandle n; ros::Timer timer; //標志位置為假，表示線程已喚醒 bool wait_for_wake = false; boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_); while(n.ok()){ //不斷循環，直到wait_for_wake（上面行已置為假）和runPlanner_為真，跳出循環 while(wait_for_wake || !runPlanner_){ //如果waitforwake是真或者runplanner是假，不斷執行循環，wait()等待 ROS_DEBUG_NAMED("move_base_plan_thread","Planner thread is suspending"); //調用wait函數時，函數會自動調用lock.unlock()釋放鎖，使得其他被阻塞在鎖競爭上的線程得以繼續執行 //比如executeCB中先把標志位runPlanner設為true，然后用notify，使這里的鎖由unlock變成lock，競爭到了資源，並且這里的規划線程開始執行 planner_cond_.wait(lock); wait_for_wake = false; } //start_time設為當前時間 ros::Time start_time = ros::Time::now();
調用makePlan函數，進行全局規划，如果成功，結果將儲存進planner_plan_中。接下來對makePlan函數的結果進行判斷：
① 若全局規划成功，則交換planner_plan_和latest_plan的值，即令latest_plan中存儲的是本次全局規划的結果（最新），planner_plan_中存儲的是上次全局規划的結果（次新）。設置標志位new_global_plan_ = true，表示得到了新的全局規划路線，並設置Movebase狀態標志位state_為CONTROLLING，即全局規划完成，開始進行局部控制。
② 如果全局規划失敗，MoveBase還在planning狀態，即機器人沒有移動，則進入自轉模式。
//把全局中被更新的全局目標planner_goal存儲為臨時目標 geometry_msgs::PoseStamped temp_goal = planner_goal_; lock.unlock();// ROS_DEBUG_NAMED("move_base_plan_thread","Planning..."); //全局規划初始化，清空 planner_plan_->clear(); //調用MoveBase類的makePlan函數，如果成功為臨時目標制定全局規划planner_plan_，則返回true bool gotPlan = n.ok() && makePlan(temp_goal, *planner_plan_); //若成功為臨時目標制定全局規划 if(gotPlan){ //輸出成功制定全局規划，並打印規划路線上的點數 ROS_DEBUG_NAMED("move_base_plan_thread","Got Plan with %zu points!", planner_plan_->size()); //用指針交換planner_plan_和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"); //確保只有在我們還沒到達目標時才啟動controller以局部規划 //如果runPlanner_在調用此函數時被置為真，將MoveBase狀態設置為CONTROLLING（局部規划中） if(runPlanner_) state_ = CONTROLLING; //如果規划頻率小於0，runPlanner_置為假 if(planner_frequency_ <= 0) runPlanner_ = false; lock.unlock(); } //如果全局規划失敗並且MoveBase還在planning狀態，即機器人沒有移動，則進入自轉模式 else if(state_==PLANNING){ ROS_DEBUG_NAMED("move_base_plan_thread","No Plan..."); //最遲制定出本次全局規划的時間=上次成功規划的時間+容忍時間 ros::Time attempt_end = last_valid_plan_ + ros::Duration(planner_patience_); //檢查時間和次數是否超過限制，若其中一項不滿足限制，停止全局規划 lock.lock(); //對同一目標的全局規划的次數記錄+1 planning_retries_++; //如果runplanner被置為真，且目前超時或超次數，進入恢復行為模式 if(runPlanner_ && (ros::Time::now() > attempt_end || planning_retries_ > uint32_t(max_planning_retries_))){ //將MoveBase狀態設置為恢復行為 state_ = CLEARING; //全局規划標志位置為假 runPlanner_ = false; //發布0速度 publishZeroVelocity(); //恢復行為觸發器狀態設置為全局規划失敗 recovery_trigger_ = PLANNING_R; } lock.unlock(); } } }
該函數最后部分的代碼與主體關系不大，這里略去。
<4> 全局規划 MoveBase::makePlan
該函數先進行一些預備工作，如檢查全局代價地圖、起始位姿，然后將起始位姿的數據格式做轉換。
bool MoveBase::makePlan(const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan){ boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(planner_costmap_ros_->getCostmap()->getMutex())); //初始化空plan plan.clear(); //如果沒有全局代價地圖，返回false，因為全局規划必須基於全局代價地圖 if(planner_costmap_ros_ == NULL) { ROS_ERROR("Planner costmap ROS is NULL, unable to create global plan"); return false; } //如果得不到機器人的起始位姿，返回false tf::Stamped<tf::Pose> global_pose; if(!planner_costmap_ros_->getRobotPose(global_pose)) { ROS_WARN("Unable to get starting pose of robot, unable to create global plan"); return false; } geometry_msgs::PoseStamped start; tf::poseStampedTFToMsg(global_pose, start);
接下來是實際進行全局規划的函數，調用全局規划器的makePlan函數planner_-> makePlan(start, goal, plan)，傳入機器人當前位姿和目標，得到plan，若規划失敗或得到的plan為空，返回false，否則返回true。關於全局規划的具體算法實現，在NavFn部分具體學習理解。
//調用BaseGlobalPlanner類的makePlan函數做全局規划 //如果全局規划失敗或者全局規划為空，返回false if(!planner_-> makePlan(start, goal, plan) || plan.empty()){ ROS_DEBUG_NAMED("move_base","Failed to find a plan to point (%.2f, %.2f)", goal.pose.position.x, goal.pose.position.y); return false; } return true; }
<5> 局部規划 MoveBase::executeCycle
executeCycle函數的作用是進行局部規划，函數先聲明了將要發布的速度，然后獲取當前位姿並格式轉換。
bool MoveBase::executeCycle(geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& global_plan){ boost::recursive_mutex::scoped_lock ecl(configuration_mutex_); //聲明速度消息 geometry_msgs::Twist cmd_vel; //聲明全局姿態 tf::Stamped<tf::Pose> global_pose; //從全局代價地圖上獲取當前位姿 planner_costmap_ros_->getRobotPose(global_pose); //把當前位姿轉換為geometry_msgs::PoseStamped格式，存儲在current_position中 geometry_msgs::PoseStamped current_position; tf::poseStampedTFToMsg(global_pose, current_position); //feedback指的是從服務端周期反饋回客戶端的信息，把當前位姿反饋給客戶端 move_base_msgs::MoveBaseFeedback feedback; feedback.base_position = current_position; as_->publishFeedback(feedback);
做幾個判斷，判斷機器人是否被困住（若是，則進入恢復行為，即針對機器人運動異常情況做出的指令，具體內容再該部分學習理解），並檢查局部規划的地圖是否是當前的，否則發布零速，停止規划，制停機器人。
//如果長時間內移動距離沒有超過震盪距離，那么認為機器人在震盪（長時間被困在一片小區域），進入恢復行為
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;
}
//檢查局部代價地圖是否是當前的
if(!controller_costmap_ros_->isCurrent()){
ROS_WARN("[%s]:Sensor data is out of date, we're not going to allow commanding of the base for safety",ros::this_node::getName().c_str());
publishZeroVelocity();
return false;
}
通過標志位判定全局規划是否得出新的路線，然后通過指針交換，將latest_plan_（最新的全局規划結果）的值傳遞給controller_plan_即局部規划使用，然后將上一次的局部規划路線傳遞給latest_plan。
//new_global_plan_標志位在planThread中被置為真，表示生成了全局規划 if(new_global_plan_){ //重置標志位 new_global_plan_ = false; ROS_DEBUG_NAMED("move_base","Got a new plan...swap pointers"); std::vector<geometry_msgs::PoseStamped>* temp_plan = controller_plan_; //在指針的保護下，交換latest_plan和controller_plan的值 boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_); //controller_plan_存儲【當前最新制定好的待到達的全局規划】 controller_plan_ = latest_plan_; //使得全局規划制定好的planner_plan經由latest_plan一路傳遞到controller_plan供局部規划器使用 latest_plan_ = temp_plan; lock.unlock(); ROS_DEBUG_NAMED("move_base","pointers swapped!");
在實例tc_上調用局部規划器BaseLocalPlanner的類函數setPlan()， 把全局規划的結果傳遞給局部規划器，如果傳遞失敗，退出並返回。
if(!tc_->setPlan(*controller_plan_)){ ROS_ERROR("Failed to pass global plan to the controller, aborting."); resetState(); //停止全局規划線程 lock.lock(); runPlanner_ = false; lock.unlock(); //停止Action服務器，打印“將全局規划傳遞至局部規划器控制失敗” as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to pass global plan to the controller."); return true; } //如果我們找到有效的局部規划路線，且恢復行為是“全局規划失敗”，重置恢復行為索引 if(recovery_trigger_ == PLANNING_R) recovery_index_ = 0; }
對MoveBase狀態進行判斷，由於局部規划在全局規划結束后才調用，所以有以下幾種結果：
① PLANNING：全局規划還沒完成，還沒得到一個全局路線，那么喚醒一個全局規划線程去制定全局路線
switch(state_){ //如果我們還在全局規划狀態 case PLANNING: { boost::recursive_mutex::scoped_lock lock(planner_mutex_); runPlanner_ = true; planner_cond_.notify_one(); } ROS_DEBUG_NAMED("move_base","Waiting for plan, in the planning state."); break;
② CONTROLLING：全局規划成功，得到全局路線，這里進行真正的局部規划：
- 如果已經位於終點，結束局部規划；
- 如果沒到終點，檢查機器人是否被困住，如果是，則進入恢復行為；
- 如果沒到終點，且狀態正常，調用局部規划器實例tc_->computeVelocityCommands(cmd_vel)函數，這個函數結合傳入的全局規划路線和其他因素得出局部規划結果，即速度指令，存放在cmd_vel中，將其發布，控制機器人運行。computeVelocityCommands(cmd_vel)函數內容具體在base_local_planner中，它通過在給定速度范圍內模擬各個方向的速度得到當前可能的小范圍行駛路線，進一步對得到的路線進行評估，得分最高的路線對應的速度就是函數得到的結果。所以局部規划更像是不斷循環進行的“短期規划”，實現在該部分詳述。
//如果全局規划成功，進入CONTROLLING狀態，就去找有效的速度控制 case CONTROLLING: ROS_DEBUG_NAMED("move_base","In controlling state."); //查詢是否到達終點，如果到達終點，結束 if(tc_->isGoalReached()){ ROS_DEBUG_NAMED("move_base","Goal reached!"); resetState(); //結束全局規划線程 boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_); //標志位置false runPlanner_ = false; lock.unlock(); //Action返回成功 as_->setSucceeded(move_base_msgs::MoveBaseResult(), "Goal reached."); return true; } //如果未到達終點，檢查是否處於振盪狀態 if(oscillation_timeout_ > 0.0 && last_oscillation_reset_ + ros::Duration(oscillation_timeout_) < ros::Time::now()) { //如果振盪狀態超時了，發布0速度 publishZeroVelocity(); //MoveBase狀態置為恢復行為 state_ = CLEARING; //恢復行為觸發器置為，長時間困在一片小區域 recovery_trigger_ = OSCILLATION_R; } { boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_ros_->getCostmap()->getMutex())); //局部規划器實例tc_被傳入了全局規划后，調用computeVelocityCommands函數計算速度存儲在cmd_vel中 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(); //向底盤發送速度控制消息，一個循環只發一次速度命令 vel_pub_.publish(cmd_vel); //如果恢復行為觸發器值是局部規划失敗，把索引置0 if(recovery_trigger_ == CONTROLLING_R) recovery_index_ = 0; }
若computeVelocityCommands(cmd_vel)函數計算失敗，且超時，則進入對應恢復行為，若未超時，則發布零速制停機器人，重新全局規划，再進行下一次局部規划。
//若速度計算失敗 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_); //若局部規划用時超過限制 if(ros::Time::now() > attempt_end){ //發布0速度，進入恢復行為，觸發器置為局部規划失敗 publishZeroVelocity(); state_ = CLEARING; recovery_trigger_ = CONTROLLING_R; } //若局部規划用時沒超過限制 else{ //發布0速度，在機器人當前位置再次回到全局規划 last_valid_plan_ = ros::Time::now(); planning_retries_ = 0; state_ = PLANNING; publishZeroVelocity(); boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_); runPlanner_ = true; planner_cond_.notify_one(); lock.unlock(); } } } break;
③ CLEARING：全局規划失敗，進入恢復行為
//如果全局規划失敗，進入了恢復行為狀態，我們嘗試去用用戶提供的恢復行為去清除空間 case CLEARING: ROS_DEBUG_NAMED("move_base","In clearing/recovery state"); //如果允許使用恢復行為，且恢復行為索引值小於恢復行為數組的大小 if(recovery_behavior_enabled_ && recovery_index_ < recovery_behaviors_.size()){ ROS_DEBUG_NAMED("move_base_recovery","Executing behavior %u of %zu", recovery_index_, recovery_behaviors_.size()); //開始恢復行為，在executeCycle的循環中一次次迭代恢復行為 recovery_behaviors_[recovery_index_]->runBehavior(); //we at least want to give the robot some time to stop oscillating after executing the behavior //上一次震盪重置時間設為現在 last_oscillation_reset_ = ros::Time::now(); //we'll check if the recovery behavior actually worked ROS_DEBUG_NAMED("move_base_recovery","Going back to planning state"); last_valid_plan_ = ros::Time::now(); planning_retries_ = 0; state_ = PLANNING; //update the index of the next recovery behavior that we'll try recovery_index_++; } //若恢復行為無效 else{ //打印“所有的恢復行為都失敗了，關閉全局規划器” ROS_DEBUG_NAMED("move_base_recovery","All recovery behaviors have failed, locking the planner and disabling it."); boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_); runPlanner_ = false; lock.unlock(); ROS_DEBUG_NAMED("move_base_recovery","Something should abort after this."); //反饋失敗的具體信息 if(recovery_trigger_ == CONTROLLING_R){ ROS_ERROR("Aborting because a valid control could not be found. Even after executing all recovery behaviors"); as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to find a valid control. Even after executing recovery behaviors."); } else if(recovery_trigger_ == PLANNING_R){ ROS_ERROR("Aborting because a valid plan could not be found. Even after executing all recovery behaviors"); as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to find a valid plan. Even after executing recovery behaviors."); } else if(recovery_trigger_ == OSCILLATION_R){ ROS_ERROR("Aborting because the robot appears to be oscillating over and over. Even after executing all recovery behaviors"); as_->setAborted(move_base_msgs::MoveBaseResult(), "Robot is oscillating. Even after executing recovery behaviors."); } resetState(); return true; } break; default: ROS_ERROR("This case should never be reached, something is wrong, aborting"); resetState(); //關閉全局規划器 boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_); runPlanner_ = false; lock.unlock(); as_->setAborted(move_base_msgs::MoveBaseResult(), "Reached a case that should not be hit in move_base. This is a bug, please report it."); return true; }
【總結】
在Movebase主體中，各層地圖的更新被啟動，Action的回調函數觸發全局規划線程，若成功，則將全局規划結果傳入局部規划器，循環進行局部規划，得到速度指令，控制機器人前進，直到到達目標。其間，需要判斷機器人是否到達終點（若是則規划停止）、機器人是否狀態異常如發生震盪行為（若是則進入恢復行為）、機器人是否超時（若是則停止規划發布零速，否則重新規划）等等。
這個主體是一個大的調用框架，保證了運動規划的正常運行，具體算法在各子過程中分別實