在我之前的文章《ROS导航包Navigation中的 Movebase节点路径规划相关流程梳理》中已经介绍过Move_base节点调用局部路径规划器插件的接口函数是computeVelocityCommands，本部分来，我们从这个函数入手梳理teb_local_planner功能包的工作流程。
☆注：因篇幅较长，本部分内容分成了上和下两篇文章，在上篇中我们已经梳理完成了规划前的准备工作，本文是对上篇的延续，在阅读本文之前，确保已经阅读完上篇的内容，以此来更好的对整个流程进行理解。
上篇文章链接如下：
《ROS局部路径规划器插件teb_local_planner流程梳理（上）》
2、从里程计读取当前速度，作为起点处的速度
setVelocityStart(*start_vel);
3、根据参数free_goal_vel设定目标点处速度，若free_goal_vel为True，则允许到达目标点处速度不为0，为false，则到达目标点处的速度要为0。
4、调用optimizeTEB函数，根据设定的teb约束构建图，然后利用g2o优化图，也就是TEB算法核心中的核心部分，接下来，我们详细对该函数进行介绍。
optimizeTEB(cfg_->optim.no_inner_iterations, cfg_->optim.no_outer_iterations);
（1）根据外部参数 teb_autosize的值决定是否再优化一下局部路径点，若为True，则进行优化，调用函数autoResize进行优化
teb_.autoResize(cfg_->trajectory.dt_ref, cfg_->trajectory.dt_hysteresis, cfg_->trajectory.min_samples, cfg_->trajectory.max_samples, fast_mode);
还记得我们在上面对plan函数介绍时的第（3）步中我们根据局部路径点中相邻两点的距离和最大线速度和角速度估计出了该两点间的运动时间，在本步中我们需要对两点间的运动时间间隔进行检查，如果某个时间间隔大于参考时间间隔加上时间滞后，且采样数小于最大采样数，则在此处插入一个新的状态；如果时间间隔小于参考时间间隔减去时间滞后，并且采样数大于最小采样数，则删除此处的状态。
函数在进行这些插入和删除操作后会调整插入和删除点之间的时间间隔。如果fast_mode为真，则会在一次迭代中只进行一次调整，即在本次调用autoResize函数中，调整过一个点后，就返回，否则将进行多次循环，直到没有点需要调整或者达到设定的最大循环次数100才返回。
上述过程中的参考时间间隔和时间滞后分别由外部参数 dt_ref和dt_hysteresis决定，最大采样点和最小采样点由外部参数 max_samples和min_samples决定，fast_mode由外部参数 include_dynamic_obstacles取反后得到
（2）调用buildGraph函数进行图的构建，包括添加顶点、添加约束等过程
// 调用buildGraph函数构建图，并根据weight_multiplier进行权重调整。如果buildGraph函数执行失败，则它将清空图，并返回false。
success = buildGraph(weight_multiplier);
①、调用AddTEBVertices函数为图添加顶点，图的顶点包含姿态和运动时间两部分信息，将局部路径点及运动时间添加到优化器中，以便后续的约束关系可以被添加到这些顶点之间。
AddTEBVertices();
②、为图添加边
首先添加障碍物边，若外部参数 egacy_obstacle_association值为True，则调用AddEdgesObstaclesLegacy函数，采用旧的障碍物关联策略，对于每个障碍，找到最近的TEB路径点，值为false，则调用AddEdgesObstacles函数，采用新的障碍物关联策略，对于每个TEB路径点，仅找到相关"障碍
其中AddEdgesObstacles函数的具体实现如下：首先判断是否进行了障碍物膨胀，之后定义了一个创建边的函数。之后，遍历每一个局部路径点，找到离该路径点距离小于阈值的障碍物，以及左侧和右侧最近的障碍物，构建EdgeObstacle对象，作为图的障碍物边。边误差的计算为该路径点到障碍物的距离，再过一个激活函数。
AddEdgesObstaclesLegacy(weight_multiplier);
AddEdgesObstacles(weight_multiplier);
然后根据外部参数 include_dynamic_obstacles判断是否启用了动态障碍物，若启用了，则调用AddEdgesDynamicObstacles函数，添加动态障碍物边。
AddEdgesDynamicObstacles();
然后依次调用如下函数，分别添加经过点的边、速度边、加速度边、最优时间边、最短路径边。
其中添加经过点边的AddEdgesViaPoints函数会先遍历每一个路径点，计算与当前路径点最近的坐标点，构建路径点边，类型为 EdgeViaPoint，边的误差计算就是欧氏距离。
AddEdgesViaPoints(); //添加经过点的边。
AddEdgesVelocity(); //添加速度边。
AddEdgesAcceleration(); //添加加速度边
AddEdgesTimeOptimal(); //添加最优时间边。
AddEdgesShortestPath(); //添加最短路径边。
然后根据外部参数 min_turning_radius和weight_kinematics_turning_radius判断机器人的模型，若这两个参数任意一个为0，则认为当前机器人为差分驱动的机器人，即最小转弯半径为0，此时调用AddEdgesKinematicsDiffDrive函数添加差分驱动的边，若这两个参数都不为0，则认为是汽车模型，调用AddEdgesKinematicsCarlike函数添加汽车模型的边，此目的是使得生成的轨迹满足运动学约束。
AddEdgesKinematicsDiffDrive()
AddEdgesKinematicsCarlike()
然后调用AddEdgesPreferRotDir函数添加首选旋转方向的边，
AddEdgesPreferRotDir(); //添加首选旋转方向的边。
如果权重速度障碍物比weight_velocity_obstacle_ratio大于0，则调用AddEdgesVelocityObstacleRatio函数，添加速度障碍物比边
AddEdgesVelocityObstacleRatio()
（3）调用optimizeGraph函数对图进行优化，首先设置优化器的详细信息并初始化优化器，并按照内循环次数参数 no_inner_iterations调用optimize函数进行具体的优化。
// //设置优化器的详细信息并初始化优化器。
optimizer_->setVerbose(cfg_->optim.optimization_verbose);
optimizer_->initializeOptimization();
int iter = optimizer_->optimize(no_iterations); //进行指定次数的优化迭代。
（4）清空图，按照外部参数设定的外循环次数 no_outer_iterations，循环执行上述（1）~（3）步
（5）若参数compute_cost_afterwards为真，则在最后一次外循环迭代后调用computeCurrentCost函数计算当前的代价Cost，包含时间成本及障碍物成本，最后返回当前的总成本，缺省调用optimizeTEB函数时，compute_cost_afterwards的值默认为false。
至此，optimizeTEB函数就执行完成了，也代表着plan函数执行完成了，到这里我们得到了优化后的路径或者说轨迹，接下来就要根据该轨迹计算每个姿态点处的线速度和角速度。
三、计算下发给下位机的速度指令
我们接着回到computeVelocityCommands函数中，在调用plan函数后，得到了优化后的轨迹，接下来需要进行一些准备工作，然后根据轨迹中的位姿点及时间差计算运动指令，即线速度和角速度
1、根据外部参数 is_footprint_dynamic决定是否更新机器人的外形信息以及从机器人中心到其外形边缘顶点的最小和最大距离，若参数is_footprint_dynamic为真，则认为机器人的外形是变化的需要进行此步，否则不进行此步。
2、调用isTrajectoryFeasible函数检查优化后得到的路径是否可行，全局路径规划得到的路径是确认可以到达终点的，经过局部路径规划优化后的轨迹会与全局路径规划有所区别，同时还有上面的可能存在的机器人模型的变化，所以对于优化后的轨迹我们需要重新判断它的可行性，根据外部参数 feasibility_check_no_poses的值决定检查轨迹中路径点的个数，比如其值为4，则检测轨迹中前4个路径点处是否与障碍物相交，需要注意的是，会把机器人的外形轮廓放置到该路径点处进行碰撞检测，而不是仅仅对该点进行检测，若有碰撞，则返回可行性检测未通过，将线速度和角速度设置为0，重置规划器，开始新的规划。
此外，同时还会判断局部路径上每个点之间的距离以及朝向角度差，这个距离不能超过机器人的长度，如果超过了机器人的长度则点上的模型不能完全覆盖机器人路径，可能会存在说两个姿态点处没有碰撞，但是两点间的路径之间存在障碍物的问题，这样仅仅对点进行判断是不能完全保证路径可行的。此时，要对路径按照机器人长度进行差值，直到模型覆盖整条路径为止。角度的问题也是一样的。
bool feasible = planner_->isTrajectoryFeasible(costmap_model_.get(), footprint_spec_, robot_inscribed_radius_, robot_circumscribed_radius, cfg_.trajectory.feasibility_check_no_poses);
3、可行性检测通过后，调用getVelocityCommand函数计算速度指令，也是本部分的核心函数
getVelocityCommand(cmd_vel.twist.linear.x, cmd_vel.twist.linear.y, cmd_vel.twist.angular.z, cfg_.trajectory.control_look_ahead_poses)
在该函数中首先检查轨迹中路径点的个数若小于2个则将速度设为0，报错返回，大于等于2，则继续进行，从局部路径点容器中依次累加两点间的时间差直至大于时间分辨率 dt_ref * control_look_ahead_poses，至此找到了局部路径点容器中和起点之间时间差大于dt_ref * control_look_ahead_poses的点，该过程还会受参数 control_look_ahead_poses（默认为1）的影响，这个参数决定了最多累加几次，默认为1时，找到的点其实就是局部路径点中起点和其后面的相邻点，无论他们的时间差是否大于dt_ref，因为只允许累加1次。其实这个参数可以理解成控制前瞻，即更倾向于选择当前点与之后的第几个点来计算当前的速度指令。
然后，根据这两个点的位姿信息、时间差、当前的速度信息，调用extractVelocity函数，计算并更新速度信息，对于非完整约束的类车型机器人，其y轴速度为0，x轴速度为两点间距离除以时间差，若为完整约束的机器人，即y轴上的速度可以不为0在，则x轴与y轴速度分别为两点在x轴与y轴的距离差除以时间差，角速度为姿态角差除以时间差。返回true
extractVelocity(teb_.Pose(0), teb_.Pose(look_ahead_poses), dt, vx, vy, omega);
4、速度限幅，如果优化结果违反速度约束(可能是由于软约束引起的)。则根据参数use_proportional_saturation进行处理，若该参数为默认值false，则对x、y轴速度及角速度分别按各自的比例进行处理，效果等效于若超过其对应的最大值，则限幅为其最大值。
若use_proportional_saturation的值为True，则按同比例进行处理，比如当前x，y的速度及角速度分别为2、1、1，其对应最大值分别为 1.5、1.5、0.5，y轴速度未超过最大值，比例设为1，x轴速度超过最大值比例取为最大速度/当前速度，即1.5/2=0.75，同理角度比例为 0.5/1=0.5，取这三个比例的最小值，为共同的比例，即min（1,0.75,0.5）=0.5，则将当前速度都乘以该比例，由 2、1、1，变为1、0.5、0.5。
saturateVelocity(cmd_vel.twist.linear.x, cmd_vel.twist.linear.y, cmd_vel.twist.angular.z,
cfg_.robot.max_vel_x, cfg_.robot.max_vel_y, cfg_.robot.max_vel_theta, cfg_.robot.max_vel_x_backwards);
5、非完整约束机器人的角速度转换
若外部参数 cmd_angle_instead_rotvel为真，则需要将上面得到的角速度转换为舵机的转角，常用于类车型机器人（当然也可以不用），其具体过程是，先根据当前x轴的线速度除以角速度得到转向半径radius，若该半径小于最小转弯半径 min_turning_radius，则修正为最小转弯半径，然后根据外部参数车辆模型的前后轮间距 wheelbase与该转弯半径radius的比值取反三角函数即可得到需要的转角
// 根据公式计算前轮转角并返回
return std::atan(wheelbase / radius);
6、将得到的线速度和角速度/角度发布出去，至此TEB的流程结束。