Navigation（五） Move_base最最全解析（按执行逻辑梳理）

零、前言

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

posted @ 2020-06-04 12:06 爱喝果珍的猫阅读(11771) 评论(3) 编辑收藏举报

刷新页面返回顶部

爱喝果珍的猫

Navigation（五） Move_base最最全解析（按执行逻辑梳理）

公告