Navigation Move_base最最全解析（按执行逻辑梳理）
Posted juicecat
tags:
篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了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 ", 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 ", 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 ", 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 ", 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、多线程、智能指针有必要深入学一下，对内存和任务管理很有帮助。
以上是关于Navigation Move_base最最全解析（按执行逻辑梳理）的主要内容，如果未能解决你的问题，请参考以下文章
ros/move_base终止正在进行的导航navigation
[ROS] Chinese MOOC || Chapter-10.1 Navigation Stack 10.2 move_base and plug-ins