Navigation（五） Move_base最最全解析（按執行邏輯梳理）

本文轉載自查看原文 2020-06-04 12:06 3918

零、前言

此前寫了一篇對move_base的解析，號稱全網最全，因為把整個包都加了注釋hhh。

但是那篇文章由於是以源代碼+注釋的形式呈現，存在諸多問題，比如：沒有按照邏輯順序解析（而閱讀代碼的正確步驟是按照程序執行順序一步一步看），沒有深挖里面的函數而更多的是對本框架的理解，也沒有把各個函數之間通用的變量講清楚（比如：move_base的狀態實際上是貫穿整個規划過程的、各個標志位在不同函數之間都是有變化的），等等。

本篇博文在此前那篇的基礎上，按照程序執行順序一步一步進行講解，並且在一定程度上進行了深挖（譬如，與其他包是怎么關聯的），也在程序講解時把頭文件中列出的類成員的含義進行了說明。

個人可能會有錯誤，歡迎朋友們批評指正，並私信我取得聯系，在此感謝。

話不多說，開始解析——

一、入口

入口為文件move_base_node.cpp，聲明了move_base::MoveBase對象move_base，傳入參數為tf2_ros::Buffer& tf 。聲明的時候，便進入了構造函數。但是在看構造函數前，我們先來看一下在move_base這個命名空間中都有哪些數據。

二、頭文件-命名空間

進入入口的命名空間，命名空間為move_base。接下來挨個介紹一下命名空間move_base的內容：

1、聲明server端，消息類型是move_base_msgs::MoveBaseAction

1 typedef actionlib::SimpleActionServer<move_base_msgs::MoveBaseAction> MoveBaseActionServer;

2、枚舉movebase狀態表示

1 enum MoveBaseState { 
2     PLANNING,
3     CONTROLLING,
4     CLEARING
5   };

3、枚舉，觸發恢復模式

1 enum RecoveryTrigger
2   {
3     PLANNING_R,
4     CONTROLLING_R,
5     OSCILLATION_R
6   };

4、MoveBase類，使用actionlib：：ActionServer接口，該接口將robot移動到目標位置

1 class MoveBase{ }  //下面詳細展開

三、頭文件-MoveBase類

在move_base命名空間中，最重要的就是MoveBase類了，在（一）中提到的入口，實際上就是構造函數。在這里，先羅列一下類中有哪些成員，然后在（四）中再詳細解釋各個函數成員與數據成員的關系與執行順序。

1、構造函數，傳入的參數是tf

MoveBase(tf2_ros::Buffer& tf);

2、析構函數

virtual ~MoveBase();

3、控制閉環、全局規划、到達目標返回true，沒有到達返回false

bool executeCycle(geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& global_plan);

4、清除costmap

bool clearCostmapsService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp);

5、當action不活躍時，調用此函數，返回plan

bool planService(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &resp);

6、新的全局規划，goal 規划的目標點，plan 規划

bool makePlan(const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan);

7、從參數服務器加載導航恢復行為

bool loadRecoveryBehaviors(ros::NodeHandle node);

8、加載默認導航恢復行為

void loadDefaultRecoveryBehaviors();

9、清除機器人局部規划框的障礙物，size_x 局部規划框的長x， size_y 局部規划框的寬y

void clearCostmapWindows(double size_x, double size_y);

10、發布速度為0的指令

void publishZeroVelocity();

11、重置move_base action的狀態，設置速度為0

void resetState();

12、周期性地喚醒規划器

void wakePlanner(const ros::TimerEvent& event);

13、其它函數

 1 void goalCB(const geometry_msgs::PoseStamped::ConstPtr& goal);
 2 
 3 void planThread();
 4 
 5 void executeCb(const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal);
 6 
 7 bool isQuaternionValid(const geometry_msgs::Quaternion& q);
 8 
 9 bool getRobotPose(geometry_msgs::PoseStamped& global_pose, costmap_2d::Costmap2DROS* costmap);
10 
11 double distance(const geometry_msgs::PoseStamped& p1, const geometry_msgs::PoseStamped& p2);
12 
13 geometry_msgs::PoseStamped goalToGlobalFrame(const geometry_msgs::PoseStamped& goal_pose_msg);

14、數據成員

 1 tf2_ros::Buffer& tf_;
 2 
 3 MoveBaseActionServer* as_; //就是actionlib的server端
 4 
 5 boost::shared_ptr<nav_core::BaseLocalPlanner> tc_;//局部規划器，加載並創建實例后的指針
 6 costmap_2d::Costmap2DROS* planner_costmap_ros_, *controller_costmap_ros_;//costmap的實例化指針
 7 
 8 boost::shared_ptr<nav_core::BaseGlobalPlanner> planner_;//全局規划器，加載並創建實例后的指針
 9 std::string robot_base_frame_, global_frame_;
10 
11 std::vector<boost::shared_ptr<nav_core::RecoveryBehavior> > recovery_behaviors_;//可能是出錯后的恢復
12 unsigned int recovery_index_;
13 
14 geometry_msgs::PoseStamped global_pose_;
15 double planner_frequency_, controller_frequency_, inscribed_radius_, circumscribed_radius_;
16 double planner_patience_, controller_patience_;
17 int32_t max_planning_retries_;
18 uint32_t planning_retries_;
19 double conservative_reset_dist_, clearing_radius_;
20 ros::Publisher current_goal_pub_, vel_pub_, action_goal_pub_;
21 ros::Subscriber goal_sub_;
22 ros::ServiceServer make_plan_srv_, clear_costmaps_srv_;
23 bool shutdown_costmaps_, clearing_rotation_allowed_, recovery_behavior_enabled_;
24 double oscillation_timeout_, oscillation_distance_;
25 
26 MoveBaseState state_;
27 RecoveryTrigger recovery_trigger_;
28 
29 ros::Time last_valid_plan_, last_valid_control_, last_oscillation_reset_;
30 geometry_msgs::PoseStamped oscillation_pose_;
31 pluginlib::ClassLoader<nav_core::BaseGlobalPlanner> bgp_loader_;
32 pluginlib::ClassLoader<nav_core::BaseLocalPlanner> blp_loader_;
33 pluginlib::ClassLoader<nav_core::RecoveryBehavior> recovery_loader_;
34 
35 //觸發哪種規划器
36 std::vector<geometry_msgs::PoseStamped>* planner_plan_;//保存最新規划的路徑，傳給latest_plan_
37 std::vector<geometry_msgs::PoseStamped>* latest_plan_;//在executeCycle中傳給controller_plan_
38 std::vector<geometry_msgs::PoseStamped>* controller_plan_;
39 
40 //規划器線程
41 bool runPlanner_;
42 boost::recursive_mutex planner_mutex_;
43 boost::condition_variable_any planner_cond_;
44 geometry_msgs::PoseStamped planner_goal_;
45 boost::thread* planner_thread_;
46 
47 
48 boost::recursive_mutex configuration_mutex_;
49 dynamic_reconfigure::Server<move_base::MoveBaseConfig> *dsrv_;
50 
51 void reconfigureCB(move_base::MoveBaseConfig &config, uint32_t level);
52 
53 move_base::MoveBaseConfig last_config_;
54 move_base::MoveBaseConfig default_config_;
55 bool setup_, p_freq_change_, c_freq_change_;
56 bool new_global_plan_;

四、正文來了

文件move_base.cpp定義了上面頭文件里寫的函數，接下來挨個捋一遍：

入口是構造函數MoveBase::MoveBase。

首先，初始化了一堆參數：

1     tf_(tf), //tf2_ros::Buffer& 引用？取址？
2     as_(NULL), //MoveBaseActionServer* 指針
3     planner_costmap_ros_(NULL), controller_costmap_ros_(NULL),//costmap的實例化指針
4     bgp_loader_("nav_core", "nav_core::BaseGlobalPlanner"), //nav_core::BaseGlobalPlanner類型的插件
5     blp_loader_("nav_core", "nav_core::BaseLocalPlanner"),//nav_core::BaseLocalPlanner類型的插件
6     recovery_loader_("nav_core", "nav_core::RecoveryBehavior"), //nav_core::RecoveryBehavior類型的插件
7     planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL),//三種規划器，看觸發哪種
8     runPlanner_(false), setup_(false), p_freq_change_(false), c_freq_change_(false), //配置參數
9     new_global_plan_(false)  //配置參數

創建move_base action,綁定回調函數。定義一個名為move_base的SimpleActionServer。該服務器的Callback為moveBase::executeCb

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

這個時候調用回調函數MoveBase::executeCb(const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal)，具體如下：

如果目標非法，則直接返回：

1     if(!isQuaternionValid(move_base_goal->target_pose.pose.orientation)){
2         as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on goal because it was sent with an invalid quaternion");
3         return;
4     }

其中，isQuaternionValid(const geometry_msgs::Quaternion& q)函數如下，主要用於檢查四元數的合法性：

 1 bool MoveBase::isQuaternionValid(const geometry_msgs::Quaternion& q){
 2     //1、首先檢查四元數是否元素完整
 3     if(!std::isfinite(q.x) || !std::isfinite(q.y) || !std::isfinite(q.z) || !std::isfinite(q.w)){
 4         ROS_ERROR("Quaternion has nans or infs... discarding as a navigation goal");
 5         return false;
 6     }
 7     tf2::Quaternion tf_q(q.x, q.y, q.z, q.w);
 8     //2、檢查四元數是否趨近於0
 9     if(tf_q.length2() < 1e-6){
10         ROS_ERROR("Quaternion has length close to zero... discarding as navigation goal");
11         return false;
12     }
13     //3、對四元數規范化，轉化為vector
14     tf_q.normalize();
15     tf2::Vector3 up(0, 0, 1);
16     double dot = up.dot(up.rotate(tf_q.getAxis(), tf_q.getAngle()));
17     if(fabs(dot - 1) > 1e-3){
18         ROS_ERROR("Quaternion is invalid... for navigation the z-axis of the quaternion must be close to vertical.");
19         return false;
20     }
21 
22     return true;
23 }

ok，回到回調函數繼續看——

將目標的坐標系統一轉換為全局坐標系：

1     geometry_msgs::PoseStamped goal = goalToGlobalFrame(move_base_goal->target_pose);

其中，函數MoveBase::goalToGlobalFrame(const geometry_msgs::PoseStamped& goal_pose_msg)如下：

 1 geometry_msgs::PoseStamped MoveBase::goalToGlobalFrame(const geometry_msgs::PoseStamped& goal_pose_msg){
 2     std::string global_frame = planner_costmap_ros_->getGlobalFrameID();
 3     geometry_msgs::PoseStamped goal_pose, global_pose;
 4     goal_pose = goal_pose_msg;
 5 
 6     //tf一下
 7     goal_pose.header.stamp = ros::Time();
 8 
 9     try{
10         tf_.transform(goal_pose_msg, global_pose, global_frame);
11     }
12     catch(tf2::TransformException& ex){
13         ROS_WARN("Failed to transform the goal pose from %s into the %s frame: %s",
14                  goal_pose.header.frame_id.c_str(), global_frame.c_str(), ex.what());
15         return goal_pose_msg;
16     }
17 
18     return global_pose;
19 }

ok，回到回調函數繼續看——

設置目標點並喚醒路徑規划線程：

1     boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
2     planner_goal_ = goal;
3     runPlanner_ = true;
4     planner_cond_.notify_one();
5     lock.unlock();

然后發布goal，設置控制頻率：

1     current_goal_pub_.publish(goal);
2     std::vector<geometry_msgs::PoseStamped> global_plan;
3     ros::Rate r(controller_frequency_);

開啟costmap更新：

1     if(shutdown_costmaps_){
2         ROS_DEBUG_NAMED("move_base","Starting up costmaps that were shut down previously");
3         planner_costmap_ros_->start();
4         controller_costmap_ros_->start();
5     }

重置時間標志位：

1     last_valid_control_ = ros::Time::now();
2     last_valid_plan_ = ros::Time::now();
3     last_oscillation_reset_ = ros::Time::now();
4     planning_retries_ = 0;

開啟循環，循環判斷是否有新的goal搶占（重要！！！）：

  1     while(n.ok())
  2     {
  3 
  4         //7. 修改循環頻率
  5         if(c_freq_change_)
  6         {
  7             ROS_INFO("Setting controller frequency to %.2f", controller_frequency_);
  8             r = ros::Rate(controller_frequency_);
  9             c_freq_change_ = false;
 10         }
 11 
 12         //8. 如果獲得一個搶占式目標
 13         if(as_->isPreemptRequested()){ //action的搶斷函數
 14             if(as_->isNewGoalAvailable()){
 15                 //如果有新的目標，會接受的，但不會關閉其他進程
 16                 move_base_msgs::MoveBaseGoal new_goal = *as_->acceptNewGoal();
 17 
 18                 //9.如果目標無效,則返回
 19                 if(!isQuaternionValid(new_goal.target_pose.pose.orientation)){
 20                     as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on goal because it was sent with an invalid quaternion");
 21                     return;
 22                 }
 23                 //10.將目標轉換為全局坐標系
 24                 goal = goalToGlobalFrame(new_goal.target_pose);
 25 
 26                 //we'll make sure that we reset our state for the next execution cycle
 27                 //11.設置狀態為PLANNING
 28                 recovery_index_ = 0;
 29                 state_ = PLANNING;
 30 
 31                 //we have a new goal so make sure the planner is awake
 32                 //12. 設置目標點並喚醒路徑規划線程
 33                 lock.lock();
 34                 planner_goal_ = goal;
 35                 runPlanner_ = true;
 36                 planner_cond_.notify_one();
 37                 lock.unlock();
 38 
 39                 //13. 把goal發布給可視化工具
 40                 ROS_DEBUG_NAMED("move_base","move_base has received a goal of x: %.2f, y: %.2f", goal.pose.position.x, goal.pose.position.y);
 41                 current_goal_pub_.publish(goal);
 42 
 43                 //make sure to reset our timeouts and counters
 44                 //14. 重置規划時間
 45                 last_valid_control_ = ros::Time::now();
 46                 last_valid_plan_ = ros::Time::now();
 47                 last_oscillation_reset_ = ros::Time::now();
 48                 planning_retries_ = 0;
 49             }
 50             else {
 51 
 52                 //14.重置狀態,設置為搶占式任務
 53                 //if we've been preempted explicitly we need to shut things down
 54                 resetState();
 55 
 56                 //通知ActionServer已成功搶占
 57                 ROS_DEBUG_NAMED("move_base","Move base preempting the current goal");
 58                 as_->setPreempted();
 59 
 60                 //we'll actually return from execute after preempting
 61                 return;
 62             }
 63         }
 64 
 65         //we also want to check if we've changed global frames because we need to transform our goal pose
 66         //15.如果目標點的坐標系和全局地圖的坐標系不相同
 67         if(goal.header.frame_id != planner_costmap_ros_->getGlobalFrameID()){
 68             //16,轉換目標點坐標系
 69             goal = goalToGlobalFrame(goal);
 70 
 71             //we want to go back to the planning state for the next execution cycle
 72             recovery_index_ = 0;
 73             state_ = PLANNING;
 74 
 75             //17. 設置目標點並喚醒路徑規划線程
 76             lock.lock();
 77             planner_goal_ = goal;
 78             runPlanner_ = true;
 79             planner_cond_.notify_one();
 80             lock.unlock();
 81 
 82             //publish the goal point to the visualizer
 83             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);
 84             current_goal_pub_.publish(goal);
 85 
 86             //18.重置規划器相關時間標志位
 87             last_valid_control_ = ros::Time::now();
 88             last_valid_plan_ = ros::Time::now();
 89             last_oscillation_reset_ = ros::Time::now();
 90             planning_retries_ = 0;
 91         }
 92 
 93         //for timing that gives real time even in simulation
 94         ros::WallTime start = ros::WallTime::now();
 95 
 96         //19. 到達目標點的真正工作，控制機器人進行跟隨
 97         bool done = executeCycle(goal, global_plan);
 98 
 99         //20.如果完成任務則返回
100         if(done)
101             return;
102 
103         //check if execution of the goal has completed in some way
104 
105         ros::WallDuration t_diff = ros::WallTime::now() - start;
106         ROS_DEBUG_NAMED("move_base","Full control cycle time: %.9f\n", t_diff.toSec());
107         //21. 執行休眠動作
108         r.sleep();
109         //make sure to sleep for the remainder of our cycle time
110         if(r.cycleTime() > ros::Duration(1 / controller_frequency_) && state_ == CONTROLLING)
111             ROS_WARN("Control loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds", controller_frequency_, r.cycleTime().toSec());
112     }

其中，done的值即為MoveBase::executeCycle（）函數的返回值，這個值非常重要，直接判斷了是否到達目標點；MoveBase::executeCycle（）函數是控制機器人進行跟隨的函數（重要！！！），如下：

獲取機器人當前位置：

1     geometry_msgs::PoseStamped global_pose;
2     getRobotPose(global_pose, planner_costmap_ros_);
3     const geometry_msgs::PoseStamped& current_position = global_pose;
4 
5     //push the feedback out
6     move_base_msgs::MoveBaseFeedback feedback;
7     feedback.base_position = current_position;
8     as_->publishFeedback(feedback);

其中，getRobotPose（）函數如下，需要對准坐標系和時間戳：

 1 bool MoveBase::getRobotPose(geometry_msgs::PoseStamped& global_pose, costmap_2d::Costmap2DROS* costmap)
 2 {
 3     tf2::toMsg(tf2::Transform::getIdentity(), global_pose.pose);
 4     geometry_msgs::PoseStamped robot_pose;
 5     tf2::toMsg(tf2::Transform::getIdentity(), robot_pose.pose);
 6     robot_pose.header.frame_id = robot_base_frame_;
 7     robot_pose.header.stamp = ros::Time(); // latest available
 8     ros::Time current_time = ros::Time::now();  // save time for checking tf delay later
 9 
10     // 轉換到統一的全局坐標
11     try
12     {
13         tf_.transform(robot_pose, global_pose, costmap->getGlobalFrameID());
14     }
15     catch (tf2::LookupException& ex)
16     {
17         ROS_ERROR_THROTTLE(1.0, "No Transform available Error looking up robot pose: %s\n", ex.what());
18         return false;
19     }
20     catch (tf2::ConnectivityException& ex)
21     {
22         ROS_ERROR_THROTTLE(1.0, "Connectivity Error looking up robot pose: %s\n", ex.what());
23         return false;
24     }
25     catch (tf2::ExtrapolationException& ex)
26     {
27         ROS_ERROR_THROTTLE(1.0, "Extrapolation Error looking up robot pose: %s\n", ex.what());
28         return false;
29     }
30 
31     // 全局坐標時間戳是否在costmap要求下
32     if (current_time.toSec() - global_pose.header.stamp.toSec() > costmap->getTransformTolerance())
33     {
34         ROS_WARN_THROTTLE(1.0, "Transform timeout for %s. " \
35                                "Current time: %.4f, pose stamp: %.4f, tolerance: %.4f", costmap->getName().c_str(),
36                           current_time.toSec(), global_pose.header.stamp.toSec(), costmap->getTransformTolerance());
37         return false;
38     }
39 
40     return true;
41 }

ok，返回MoveBase::executeCycle（）函數繼續看——

判斷當前位置和是否振盪，其中distance函數返回的是兩個位置的直線距離（歐氏距離），recovery_trigger_是枚舉RecoveryTrigger的對象：

1     if(distance(current_position, oscillation_pose_) >= oscillation_distance_)
2     {
3         last_oscillation_reset_ = ros::Time::now();
4         oscillation_pose_ = current_position;
5 
6         //如果上次的恢復是由振盪引起的，重置恢復指數
7         if(recovery_trigger_ == OSCILLATION_R)
8             recovery_index_ = 0;
9     }

地圖數據超時，即觀測傳感器數據不夠新，停止機器人，返回false，其中publishZeroVelocity（）函數把geometry_msgs::Twist類型的cmd_vel設置為0並發布出去:

1     if(!controller_costmap_ros_->isCurrent()){
2         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());
3         publishZeroVelocity();
4         return false;
5     }

如果獲取新的全局路徑,則將它傳輸給控制器，完成latest_plan_到controller_plan_的轉換（提示：頭文件那里講過規划轉換的規則）：

 1     if(new_global_plan_){
 2         //make sure to set the new plan flag to false
 3         new_global_plan_ = false;
 4 
 5         ROS_DEBUG_NAMED("move_base","Got a new plan...swap pointers");
 6 
 7         //do a pointer swap under mutex
 8         std::vector<geometry_msgs::PoseStamped>* temp_plan = controller_plan_;
 9 
10         boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
11         controller_plan_ = latest_plan_;
12         latest_plan_ = temp_plan;
13         lock.unlock();
14         ROS_DEBUG_NAMED("move_base","pointers swapped!");
15 
16         //5. 給控制器設置全局路徑
17         if(!tc_->setPlan(*controller_plan_)){
18             //ABORT and SHUTDOWN COSTMAPS
19             ROS_ERROR("Failed to pass global plan to the controller, aborting.");
20             resetState();
21 
22             //disable the planner thread
23             lock.lock();
24             runPlanner_ = false;
25             lock.unlock();
26             //6.設置動作中斷,返回true
27             as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to pass global plan to the controller.");
28             return true;
29         }
30 
31         //如果全局路徑有效，則不需要recovery
32         if(recovery_trigger_ == PLANNING_R)
33             recovery_index_ = 0;
34     }

其中，tc_是局部規划器的指針，setPlan是TrajectoryPlannerROS的函數（注意！！！跟外部包有關系了！！）

然后判斷move_base狀態，一般默認狀態或者接收到一個有效goal時是PLANNING，在規划出全局路徑后state_會由PLANNING變為CONTROLLING，如果規划失敗則由PLANNING變為CLEARING，架構如下：

1 switch(state_){
2     case PLANNING:
3     case CONTROLLING:
4     case CLEARING:
5     default:
6 }

其中，詳細介紹一下各個狀態：

（1）機器人規划狀態，嘗試獲取一條全局路徑：

1     case PLANNING:
2     {
3         boost::recursive_mutex::scoped_lock lock(planner_mutex_);
4         runPlanner_ = true;
5         planner_cond_.notify_one();//還在PLANNING中則喚醒規划線程讓它干活
6     }
7         ROS_DEBUG_NAMED("move_base","Waiting for plan, in the planning state.");
8         break;

（2）機器人控制狀態，嘗試獲取一個有效的速度命令：

1     case CONTROLLING:

如果到達目標點，重置狀態，設置動作成功，返回true，其中isGoalReached()函數是TrajectoryPlannerROS的函數（注意！！！這里跟外部包有關！！）：

 1         if(tc_->isGoalReached()){
 2             ROS_DEBUG_NAMED("move_base","Goal reached!");
 3             resetState();
 4 
 5             //disable the planner thread
 6             boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
 7             runPlanner_ = false;
 8             lock.unlock();
 9 
10             as_->setSucceeded(move_base_msgs::MoveBaseResult(), "Goal reached.");
11             return true;
12         }

其中，resetState（）函數如下，配合上面的看，機器人到達目標點后把move_base狀態設置為PLANNING：

 1 void MoveBase::resetState(){
 2     // Disable the planner thread
 3     boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
 4     runPlanner_ = false;
 5     lock.unlock();
 6 
 7     // Reset statemachine
 8     state_ = PLANNING;
 9     recovery_index_ = 0;
10     recovery_trigger_ = PLANNING_R;
11     publishZeroVelocity();
12 
13     //if we shutdown our costmaps when we're deactivated... we'll do that now
14     if(shutdown_costmaps_){
15         ROS_DEBUG_NAMED("move_base","Stopping costmaps");
16         planner_costmap_ros_->stop();
17         controller_costmap_ros_->stop();
18     }
19 }

返回去繼續看，如果超過震盪時間，停止機器人，設置清障標志位：

1         if(oscillation_timeout_ > 0.0 &&
2                 last_oscillation_reset_ + ros::Duration(oscillation_timeout_) < ros::Time::now())
3         {
4             publishZeroVelocity();
5             state_ = CLEARING;
6             recovery_trigger_ = OSCILLATION_R;
7         }

獲取有效速度,如果獲取成功，直接發布到cmd_vel：

1         if(tc_->computeVelocityCommands(cmd_vel)){
2             ROS_DEBUG_NAMED( "move_base", "Got a valid command from the local planner: %.3lf, %.3lf, %.3lf",
3                              cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z );
4             last_valid_control_ = ros::Time::now();
5             //make sure that we send the velocity command to the base
6             vel_pub_.publish(cmd_vel);
7             if(recovery_trigger_ == CONTROLLING_R)
8                 recovery_index_ = 0;
9         }

其中，computeVelocityCommands函數又是TrajectoryPlannerROS的函數（注意！！！這里跟外部包有關！！）。

如果沒有獲取有效速度，則判斷是否超過嘗試時間，如果超時，則停止機器人，進入清障模式：

1             if(ros::Time::now() > attempt_end){
2                 //we'll move into our obstacle clearing mode
3                 publishZeroVelocity();
4                 state_ = CLEARING;
5                 recovery_trigger_ = CONTROLLING_R;
6             }

如果沒有超時，則再全局規划一個新的路徑：

 1             last_valid_plan_ = ros::Time::now();
 2             planning_retries_ = 0;
 3             state_ = PLANNING;
 4             publishZeroVelocity();
 5 
 6             //enable the planner thread in case it isn't running on a clock
 7             boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
 8             runPlanner_ = true;
 9             planner_cond_.notify_one();
10             lock.unlock();

（3）機器人清障狀態，規划或者控制失敗，恢復或者進入到清障模式：

1 case CLEARING:

如果有可用恢復器，執行恢復動作，並設置狀態為PLANNING：

 1     if(recovery_behavior_enabled_ && recovery_index_ < recovery_behaviors_.size()){
 2         ROS_DEBUG_NAMED("move_base_recovery","Executing behavior %u of %zu", recovery_index_, recovery_behaviors_.size());
 3         recovery_behaviors_[recovery_index_]->runBehavior();
 4 
 5         //we at least want to give the robot some time to stop oscillating after executing the behavior
 6         last_oscillation_reset_ = ros::Time::now();
 7 
 8         //we'll check if the recovery behavior actually worked
 9         ROS_DEBUG_NAMED("move_base_recovery","Going back to planning state");
10         last_valid_plan_ = ros::Time::now();
11         planning_retries_ = 0;
12         state_ = PLANNING;
13 
14         //update the index of the next recovery behavior that we'll try
15         recovery_index_++;
16     }

其中，recovery_behaviors_類型為std::vector<boost::shared_ptr<nav_core::RecoveryBehavior> >，runBehavior（）函數為move_slow_and_clear包里面的函數。（注意！！！鏈接到外部包！！！）。

如果沒有可用恢復器，結束動作，返回true：

 1     ROS_DEBUG_NAMED("move_base_recovery","All recovery behaviors have failed, locking the planner and disabling it.");
 2     //disable the planner thread
 3     boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
 4     runPlanner_ = false;
 5     lock.unlock();
 6 
 7     ROS_DEBUG_NAMED("move_base_recovery","Something should abort after this.");
 8 
 9     if(recovery_trigger_ == CONTROLLING_R){
10         ROS_ERROR("Aborting because a valid control could not be found. Even after executing all recovery behaviors");
11         as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to find a valid control. Even after executing recovery behaviors.");
12     }
13     else if(recovery_trigger_ == PLANNING_R){
14         ROS_ERROR("Aborting because a valid plan could not be found. Even after executing all recovery behaviors");
15         as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to find a valid plan. Even after executing recovery behaviors.");
16     }
17     else if(recovery_trigger_ == OSCILLATION_R){
18         ROS_ERROR("Aborting because the robot appears to be oscillating over and over. Even after executing all recovery behaviors");
19         as_->setAborted(move_base_msgs::MoveBaseResult(), "Robot is oscillating. Even after executing recovery behaviors.");
20     }
21     resetState();
22     return true;

（4）除了上述狀態以外：

1     default:
2         ROS_ERROR("This case should never be reached, something is wrong, aborting");
3         resetState();
4         //disable the planner thread
5         boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
6         runPlanner_ = false;
7         lock.unlock();
8         as_->setAborted(move_base_msgs::MoveBaseResult(), "Reached a case that should not be hit in move_base. This is a bug, please report it.");
9         return true;

ok，MoveBase::executeCycle（）函數終於介紹完了，回到回調函數繼續看——

剩下的就是解放線程，反饋給action結果：

1     //22. 喚醒計划線程，以便它可以干凈地退出
2     lock.lock();
3     runPlanner_ = true;
4     planner_cond_.notify_one();
5     lock.unlock();
6 
7     //23. 如果節點結束就終止並返回
8     as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on the goal because the node has been killed");
9     return;

ok，MoveBase::executeCb（）函數看完了，回到構造函數MoveBase::MoveBase——

觸發模式（三種模式：規划、控制、振盪）設置為“規划中”：

1     recovery_trigger_ = PLANNING_R;

加載參數，從參數服務器獲取一些參數，包括兩個規划器名稱、代價地圖坐標系、規划頻率、控制周期等

 1     std::string global_planner, local_planner;
 2     private_nh.param("base_global_planner", global_planner, std::string("navfn/NavfnROS"));
 3     private_nh.param("base_local_planner", local_planner, std::string("base_local_planner/TrajectoryPlannerROS"));
 4     private_nh.param("global_costmap/robot_base_frame", robot_base_frame_, std::string("base_link"));
 5     private_nh.param("global_costmap/global_frame", global_frame_, std::string("map"));
 6     private_nh.param("planner_frequency", planner_frequency_, 0.0);
 7     private_nh.param("controller_frequency", controller_frequency_, 20.0);
 8     private_nh.param("planner_patience", planner_patience_, 5.0);
 9     private_nh.param("controller_patience", controller_patience_, 15.0);
10     private_nh.param("max_planning_retries", max_planning_retries_, -1);  // disabled by default
11     private_nh.param("oscillation_timeout", oscillation_timeout_, 0.0);
12     private_nh.param("oscillation_distance", oscillation_distance_, 0.5);

為三種規划器（planner_plan_保存最新規划的路徑，傳遞給latest_plan_，然后latest_plan_通過executeCycle中傳給controller_plan_）設置內存緩沖區：

1     planner_plan_ = new std::vector<geometry_msgs::PoseStamped>();
2     latest_plan_ = new std::vector<geometry_msgs::PoseStamped>();
3     controller_plan_ = new std::vector<geometry_msgs::PoseStamped>();

設置規划器線程，planner_thread_是boost::thread*類型的指針：

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

其中，MoveBase::planThread()函數是planner線程的入口。這個函數需要等待actionlib服務器的cbMoveBase::executeCb來喚醒啟動。主要作用是調用全局路徑規划獲取路徑，同時保證規划的周期性以及規划超時清除goal，如下：

 1 void MoveBase::planThread(){
 2     ROS_DEBUG_NAMED("move_base_plan_thread","Starting planner thread...");
 3     ros::NodeHandle n;
 4     ros::Timer timer;
 5     bool wait_for_wake = false;
 6     //1. 創建遞歸鎖
 7     boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
 8     while(n.ok()){
 9         //check if we should run the planner (the mutex is locked)
10         //2.判斷是否阻塞線程
11         while(wait_for_wake || !runPlanner_){
12             //if we should not be running the planner then suspend this thread
13             ROS_DEBUG_NAMED("move_base_plan_thread","Planner thread is suspending");
14             //當 std::condition_variable 對象的某個 wait 函數被調用的時候，
15             //它使用 std::unique_lock(通過 std::mutex) 來鎖住當前線程。
16             //當前線程會一直被阻塞，直到另外一個線程在相同的 std::condition_variable 對象上調用了 notification 函數來喚醒當前線程。
17             planner_cond_.wait(lock);
18             wait_for_wake = false;
19         }
20         ros::Time start_time = ros::Time::now();
21 
22         //time to plan! get a copy of the goal and unlock the mutex
23         geometry_msgs::PoseStamped temp_goal = planner_goal_;
24         lock.unlock();
25         ROS_DEBUG_NAMED("move_base_plan_thread","Planning...");
26 
27         //run planner
28 
29         //3. 獲取規划的全局路徑
30         //這里的makePlan作用是獲取機器人的位姿作為起點，然后調用全局規划器的makePlan返回規划路徑，存儲在planner_plan_
31         planner_plan_->clear(); 
32         bool gotPlan = n.ok() && makePlan(temp_goal, *planner_plan_);
33 
34 
35         //4.如果獲得了plan,則將其賦值給latest_plan_
36         if(gotPlan){
37             ROS_DEBUG_NAMED("move_base_plan_thread","Got Plan with %zu points!", planner_plan_->size());
38             //pointer swap the plans under mutex (the controller will pull from latest_plan_)
39             std::vector<geometry_msgs::PoseStamped>* temp_plan = planner_plan_;
40 
41             lock.lock();
42             planner_plan_ = latest_plan_;
43             latest_plan_ = temp_plan;
44             last_valid_plan_ = ros::Time::now();
45             planning_retries_ = 0;
46             new_global_plan_ = true;
47 
48             ROS_DEBUG_NAMED("move_base_plan_thread","Generated a plan from the base_global_planner");
49 
50             //make sure we only start the controller if we still haven't reached the goal
51             if(runPlanner_)
52                 state_ = CONTROLLING;
53             if(planner_frequency_ <= 0)
54                 runPlanner_ = false;
55             lock.unlock();
56         }
57 
58         //5. 達到一定條件后停止路徑規划,進入清障模式
59         //如果沒有規划出路徑，並且處於PLANNING狀態，則判斷是否超過最大規划周期或者規划次數
60         //如果是則進入自轉模式，否則應該會等待MoveBase::executeCycle的喚醒再次規划
61         else if(state_==PLANNING){ 
62             //僅在MoveBase::executeCb及其調用的MoveBase::executeCycle或者重置狀態時會被設置為PLANNING
63             //一般是剛獲得新目標，或者得到路徑但計算不出下一步控制時重新進行路徑規划
64             ROS_DEBUG_NAMED("move_base_plan_thread","No Plan...");
65             ros::Time attempt_end = last_valid_plan_ + ros::Duration(planner_patience_);
66 
67             //check if we've tried to make a plan for over our time limit or our maximum number of retries
68             //issue #496: we stop planning when one of the conditions is true, but if max_planning_retries_
69             //is negative (the default), it is just ignored and we have the same behavior as ever
70             lock.lock();
71             planning_retries_++;
72             if(runPlanner_ &&
73                     (ros::Time::now() > attempt_end || planning_retries_ > uint32_t(max_planning_retries_))){
74                 //we'll move into our obstacle clearing mode
75                 state_ = CLEARING;
76                 runPlanner_ = false;  // proper solution for issue #523
77                 publishZeroVelocity();
78                 recovery_trigger_ = PLANNING_R;
79             }
80 
81             lock.unlock();
82         }
83 
84         //take the mutex for the next iteration
85         lock.lock();
86 
87 
88         //6.設置睡眠模式
89         //如果還沒到規划周期則定時器睡眠，在定時器中斷中通過planner_cond_喚醒，這里規划周期為0
90         if(planner_frequency_ > 0){
91             ros::Duration sleep_time = (start_time + ros::Duration(1.0/planner_frequency_)) - ros::Time::now();
92             if (sleep_time > ros::Duration(0.0)){
93                 wait_for_wake = true;
94                 timer = n.createTimer(sleep_time, &MoveBase::wakePlanner, this);
95             }
96         }
97     }
98 }

其中，planner_cond_專門用於開啟路徑規划的線程，在其他地方也經常遇到；這一步中，實現了從latest_plan_到planner_plan_ 的跨越；MoveBase::wakePlanner（）函數中用planner_cond_開啟了路徑規划的線程。

ok，MoveBase::planThread()看完了，回到構造函數MoveBase::MoveBase繼續看——

創建發布者，話題名一個是cmd_vel，一個是cunrrent_goal，一個是goal：

1     vel_pub_ = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
2     current_goal_pub_ = private_nh.advertise<geometry_msgs::PoseStamped>("current_goal", 0 );
3 
4     ros::NodeHandle action_nh("move_base");
5     action_goal_pub_ = action_nh.advertise<move_base_msgs::MoveBaseActionGoal>("goal", 1);

提供消息類型為geometry_msgs::PoseStamped的發送goals的接口，比如cb為MoveBase::goalCB，在rviz中輸入的目標點就是通過這個函數來響應的：

1     ros::NodeHandle simple_nh("move_base_simple");
2     goal_sub_ = simple_nh.subscribe<geometry_msgs::PoseStamped>("goal", 1, boost::bind(&MoveBase::goalCB, this, _1));

其中，我們來看MoveBase::goalCB（）函數，傳入的是goal，主要作用是為rviz等提供調用借口，將geometry_msgs::PoseStamped形式的goal轉換成move_base_msgs::MoveBaseActionGoal，再發布到對應類型的goal話題中：

1 void MoveBase::goalCB(const geometry_msgs::PoseStamped::ConstPtr& goal){
2     ROS_DEBUG_NAMED("move_base","In ROS goal callback, wrapping the PoseStamped in the action message and re-sending to the server.");
3     move_base_msgs::MoveBaseActionGoal action_goal;
4     action_goal.header.stamp = ros::Time::now();
5     action_goal.goal.target_pose = *goal;
6 
7     action_goal_pub_.publish(action_goal);
8 }

ok，MoveBase::goalCB（）函數看完了，回到構造函數MoveBase::MoveBase繼續看——

設置costmap參數，技巧是把膨脹層設置為大於機器人的半徑：

1     private_nh.param("local_costmap/inscribed_radius", inscribed_radius_, 0.325);
2     private_nh.param("local_costmap/circumscribed_radius", circumscribed_radius_, 0.46);
3     private_nh.param("clearing_radius", clearing_radius_, circumscribed_radius_);
4     private_nh.param("conservative_reset_dist", conservative_reset_dist_, 3.0);
5 
6     private_nh.param("shutdown_costmaps", shutdown_costmaps_, false);
7     private_nh.param("clearing_rotation_allowed", clearing_rotation_allowed_, true);
8     private_nh.param("recovery_behavior_enabled", recovery_behavior_enabled_, true);

設置全局路徑規划器，planner_costmap_ros_是costmap_2d::Costmap2DROS*類型的實例化指針，planner_是boost::shared_ptr<nav_core::BaseGlobalPlanner>類型：

1     planner_costmap_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tf_);
2     planner_costmap_ros_->pause();
3     try {
4         planner_ = bgp_loader_.createInstance(global_planner);
5         planner_->initialize(bgp_loader_.getName(global_planner), planner_costmap_ros_);
6     } catch (const pluginlib::PluginlibException& ex) {
7         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());
8         exit(1);
9     }

設置局部路徑規划器：

 1     controller_costmap_ros_ = new costmap_2d::Costmap2DROS("local_costmap", tf_);
 2     controller_costmap_ros_->pause();
 3     try {
 4         tc_ = blp_loader_.createInstance(local_planner);
 5         ROS_INFO("Created local_planner %s", local_planner.c_str());
 6         tc_->initialize(blp_loader_.getName(local_planner), &tf_, controller_costmap_ros_);
 7     } catch (const pluginlib::PluginlibException& ex) {
 8         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());
 9         exit(1);
10     }

這個時候，tc_就成了局部規划器的實例對象。

開始更新costmap：

1  planner_costmap_ros_->start();
2  controller_costmap_ros_->start();

全局規划：

1  make_plan_srv_ = private_nh.advertiseService("make_plan", &MoveBase::planService, this);

其中，MoveBase::planService（）函數寫了全局規划的策略，以多少距離向外搜索路徑：

1 bool MoveBase::planService(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &resp)

獲取起始點，如果沒有起始點，那就獲取當前的全局位置為起始點：

 1     geometry_msgs::PoseStamped start;
 2     //如果起始點為空，設置global_pose為起始點
 3     if(req.start.header.frame_id.empty())
 4     {
 5         geometry_msgs::PoseStamped global_pose;
 6         if(!getRobotPose(global_pose, planner_costmap_ros_)){
 7             ROS_ERROR("move_base cannot make a plan for you because it could not get the start pose of the robot");
 8             return false;
 9         }
10         start = global_pose;
11     }
12     else
13     {
14         start = req.start;
15     }

制定規划策略：

 1     std::vector<geometry_msgs::PoseStamped> global_plan;
 2     if(!planner_->makePlan(start, req.goal, global_plan) || global_plan.empty()){
 3         ROS_DEBUG_NAMED("move_base","Failed to find a plan to exact goal of (%.2f, %.2f), searching for a feasible goal within tolerance",
 4                         req.goal.pose.position.x, req.goal.pose.position.y);
 5 
 6         //在規定的公差范圍內向外尋找可行的goal
 7         geometry_msgs::PoseStamped p;
 8         p = req.goal;
 9         bool found_legal = false;
10         float resolution = planner_costmap_ros_->getCostmap()->getResolution();
11         float search_increment = resolution*3.0;//以3倍分辨率的增量向外尋找
12         if(req.tolerance > 0.0 && req.tolerance < search_increment) search_increment = req.tolerance;
13         for(float max_offset = search_increment; max_offset <= req.tolerance && !found_legal; max_offset += search_increment) {
14             for(float y_offset = 0; y_offset <= max_offset && !found_legal; y_offset += search_increment) {
15                 for(float x_offset = 0; x_offset <= max_offset && !found_legal; x_offset += search_increment) {
16 
17                     //不在本位置的外側layer查找，太近的不找
18                     if(x_offset < max_offset-1e-9 && y_offset < max_offset-1e-9) continue;
19 
20                     //從兩個方向x、y查找精確的goal
21                     for(float y_mult = -1.0; y_mult <= 1.0 + 1e-9 && !found_legal; y_mult += 2.0) {
22 
23                         //第一次遍歷如果偏移量過小,則去除這個點或者上一點
24                         if(y_offset < 1e-9 && y_mult < -1.0 + 1e-9) continue;
25 
26                         for(float x_mult = -1.0; x_mult <= 1.0 + 1e-9 && !found_legal; x_mult += 2.0) {
27                             if(x_offset < 1e-9 && x_mult < -1.0 + 1e-9) continue;
28 
29                             p.pose.position.y = req.goal.pose.position.y + y_offset * y_mult;
30                             p.pose.position.x = req.goal.pose.position.x + x_offset * x_mult;
31 
32                             if(planner_->makePlan(start, p, global_plan)){
33                                 if(!global_plan.empty()){
34 
35                                     //adding the (unreachable) original goal to the end of the global plan, in case the local planner can get you there
36                                     //(the reachable goal should have been added by the global planner)
37                                     global_plan.push_back(req.goal);
38 
39                                     found_legal = true;
40                                     ROS_DEBUG_NAMED("move_base", "Found a plan to point (%.2f, %.2f)", p.pose.position.x, p.pose.position.y);
41                                     break;
42                                 }
43                             }
44                             else{
45                                 ROS_DEBUG_NAMED("move_base","Failed to find a plan to point (%.2f, %.2f)", p.pose.position.x, p.pose.position.y);
46                             }
47                         }
48                     }
49                 }
50             }
51         }
52     }

然后把規划后的global_plan附給resp，並且傳出去：

1     resp.plan.poses.resize(global_plan.size());
2     for(unsigned int i = 0; i < global_plan.size(); ++i){
3         resp.plan.poses[i] = global_plan[i];
4     }

ok，MoveBase::planService（）函數看完了，回到構造函數MoveBase::MoveBase繼續看——

開始清除地圖服務：

1     clear_costmaps_srv_ = private_nh.advertiseService("clear_costmaps", &MoveBase::clearCostmapsService, this);

其中，調用了MoveBase::clearCostmapsService（）函數，提供清除一次costmap的功能：

1 bool MoveBase::clearCostmapsService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp){
2     //clear the costmaps
3     boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock_controller(*(controller_costmap_ros_->getCostmap()->getMutex()));
4     controller_costmap_ros_->resetLayers();
5 
6     boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock_planner(*(planner_costmap_ros_->getCostmap()->getMutex()));
7     planner_costmap_ros_->resetLayers();
8     return true;
9 }

值得注意的是，其中有一個函數resetLayers()，調用的是costmap包（注意！！外部鏈接！！！），該函數的功能是重置地圖，內部包括重置總地圖、重置地圖各層：

 1 void Costmap2DROS::resetLayers()
 2 {
 3   Costmap2D* top = layered_costmap_->getCostmap();
 4   top->resetMap(0, 0, top->getSizeInCellsX(), top->getSizeInCellsY());
 5   std::vector < boost::shared_ptr<Layer> > *plugins = layered_costmap_->getPlugins();
 6   for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins->begin(); plugin != plugins->end();
 7       ++plugin)
 8   {
 9     (*plugin)->reset();
10   }
11 }

在該函數中，首先將總的地圖信息重置為缺省值。然后調用各層的reset函數對各層進行重置（第9行）。針對不同的reset函數，功能如下：

對於靜態層，在reset函數中，會調用onInitialize函數重新進行初始化，但基本不進行特別的操作，只是將地圖更新標志位設為true，從而引起邊界的更新（最大地圖邊界），從而導致后面更新地圖時更新整個地圖：

 1 void StaticLayer::reset()
 2 {
 3   if (first_map_only_)
 4   {
 5     has_updated_data_ = true;
 6   }
 7   else
 8   {
 9     onInitialize();
10   }
11 }

對於障礙物層，在reset函數中，會先取消訂閱傳感器話題，然后復位地圖，然后在重新訂閱傳感器話題，從而保證整個層從新開始：

1 void ObstacleLayer::reset()
2 {
3     deactivate();
4     resetMaps();
5     current_ = true;
6     activate();
7 }

ok，MoveBase::clearCostmapsService（）函數看完了，回到構造函數MoveBase::MoveBase繼續看——

如果不小心關閉了costmap，則停用：

1     if(shutdown_costmaps_){
2         ROS_DEBUG_NAMED("move_base","Stopping costmaps initially");
3         planner_costmap_ros_->stop();
4         controller_costmap_ros_->stop();
5     }

加載指定的恢復器，加載不出來則使用默認的，這里包括了找不到路自轉360：

1     if(!loadRecoveryBehaviors(private_nh)){
2         loadDefaultRecoveryBehaviors();
3     }

導航過程基本結束，把狀態初始化：

1     //initially, we'll need to make a plan
2     state_ = PLANNING;
3     //we'll start executing recovery behaviors at the beginning of our list
4     recovery_index_ = 0;
5     //10.開啟move_base動作器
6     as_->start();

啟動動態參數服務器：

1     dsrv_ = new dynamic_reconfigure::Server<move_base::MoveBaseConfig>(ros::NodeHandle("~"));
2     dynamic_reconfigure::Server<move_base::MoveBaseConfig>::CallbackType cb = boost::bind(&MoveBase::reconfigureCB, this, _1, _2);
3     dsrv_->setCallback(cb);

其中，回調函數MoveBase::reconfigureCB（），配置了各種參數，感興趣的可以看下，代碼如下：

 1 void MoveBase::reconfigureCB(move_base::MoveBaseConfig &config, uint32_t level){
 2     boost::recursive_mutex::scoped_lock l(configuration_mutex_);
 3 
 4     //一旦被調用，我們要確保有原始配置
 5     if(!setup_)
 6     {
 7         last_config_ = config;
 8         default_config_ = config;
 9         setup_ = true;
10         return;
11     }
12 
13     if(config.restore_defaults) {
14         config = default_config_;
15         //如果有人在參數服務器上設置默認值，要防止循環
16         config.restore_defaults = false;
17     }
18 
19     if(planner_frequency_ != config.planner_frequency)
20     {
21         planner_frequency_ = config.planner_frequency;
22         p_freq_change_ = true;
23     }
24 
25     if(controller_frequency_ != config.controller_frequency)
26     {
27         controller_frequency_ = config.controller_frequency;
28         c_freq_change_ = true;
29     }
30 
31     planner_patience_ = config.planner_patience;
32     controller_patience_ = config.controller_patience;
33     max_planning_retries_ = config.max_planning_retries;
34     conservative_reset_dist_ = config.conservative_reset_dist;
35 
36     recovery_behavior_enabled_ = config.recovery_behavior_enabled;
37     clearing_rotation_allowed_ = config.clearing_rotation_allowed;
38     shutdown_costmaps_ = config.shutdown_costmaps;
39 
40     oscillation_timeout_ = config.oscillation_timeout;
41     oscillation_distance_ = config.oscillation_distance;
42     if(config.base_global_planner != last_config_.base_global_planner) {
43         boost::shared_ptr<nav_core::BaseGlobalPlanner> old_planner = planner_;
44         //創建全局規划
45         ROS_INFO("Loading global planner %s", config.base_global_planner.c_str());
46         try {
47             planner_ = bgp_loader_.createInstance(config.base_global_planner);
48 
49             // 等待當前規划結束
50             boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
51 
52             // 在新規划開始前clear舊的
53             planner_plan_->clear();
54             latest_plan_->clear();
55             controller_plan_->clear();
56             resetState();
57             planner_->initialize(bgp_loader_.getName(config.base_global_planner), planner_costmap_ros_);
58 
59             lock.unlock();
60         } catch (const pluginlib::PluginlibException& ex) {
61             ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the \
62                       containing library is built? Exception: %s", config.base_global_planner.c_str(), ex.what());
63                                                    planner_ = old_planner;
64                     config.base_global_planner = last_config_.base_global_planner;
65         }
66     }
67 
68     if(config.base_local_planner != last_config_.base_local_planner){
69         boost::shared_ptr<nav_core::BaseLocalPlanner> old_planner = tc_;
70         //創建局部規划
71         try {
72             tc_ = blp_loader_.createInstance(config.base_local_planner);
73             // 清理舊的
74             planner_plan_->clear();
75             latest_plan_->clear();
76             controller_plan_->clear();
77             resetState();
78             tc_->initialize(blp_loader_.getName(config.base_local_planner), &tf_, controller_costmap_ros_);
79         } catch (const pluginlib::PluginlibException& ex) {
80             ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the \
81                       containing library is built? Exception: %s", config.base_local_planner.c_str(), ex.what());
82                                                    tc_ = old_planner;
83                     config.base_local_planner = last_config_.base_local_planner;
84         }
85     }
86 
87     last_config_ = config;
88 }

ok，構造函數MoveBase::MoveBase看完了，接着到析構函數，釋放了內存——

 1 MoveBase::~MoveBase(){
 2     recovery_behaviors_.clear();
 3 
 4     delete dsrv_;
 5 
 6     if(as_ != NULL)
 7         delete as_;
 8 
 9     if(planner_costmap_ros_ != NULL)
10         delete planner_costmap_ros_;
11 
12     if(controller_costmap_ros_ != NULL)
13         delete controller_costmap_ros_;
14 
15     planner_thread_->interrupt();
16     planner_thread_->join();
17 
18     delete planner_thread_;
19 
20     delete planner_plan_;
21     delete latest_plan_;
22     delete controller_plan_;
23 
24     planner_.reset();
25     tc_.reset();
26 }

至此，move_base全部解析完畢，恭喜你有耐心看完，如果有錯誤的地方歡迎指出，在此感謝！

五、需要注意的地方

1、move_base與costmap_2d、全局規划器、局部規划器、恢復器緊密相連，很多地方都是調用了這幾個包的函數。

2、需要着重關注頭文件中寫的那些枚舉常量，以及各個標志位，很多都是正文中的判斷條件。

3、多線程、智能指針有必要深入學一下，對內存和任務管理很有幫助。

免責聲明！

本站轉載的文章為個人學習借鑒使用，本站對版權不負任何法律責任。如果侵犯了您的隱私權益，請聯系本站郵箱yoyou2525@163.com刪除。

猜您在找 move_base 利用move_base導航--42 使用move_base導航 ---13 ROS 導航 move_base costmap planner 的調試說明 move_base的局部路徑規划代碼研究 move_base Warning: Invalid argument "/map" passed to canTransform argument target_frame的解決方法 ros move_base costmap 理解和實現動態窗口法避障 iOS面試必看，最全梳理 iOS面試題最全梳理新經資訊項目業務邏輯梳理