move_base代码解析（三）MoveBase::executeCycle
第一篇executeCb主要分析了一下move中对于传入数据的一些基本处理，该函数首先判断了传入目标点的四元数是否合理，然后将该坐标转换到世界坐标系下同时通过publisher发布该坐标用于可视化显示。随后开启全局路径规划线程以及代价地图costmap（全局路径规划在第二篇单独展开讲述），记录当前的开始时间以及规划的状态机state，最后完成这些后函数开始调用executeCycle(goal)函数执行正式的路径规划过程。这一篇则展开看executeCycle函数具体做了一些什么东西。
1、获取global坐标
在executeCycle的第一步是获取了机器人的世界坐标系：
geometry_msgs::PoseStamped global_pose;
//这里相当于通过一个tf获取了map到base_link的关系，那么也就是map下base_link的坐标，也就是map下机器人的坐标，这个坐标在global_pose中返回
getRobotPose(global_pose, planner_costmap_ros_);
对于getRobotPose这个函数虽然单独拎出来了，但其实本质只是求取了一个tf变换。也就是下面的try里面的函数：
tf2::toMsg(tf2::Transform::getIdentity(), global_pose.pose);
geometry_msgs::PoseStamped robot_pose;
tf2::toMsg(tf2::Transform::getIdentity(), robot_pose.pose);
robot_pose.header.frame_id = robot_base_frame_;
robot_pose.header.stamp = ros::Time(); // latest available
ros::Time current_time = ros::Time::now(); // save time for checking tf delay later
// get robot pose on the given costmap frame
try
{ //这里相当于通过一个tf获取了map到base_link的关系，那么也就是map下base_link的坐标，也就是map下机器人的坐标
tf_.transform(robot_pose, global_pose, costmap->getGlobalFrameID());
}
查看一下这个TF函数的本质：
/** \brief Transform an input into the target frame.
* This function is templated and can take as input any valid mathematical object that tf knows
* how to apply a transform to, by way of the templated math conversions interface.
* For example, the template type could be a Transform, Pose, Vector, or Quaternion message
* type (as defined in geometry_msgs).
* \tparam T The type of the object to transform.
* \param in The object to transform
* \param out The transformed output, preallocated by the caller.
* \param target_frame The string identifer for the frame to transform into.
* \param timeout How long to wait for the target frame. Default value is zero (no blocking).
*/
template <class T>
T& transform(const T& in, T& out,
const std::string& target_frame, ros::Duration timeout=ros::Duration(0.0)) const
{
// do the transform
tf2::doTransform(in, out, lookupTransform(target_frame, tf2::getFrameId(in), tf2::getTimestamp(in), timeout));
return out;
}
可以知道它相当于获取了一下robot_pose转换到costmap->getGlobalFrameID()下的坐标。而getGlobalFrameID()其实就是返回了世界坐标系的frame_id。而这里的robot_pose的初始值其实是一个单位矩阵，其frame_id为“base_link"，所以这个函数其实相当于返回了base_link到map坐标系的关系。也就是机器人在map中的坐标。
inline const std::string& getGlobalFrameID() const noexcept
{
return global_frame_;
}
2、发布机器人坐标
const geometry_msgs::PoseStamped& current_position = global_pose;
//push the feedback out
move_base_msgs::MoveBaseFeedback feedback;
feedback.base_position = current_position;
as_->publishFeedback(feedback);
在获取了机器人的世界坐标系后，executeCycle将该坐标再发布出来。这里的机器人坐标系不是通过publish发布的而是通过action中的feedback发布的，对于一个action而言，feedback可以返回当前时间的一些状态。具体的可以看一看关于action的一些介绍。
3、振荡距离判断
if(distance(current_position, oscillation_pose_) >= oscillation_distance_)
{
last_oscillation_reset_ = ros::Time::now();
oscillation_pose_ = current_position;
//if our last recovery was caused by oscillation, we want to reset the recovery index
//如果上次的恢复是由振荡触发的，则重置恢复指标
if(recovery_trigger_ == OSCILLATION_R)
recovery_index_ = 0;
}
这里的意思看起来像是每隔一段时间记录一下当前时间以及位姿的感觉。oscillation_distance_是一个常数，current_position是当前坐标，oscillation_pose_是上一个进入该判断时的坐标。也就是当机器人每运动一段距离会进入一个该判断然后记录几个参数，那么这几个参数有什么用处呢？先往后看。
4、检查局部代价地图
if(!controller_costmap_ros_->isCurrent()){// controller_costmap_ros_：局部代价地图和局部规划器
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());
publishZeroVelocity();
return false;
}
controller_costmap_ros_是MoveBase的局部代价地图，通过其接口isCurrent()，我们可以查看代价地图近期是否得到了更新。如果长时间没有更新，意味着机器人失去了周围的感知能力， 此时移动机器人是危险的，所以我们通过publishZeroVelocity控制机器人停下来并退出运动逻辑。
5、处理新的目标点
前面的1-4步同样是一些前期准备，后面的部分则是正式开始局部路径规划（全局路径规划在第二章中完成）。这里第5步是需要将全局路径规划的路径传递给局部路径规划器tc_。在move_base节点中，通过类加载模块载入了BaseLocalPlanner（局部路径规划）的子类DWAPlannerROS的实例tc_。所以move_base默认的局部路径规划算法是使用的DWA算法。这个算法后面展开讲，这里先大致看一下流程。
bool DWAPlannerROS::setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan) {
if (! isInitialized()) {
ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
return false;
}
//when we get a new plan, we also want to clear any latch we may have on goal tolerances
latchedStopRotateController_.resetLatching();
ROS_INFO("Got new plan");
return dp_->setPlan(orig_global_plan);
}
这里调用了DWA算法的DWAPlanner类的实例dp_：
bool DWAPlanner::setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan) {
oscillation_costs_.resetOscillationFlags();
return planner_util_->setPlan(orig_global_plan);
}
而DWAPlanner::setPlan则调用了base_local_planner::LocalPlannerUtil：
bool LocalPlannerUtil::setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan) {
if(!initialized_){
ROS_ERROR("Planner utils have not been initialized, please call initialize() first");
return false;
}
//reset the global plan
global_plan_.clear();
global_plan_ = orig_global_plan;
return true;
}
虽然这个函数前后调用了3个函数，但是最后真正实现功能的就是最后一行：
global_plan_ = orig_global_plan;
orig_global_plan是传入的来自全局路径规划的坐标点，global_plan_是定义在LocalPlannerUtil类下的std::vector
6、根据状态进行路径规划
在将路径传入给局部路径规划函数后，executeCycle函数根据当前状态state_进行下一步的控制。state_的状态分为三种：PLANNING、CONTROLLING、CLEARING。state_在全局路径规划之前被设置为PLANNING，代表当前处于路径规划阶段；然后在全局路径规划函数planThread中，完成全局路径规划并得到路径后被设置为CONTROLLING，代表当前路径成功，进入控制模式使机器人到达目标点；而在全局路径规划失败或者局部路径规划失败且超过重试次数时都会被置为CLEARING，代表取消当前规划。简单看一下这三种情况的处理：
6.1、PLANNING
对于PLANNING的情况，说明前面出现了问题，全局路径规划没有完成就开始执行局部路径规划了，这时候算法会重新启动全局路径规划线程。
case PLANNING:
{
boost::recursive_mutex::scoped_lock lock(planner_mutex_);
runPlanner_ = true;
planner_cond_.notify_one();
}
6.2、CONTROLLING
对于CONTROLLING的情况，才是正常机制下机器人应该执行的流程，它主要包含了如下几个部分：
6.2.1、判断机器人是否到达目标点
if(tc_->isGoalReached()){
ROS_DEBUG_NAMED("move_base","Goal reached!");
resetState();
//disable the planner thread
boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
runPlanner_ = false;
lock.unlock();
as_->setSucceeded(move_base_msgs::MoveBaseResult(), "Goal reached.");
return true;
}
在CONTROLLING阶段机器人会首先判断自己是否已经到达目标点，这个函数的具体实现在LatchedStopRotateController::isGoalReached里面，它首先判断了当前坐标与目标点之间的距离是否小于设定值xy_goal_tolerance。如果满足该要求说明机器人已经在目标点附近，这时候算法再判断机器人的角度值是否复合设定阈值yaw_goal_tolerance，如果两个条件都满足说明机器人已经到达目标点。则返回true。则move_base的运动会设置为Succeeded。
6.2.2、判断机器人是否处于晃荡
if(oscillation_timeout_ > 0.0 &&
last_oscillation_reset_ + ros::Duration(oscillation_timeout_) < ros::Time::now())
{
publishZeroVelocity();
state_ = CLEARING;
recovery_trigger_ = OSCILLATION_R;
}
这里的参数：oscillation_timeout_指在执行恢复动作前等多久，来 oscillation （晃荡），如果是 0.0 的话，意味着可以永久晃荡下去。不是特别理解这个东西，后面需要再看一下。
6.2.3、计算机器人的速度
if(tc_->computeVelocityCommands(cmd_vel)){
ROS_DEBUG_NAMED( "move_base", "Got a valid command from the local planner: %.3lf, %.3lf, %.3lf",
cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z );
last_valid_control_ = ros::Time::now();
//make sure that we send the velocity command to the base
vel_pub_.publish(cmd_vel);
if(recovery_trigger_ == CONTROLLING_R)
recovery_index_ = 0;
}
计算得到机器人的速度，机器人的速度由局部路径规划给出，move_base使用的局部规划器默认为TrajectoryPlannerROS，它调用TrajectoryPlanner类函数来进行局部路径规划，得到下一步速度，反馈给move_base。具体的规划过程后面单独展开讲述。
速度规划存在两种情况：如果规划成功，则下发速度到cmd_vel；如果规划失败，此时如果如果距离上次成功的规划超过了容忍时间controller_patience_则将机器人停止，取消规划发布错误信息，如果此时还没超出容忍时间，则算法会再接受一下当前的状态，将速度改为0同时设置state_为PLANNING重新进行一次全局路径规划
6.3、CLEARING
对于CLEARING的情况我们知道它存在于两种条件下：要么全局路径规划失败了，要么局部路径规划失败了。反正就是路径规划不出来了。对于这种情况，首先我们要考虑一下是不是再给次机会规划一下：
if(recovery_behavior_enabled_ && recovery_index_ < recovery_behaviors_.size())
如果重试的次数recovery_index_小于设置的次数recovery_behaviors_.size()，则我们还是会调用一下全局路径规划再找找看有没有新的路可以走。但是如果重试了很多次了，已经超出了设置的次数。则会根据设置的参数变量recovery_trigger_的状态进行对应的报错。
7、小结
MoveBase::executeCycle的总体作用主要是在全局规划完成后，调用局部规划线程进行具体的局部路径规划。在move_base中使用的局部规划器默认为TrajectoryPlannerROS，它调用TrajectoryPlanner类函数来进行局部路径规划，得到下一步速度，反馈给move_base。如果速度没有规划出来，算法根据当前状态判断是否进行新的全局路径规划，超时或者超过重试次数则退出规划上报错误。