ROS局部路径规划器插件teb_local_planner流程梳理（上）
在我之前的文章《ROS导航包Navigation中的 Movebase节点路径规划相关流程梳理》中已经介绍过Move_base节点调用局部路径规划器插件的接口函数是computeVelocityCommands，接下来，我们就从这个函数入手梳理一下teb_local_planner功能包的工作流程。
☆注：因篇幅较长，本部分内容分成了上和下两篇文章，阅读完成本文的内容后，想要阅读后续内容，请前往下，继续阅读。
下篇文章链接如下：
《ROS局部路径规划器插件teb_local_planner流程梳理（下）》
一、规划前的准备工作
1、判断插件是否已完成初始化，若没有则返回错误信息
if(!initialized_)
2、获取机器人当前位姿及速度信息
costmap_ros_->getRobotPose(robot_pose);
odom_helper_.getRobotVel(robot_vel_tf);
robot_vel_.angular.z = tf2::getYaw(robot_vel_tf.pose.orientation);
3、对走过的全局路径部分进行修剪
由于机器人是实时运动的，所以需要对已经走过的全局路径裁减处理，通过调用pruneGlobalPlan函数完成裁剪处理，其实现思路是从全局路径规划容器的起点开始一个个遍历，计算其与机器人当前点的距离差，直到找到距离小于我们设定的阈值的点，将该点之前的全局路径点全部删除，这样就可以将位于机器人后方距离超过一定阈值的点删除掉。
上述判断用的阈值使用的是外部参数：global_plan_prune_distance。
//修改全局路径规划，将已经走过的路径删除，保留机器人前方的路径
pruneGlobalPlan(*tf_, robot_pose, global_plan_, cfg_.trajectory.global_plan_prune_distance);
4、 将全局路径转换到局部代价地图的坐标系下，并从中选择合适的点作为局部路径点
调用transformGlobalPlan函数，通过tf 树将全局路径信息转换到局部代价地图的坐标系下，值得注意的是，在这个函数中并不仅仅进行了坐标系的变换，还对全局路径信息点进行了一些处理，从中选出合适的点作为局部路径规划点。
其具体过程是依次对全局路径进行坐标变换，同时计算其与当前位置的距离，并统计已转换的全局路径点之间的距离和（即已转换的全局路径的长度），直到超过设定的长度阈值或者与当前点的距离大于设定的距离阈值，则丢弃剩余的路径点。
其中，长度阈值由外部参数max_global_plan_lookahead_dist决定，距离阈值由局部代价地图的大小width和height以及系数0.85有关
transformGlobalPlan(*tf_, global_plan_, robot_pose, *costmap_, global_frame_, cfg_.trajectory.max_global_plan_lookahead_dist,
transformed_plan, &goal_idx, &tf_plan_to_global)
附：距离阈值sq_dist_threshold 计算过程：
double dist_threshold = std::max(costmap.getSizeInCellsX() * costmap.getResolution() / 2.0,
costmap.getSizeInCellsY() * costmap.getResolution() / 2.0);
dist_threshold *= 0.85; // just consider 85% of the costmap size to better incorporate point obstacle that are
double sq_dist_threshold = dist_threshold * dist_threshold;
概括一下，经过本步之后从全局路径信息中，选出的局部路径点是在以机器人当前位置为圆心距离阈值sq_dist_threshold 半径的圆内的路径点，且累积路径长度小于路径长度阈值的点的集合。
5、 对得到的局部路径规划点进行进一步的筛选
调用updateViaPointsContainer函数，对上一步得到的局部路径规划点进行进一步的筛选，其实现思路是，循环遍历局部路径规划点，从中选出距离上一个被选中的点的距离大于设定的阈值的点（局部路径规划点中的第一个点为本步筛选中第一个被选中的点）
这样处理可以将上一步得到的局部路径点稀疏化，从而减少局部路径点的个数，阈值由外部参数global_plan_viapoint_sep决定，若该参数设置为负数，则表示跳过本步处理，不进行筛选
updateViaPointsContainer(transformed_plan, cfg_.trajectory.global_plan_viapoint_sep);
第3~5步过程动态演示如下：
ROS的TEB局部路径规划器全局路径处理过程示意
6、 判断机器人是否到达目标点
得到局部路径的状态点后，先检查是否已经到达了全局目标点，具体要检查以下内容：
（1）当前点距离目标点的距离是否小于由外部参数设定的阈值xy_goal_tolerance
（2）当前点的姿态角与目标姿态角是否小于由外部参数设定的阈值yaw_goal_tolerance
（3）判断机器人是否完全到位，由参数complete_global_plan以及via_points_.size()影响，其中complete_global_plan主要作用是防止机器人在越过终点时提前结束路径
（4）有些版本中还会检测判断机器人当前速度是否停止，跟参数theta_stopped_vel、trans_stopped_vel、free_goal_vel有关，其中free_goal_vel由外部参数设定是否允许机器人以最大速度驶向目的地，即是否允许机器人达到目标点的速度不为零，参数heta_stopped_vel和trans_stopped_vel在melodic版本的包中并没有。
if(fabs(std::sqrt(dx*dx+dy*dy)) < cfg_.goal_tolerance.xy_goal_tolerance
&& fabs(delta_orient) < cfg_.goal_tolerance.yaw_goal_tolerance
&& (!cfg_.goal_tolerance.complete_global_plan || via_points_.size() == 0))
7、 判断是否进入恢复行为模式
调用configureBackupModes函数判断是否进入恢复模式：
（1）短时间内规划失败：若机器人规划失败时，没有到达目标点，但是距离上次局部路径规划成功的时间间隔很小，算法会缩小规划范围，对局部路径点进行裁减，将远处的点先裁减掉，以更近的点作为规划目标，尝试重新规划出可行路径。
（2）长时间的规划失败：多次短期规划都失败后，算法判断机器人是否长时间在某个地方发生振荡，其具体思路是：首先将机器人当前速度与角速度归一化到[0,1]，然后对线速度与角速度进行判断，若机器人在一段时间内的平均线速度与角速度都小于阈值，并且前后两次采样过程中机器人的角速度相反，则认为机器人陷入了振荡状态（算法中创建了容器buffer_。用于面存储了一段时间内的机器人线速度与角速度）
当机器人处于振荡状态，同时如果这个时间超过阈值oscillation_recovery_min_duration则机器人会进行旋转，旋转方向由机器人当前角速度反方向决定，如果没有超过阈值则以当前机器人角速度方向进行旋转。
configureBackupModes(transformed_plan, goal_idx);
8、调整局部目标点的角度
根据外部参数 global_plan_overwrite_orientation决定是否调整局部目标点的角度（也就是经过上述处理后的路径的最后一个点的角度），调整是通过调用estimateLocalGoalOrientation函数来实现的
if (cfg_.trajectory.global_plan_overwrite_orientation)
robot_goal_.theta() = estimateLocalGoalOrientation(global_plan_, transformed_plan.back(), goal_idx, tf_plan_to_global);
当需要修改时，会判断当前局部路径规划的终点与全局路径规划的终点是否接近，如果两个点比较接近(就是同一个点或者下一个点就是终点)则直接使用当前局部路径规划中终点的方向作为规划的方向。如果当前局部路径规划的终点跟全局目标点之间还间隔很多个点的话，会使用当前局部路径的终点以及向后两个点的角度进行平滑处理，得到一个新的角度值。
当不需要修改时，采用局部目标点的原有角度。
9、将机器人当前的位姿插入到局部路径点中
上一步完成了局部规划的目标点的确认，这一步要完成局部规划的起始点的确认，，前面虽然根据全局路径规划中截取了局部路径规划的点，但是这个容器的起点不一定是是机器人当前时刻所处的点，因此，需要将机器人当前位姿插入到前面得到的局部路径点中作为规划的起点。
transformed_plan.front() = robot_pose; // update start 更新起始点
10、更新障碍物信息
根据外部参数 costmap_converter决定如何更新障碍物信息，若该参数不为空，则调用costmap_converter插件进行地图信息更新，若为空直接使用costmap的信息进行更新。
分别对应下面那个函数：
updateObstacleContainerWithCostmapConverter
updateObstacleContainerWithCostmap
若使用costmap_converter插件进行更新，则在updateObstacleContainerWithCostmapConverter函数中调用了getObstacles()函数从代价地图转换器获得障碍物，并对障碍物根据其形状（圆、点、线、多边形）进行分类，将其转换成相应的障碍物类型，然后将障碍物坐标存储到obstacles_容器中，针对移动的障碍物还会设定其速度。
若使用costmap的信息进行更新，根据外部参数 include_costmap_obstacles决定是否考虑costmap中的障碍物，若考虑则遍历整个代价地图，对于每一个代价地图单元格，如果该单元格的代价为LETHAL_OBSTACLE（代价值为100），则将其视为障碍物，并检测该障碍物是否足够引人注意(例如，离机器人不远)，如果障碍物在机器人后面且距离机器人的距离超过了外部参数决定的阈值 costmap_obstacles_behind_robot_dist，则忽略该障碍物。最后将得到的障碍物添加到障碍物容器中
除此之外，还要从ROS消息中获取障碍物信息，然后将这些障碍物添加到当前的障碍物容器中。该过程通过函数updateObstacleContainerWithCustomObstacles完成
// also consider custom obstacles (must be called after other updates, since the container is not cleared)
// 还要考虑custom障碍(必须在其他更新之后调用，因为障碍容器没有被清除)
updateObstacleContainerWithCustomObstacles();
消息来源于：
custom_obst_sub_ = nh.subscribe("obstacles", 1, &TebLocalPlannerROS::customObstacleCB, this);
至此，规划前的准备工作完成，下面正式开始规划进行局部路径的规划
二、调用核心函数plan进行局部路径规划
bool success = planner_->plan(transformed_plan, &robot_vel_, cfg_.goal_tolerance.free_goal_vel);
需要注意的是存在三个名为plan的函数，根据传入的形参及其含义可知此处调用的是如下函数：
bool TebOptimalPlanner::plan(const std::vector<geometry_msgs::PoseStamped>& initial_plan, const geometry_msgs::Twist* start_vel, bool free_goal_vel)
1、根据teb_.isInit()判断是否初始化从起点到目标点的路径
若是，则调用initTrajectoryToGoal函数完成该操作，若不是则采用热启动的方式，此时需要进一步判断，如果当前目标与上一个目标的位置差小于阈值force_reinit_new_goal_dist，姿态角度差小于阈值force_reinit_new_goal_angular，则调用updateAndPruneTEB函数清理路径上已经走过的点，若不满足上述阈值，则认为当前目标点与上一个目标点之间的距离足够远了，需要重新调用initTrajectoryToGoal函数进行路径的初始化。
其中，updateAndPruneTEB函数的具体实现思路是，首先在局部路径规划点中寻找距离当前位置最近的点，然后从局部路径规划点的第一个点开始往后搜索直至找到这个最近的点。然后删除这个最近的点之前的那些点。
调用initTrajectoryToGoal函数的语句如下：
teb_.initTrajectoryToGoal(initial_plan, cfg_->robot.max_vel_x, cfg_->robot.max_vel_theta, cfg_->trajectory.global_plan_overwrite_orientation,
cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion);
需要注意的是存在两个名为initTrajectoryToGoal的函数，根据传入的形参及其含义可知此处调用的是如下函数：
bool TimedElasticBand::initTrajectoryToGoal(const std::vector<geometry_msgs::PoseStamped>& plan, double max_vel_x, double max_vel_theta, bool estimate_orient, int min_samples, bool guess_backwards_motion)
其具体实现思路如下：
（1）检查是否已经初始化路径，如果没有，则添加起始位置处的姿态点(包括机器人的位置和朝向)到局部路径点的容器中，并设置起点优化中为固定不变的。
（2）设定后退标志位，若外部参数 allow_init_with_backwards_motion为真，即否允许在开始时想后退来执行轨迹，且目标点在起始点的后方，则将后退标志位backwards标志设置为true，否则为false。
（3）从起始点开始遍历局部路径点容器中的每个点，若外部参数 global_plan_overwrite_orientation为真，即否允覆盖全局路径中局部路径点的朝向，若允许，则根据相邻两个局部路径点之间的连线的朝向利用atan2函数重新计算每个局部路径点的朝向，否则，保持每个局部路径姿态点的原有朝向不变。
在上述遍历中，还会利用外部参数 max_vel_x和max_vel_theta，即设定的最大线速度和角速度来预估相邻两个局部路径点之间要花费的时间dt，即两点间位置差除以max_vel_x，得到线运动时间，两点间角度差除以max_vel_theta得到角运动时间，然后取这两个时间中的较大值作为两点之间小路径的预估时间dt。
（4）判断是否需要强制添加局部路径点，若当前的局部路径点的数量小于由外部参数设定的最小样本数 min_samples-1，为了保证生成的路径点数量足够多，则会强制添加一些局部路径点，具体方式是不断在当前点与目标点之间取中点添加到局部路径点中（当前点会变化），直至局部路径点的数量等于min_samples-1。
static PoseSE2 average(const PoseSE2& pose1, const PoseSE2& pose2)
{
return PoseSE2( (pose1._position + pose2._position)/2 , g2o::average_angle(pose1._theta, pose2._theta) );
}
（5）将目标点添加到局部路径点的末尾处，并将其设为固定，即在优化中不能被改变。
至此，局部路径的初始化完成，可以这样理解以上过程，当我们接收到新的目标点时（新的目标点与上一个目标点的距离或姿态差大于设定的阈值）需要重新调用initTrajectoryToGoal函数进行路径的初始化，若目标点没有变化，或者变化小于阈值，则认为是机器人移动过程中针对同一目标点的多次规划，此时只需要调用updateAndPruneTEB函数对已经走过的部分的路径点进行裁剪就可以了，不需要重新进行路径初始化，也就是所谓的热启动模式。
☆注：因篇幅较长，本部分内容分成了上和下两篇文章，阅读完成本文的内容后，想要阅读后续内容，请前往下，继续阅读。
下篇文章链接如下：
《ROS局部路径规划器插件teb_local_planner流程梳理（下）》