---

---

   本文主要介绍ROS导航包Navigation中的 Movebase节点中的路径规划的相关流程，并对其进行梳理概括，同时本文也是《ROS局部路径规划器插件teb_local_planner规划流程概括总结》部分的前述文章。

---

---

   1、接收到目标点信息goal

   在接收到目标点goal之后，Move_base节点会调用回调函数goalCB，开始工作

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

   2、goalCB函数

   goalCB函数仅仅将传入的 geometry_msgs:: PoseStamped 形式的goal转换成move_base_msgs:: MoveBaseActionGoal形式，再发布到对应类型的goal话题中，真正的执行函数是在构造函数里，actionlib server注册的executeCb函数。

 void MoveBase::goalCB(const geometry_msgs::PoseStamped::ConstPtr& goal)

   3、executeCb函数

   executeCb函数是move_base中路径规划的起始函数。将上述目标点传入到executeCb函数。

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

void MoveBase::executeCb(const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal)

   （1）在executeCb函数中，首先需要判断传入目标点的四元数的合法性，包括四元数是否完整，是否趋于0，以及进行规范化等，

   （2）然后将该坐标转换到世界坐标系下，同时通过publisher发布该坐标用于可视化显示。

   （3）随后唤醒规划线程，通过参数controller_frequency设置控制频率，开启代价地图costmap更新，并重置时间标志位

   注：在唤醒规划线程的程序中，执行了planThread函数的线程启动，该步骤会调用planThread函数。该过程不是通过函数调用的形式实现的，而是通过线程开关的形式实现的，在planThread函数中调用了全局规划器插件进行了全局规划，planThread函数将在本文第4部分进行介绍

lock(planner_mutex_)  

boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);//给该线程上锁
planner_goal_ = goal;//传入目标并开启线程
runPlanner_ = true;
planner_cond_.notify_one();
lock.unlock();//打开互斥锁

   （4）完成以上步骤后，程序进人while循环，按照上述设定的频率controller_ frequency进行循环，在循环中会进行如下操作：

   ①：根据标志位判断是否更改循环的控制频率

if(c_freq_change_)

r = ros::Rate(controller_frequency_)

   ②：根据标志位isPreemptRequested判断是否有新的目标点，若有新的目标点，则需要重新执行上述的（1）~（3）中描述的过程。若无新的目标点，则重置当前状态，允许新的目标点进行抢占。

if(as_->isPreemptRequested())

   ③：判断目标点坐标系是否与全局坐标系相同，若不相同则需要进行转换

if(goal.header.frame_id != planner_costmap_ros_->getGlobalFrameID())

   ④：调用executeCycle函数进行局部路径规划

   注：在executeCycle函数中调用了局部规划器插件进行了局部规划，executeCycle函数将在本文第5部分进行介绍

bool done = executeCycle(goal, global_plan);

   ⑤：判断是否结束循环，若executeCycle的返回值为真，则结束循环

if(done)   return;

   ⑥：计算本次循环已经花费的时间，用设定的控制频率（循环频率）对应的时间与已花费的时间之差，作为本次循环的休眠时间。通过ROS的sleep机制进行休眠。

   此外，如果控制频率过高的话会输出一个警告信息，当时控制频率高于实际运算频率的时候终端会一直报警告：Control loop missed its desired rate of %.4fHz… the loop actually took %.4f seconds

r.sleep();
 //make sure to sleep for the remainder of our cycle time
if(r.cycleTime() > ros::Duration(1 / controller_frequency_) && state_ == CONTROLLING)
ROS_WARN("Control loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds", controller_frequency_, r.cycleTime().toSec());

   循环执行上面的①~⑥步

   （5）若上述循环是否因为各种原因导致节点被终止，则需要对当前的状态进行一定的处理

---

   4、planThread函数

   planThread函数在executeCb函数唤醒规划线程时被调用

   该线程创建以后会因为 runPlanner_ 值为false而挂起。当executeCb中唤醒线程前一刻时runPlanner_被设置为true。在这之后就正常情况下面没有被设置为fale过

   所以该线程被唤醒以后就不再进入等待的循环中，而是一直在大循环执行， 每个大循环都会调用一次 makeplan函数来调用全局路径规划器进行全局路径规划，并重新把新路径装入planner_plan_容器中(更新了路径) ，在executeCB中没有意外发生时只会唤醒一次该线程，以后不再唤醒。会一致执行大循环每个循环下发一次控制速度，但是每次循环中的路径可能会由于该线程一直在跑的缘故而发生改变。

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

void MoveBase::planThread()

   （1）在planThread函数中，首先设置目标点，清空路径存储容器

geometry_msgs::PoseStamped temp_goal = planner_goal_;
...
planner_plan_->clear();

   （2）接着是本函数的核心部分，调用了makePlan函数，进而调用全局路径规划器插件进行全局规划，如果成功，结果将储存进planner_plan_中。所有的全局路径规划器插件必然包含一个makePlan函数提供给move_base，从而在此处进行调用。

bool gotPlan = n.ok() && makePlan(temp_goal, *planner_plan_);

planner_->makePlan(start, goal, plan) 

   （3）若得到了可用的全局路径，则将标志位new_global_plan_ 置为ture，供局部路径规划使用

new_global_plan_ = true;

---

   5、executeCycle函数

   executeCB是路径规划的启动函数，其过程中通过调用完成了全局路径的规划，executeCB函数在上文介绍的3 -（4）- ④ 步骤中调用的executeCycle是局部路径规划的正式执行函数，在executeCycle函数中的setplan函数中将调用局部路径规划器插件进行局部路径规划。

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

   （1）在executeCycle函数中，首先获取机器人当前时刻在世界坐标系的位姿，并传递给feedback进行反馈和发布。

getRobotPose(global_pose, planner_costmap_ros_);

   （2）振荡检查，机器人每运动一段距离会记录一下当前时间以及位姿，其中current_ position是当前位姿，oscillation_pose_是上一个进入该判断时的位姿，参数oscillation_distance_是常数，默认值为0.5m

if(distance(current_position, oscillation_pose_) >= oscillation_distance_)

   （3）检查代价地图近期是否得到了更新。如果长时间没有更新，意味着机器人失去了周围的感知能力， 此时移动机器人是危险的，通过publishZeroVelocity 控制机器人停下来并退出运动逻辑。

if(!controller_costmap_ros_->isCurrent())

   （4）判断是否得到的新的全局路径，即new_global_plan_ 的值是否为True，若是，则调用setplan函数，储存全局路径，进而在需要的时候传递给局部路径规划使用，所有的局部路径规划器都应该有一个setplan函数作为接口供move_base进行调用来传递全局路径信息。

if(new_global_plan_)

if(!tc_->setPlan(*controller_plan_))

   （5）在将路径传入给局部路径规划函数后，executeCycle函数根据当前状态state_进行下一步的控制。

switch(state_)

   state_的状态分为三种：PLANNING、CONTROLLING、CLEARING。state_在全局路径规划之前被设置为PLANNING，代表当前处于路径规划阶段；然后在全局路径规划函数planThread中，完成全局路径规划并得到路径后被设置为CONTROLLING，代表当前路径成功，进入控制模式使机器人到达目标点；而在全局路径规划失败或者局部路径规划失败且超过重试次数时都会被置为CLEARING，代表取消当前规划。

   ①、若当前状态为PLANNING，说明发生的异常情况。全局路径规划没有完成就开始执行局部路径规划了，这时候算法会重新启动全局路径规划线程。
   注：（可在导航包中查询lock(planner_mutex_) 来查询启动全局规划的地方）

   ②、若当前状态为CLEARING，则全局路径规划失败了或者局部路径规划失败了。此时要考虑是否进行重新规划。

   如果重试的次数recovery_index_小于设置的次数recovery_behaviors_.size()，则调用全局路径规划看有没有新的路可以走。否则，根据设置的参数变量recovery_trigger_的状态进行对应的报错。

if(recovery_behavior_enabled_ && recovery_index_ < recovery_behaviors_.size())

   ③、若当前状态为CONTROLLING，也就是我们希望的正常状态，则先判断机器人是否到达目标点

if(tc_->isGoalReached())

   在LatchedStopRotateController::isGoalReached函数中，首先判断了当前坐标与目标点之间的距离是否小于设定值xy_goal_tolerance。如果满足该要求说明机器人已经在目标点附近，这时候算法再判断机器人的角度值是否复合设定阈值yaw_goal_tolerance，如果两个条件都满足说明机器人已经到达目标点。则返回true。则move_base的运动会设置为Succeeded。

   判断机器人是否处于震荡中，

 if(oscillation_timeout_ > 0.0 && last_oscillation_reset_ + ros::Duration(oscillation_timeout_) < ros::Time::now())

   若是，则发布将速度置为0的指令，并将状态置为CLEARING

   计算机器人的速度，通过调用局部规划器的computeVelocityCommands函数实现

if(tc_->computeVelocityCommands(cmd_vel))

   这里的computeVelocityCommands函数是move_base调用局部路径规划器插件进行局部规划的接口函数，每个局部路径规划器插件都要有一个computeVelocityCommands函数作为被调用的接口

   如果规划成功，则下发速度到cmd_vel；

   如果规划失败，此时如果如果距离上次成功的规划超过了容忍时间controller_patience_则将机器人停止，取消规划发布错误信息，如果此时还没超出容忍时间，则算法会再接受一下当前的状态，将速度改为0同时设置state_为PLANNING重新进行一次全局路径规划

---

---

   总的来说或Move_base在接收到目标点信息后，会通过回调函数，调用goalCB函数进行目标点格式的转换，真正的执行函数是构造函数里actionlib server注册的executeCb函数。executeCb函数也是move_base节点的主循环函数，按照设定的频率进行规划，executeCb函数启动规划线程的时候会调用planThread函数，而planThread函数中会调用 makeplan函数进行全局路径规划，也就是move_base节点调用全局规划器插件的接口函数 。

   executeCb函数还会调用executeCycle函数函数，通过setplan函数将得到的全局路径传递给局部路径规划器，在这个函数中会根据机器人当前的状态进行进一步处理，若为CONTROLLING，则说明为正常状态，调用 computeVelocityCommands函数进行局部路径规划 ，这个函数也是move_base节点调用局部规划器插件的接口函数。

   对于异常的PLANNING和CLEARING状态，则会根据情况决定是否要调用planThread函数重新尝试进行全局规划。

---