零、前言
此前写了一篇对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、多线程、智能指针有必要深入学一下，对内存和任务管理很有帮助。