teb_localplanner源码学习

概述

teb_localplanner是以优化的方法进行局部导航的软件包。基于timed elastic band 方法进行轨迹生成。考虑了运动时的轨迹执行时间、避障、动力学的界约束等条件。此笔记是对teb包的源代码进行学习的一个记录。

teb包的类继承图

1、teb中的g2o类型

利用doxywizard+graphiz对包进行解析，查看类继承图
teb的优化问题的解决利用了开源软件包g2o，先看看对g2o边和顶点的定义。

主要是考虑了运动学的约束关系（车的相邻的两个位姿态必须落在一个圆弧上）。二元边的两个顶点分别是两个位姿。还限制了最小转弯半径。

一元边：

EdgeViaPoint：以指定的经过点位置与待优化位姿（顶点）的误差作为目标。（保证轨迹会经过指定点）
EdgeObstacle：以障碍物与待优化位姿（顶点）的距离经过惩罚函数后的输出作为目标（保证离障碍物大于一定距离）
EdgeTimeOptimal：以时间间隔dt最小作为目标。（保证时间最短轨迹）

多元边（note:构造函数中resize顶点的个数）：

EdgeVelocity：以实际速度和限定最大速度经过惩罚函数的输出作为目标函数，顶点为相邻的位姿和时间间隔。（限制实际速度不超过最大，程序中引入了sigmoid函数决定速度的符号（根据论文内的说法，引入此函数是因为优化算法只能求解连续函数））。
Edgevelocityholonomic：与EdgeVelocity的区别在于，ds是机器人本体坐标系下的，然后去求速度在本体坐标系下的表示。其余思路一致。

2、其他类

类的简介：
此类定义了一条时变弹性带的轨迹模型，里面包含两个最重要的数据，数据类型为位姿（teb定义的g2o顶点）指针的vector，数据类型为时间（teb定义g2o顶点）指针的vector。
类的接口（仅列举一部分）：对位姿、时间顶点的入栈、出栈、插入、删除、读取等操作;初始化轨迹（以指定的起点和终点进行插值产生的轨迹）;更新和改变轨迹（根据现在的位置更新起点（vector中的第一个点），改变终点）;找到最近点（离指定的点、直线或者其他）;计算是走过的路程之和;计算花费时间之和;探测轨迹是否转弯等操作。

=======================================================

类简介：此类中主要存储了用于配置TEB的参数，包括轨迹参数、障碍参数、机器人参数、优化参数、目标tolerance参数、同伦类参数（这是啥？）。
轨迹参数：包含轨迹的时空分辨率的指定、采用、是否以全局目标覆盖局部子目标、via_point序列等等。
障碍参数：与障碍的最小分隔距离、膨胀半径、是否包含动态障碍（会启用常速度模型预测障碍位置）、等参数。
机器人参数：机器人的x，y，theta速度、加速度限制、最小转弯半径限制
目标容忍参数：机器人到达目标时允许的位姿误差。
优化参数：迭代次数、各个子目标的权重等。
同伦类参数：是否启动一次规划多个轨迹、是否多线程规划、同伦类的数量等等
恢复参数：解决一些震荡的问题。

接口：从ros服务器中加载参数、动态设置ros服务器中的参数、返回内部配置的互斥锁、检查参数。

=====================================================

PlannerInterface:
类简介：定义了局部规划器的接口。
接口：
virtual plan（）：根据初始的参照规划（位姿序列）、或者起点终点，提供方法去规划和创造一条轨迹。
virtual getVelocityCommand（）：获取用于控制机器人的速度命令
virtual void clearPlanner()：重置规划器。
virtual bool isTrajectoryFeasible（）：通过检测轨迹的一部分是否与障碍碰撞而检查轨迹是否可行。
virtual bool isHorizonReductionAppropriate（）：该方法是在确定规划器提供的轨迹不可行的情况下调用的。在某些情况下，视界长度的缩短可能会解决问题，例如，如果计划的轨道抄近路。由于轨迹表示由规划器管理，所以它是基本planner_interface的一部分。实现是可选的。如果没有指定，该方法返回false。
virtual void computeCurrentCost():返回目前的图优化的花费。

---

TebOptimalPlanner
类简介：这个类使用了g2o优化了时变弹性带的轨迹。
接口：
TebOptimalPlanner() :构造函数，初始化了cfg参数、机器人模型、viapoint和起点终点速度。
～TebOptimalPlanner() ：析构函数，清除图。
plan（）：调用方法去生成和优化轨迹（根据先前产生的轨迹，调用optimizeTEB（）来优化轨迹。
getVelocityCommand（）：若存在规划好的轨迹，则根据位姿、时间间隔求取速度。
optimizeTEB（）：里面有两层循环，分别叫外部循环和内部循环。外部循环通过调用TimedElasticBand::autoResize()来根据时间分辨率调整轨迹。内部循环调用optimizeGraph（）进行优化。外部的循环时间应根据cpu的控制速率要求来定，内部循环的时间经过实验2-6次足够。最后调用 see computeCurrentCost().计算轨迹花费。
setVelocityStart（）：设定轨迹的初始速度。
setObstVector （）：设定障碍用于轨迹规划。
visualize()：发布局部规划pose、footprint模型等。

保护成员函数：主要负责构建图优化问题。
initOptimizer()：初始化优化器。设定线性求解器、块求解器、求解算法等。
buildGraph ():根据超图问题描述构建图优化问题。若顶点和边非空，则add。
optimizeGraph（）：根据之前建好的优化问题，调用优化器optimizer优化图。
AddTEBVertices（）：将timedElasticBand类型中的位姿顶点序列、时间顶点序列依次加入图中。note：添加顺序会影响稀疏矩阵的结构进而影响优化效率。
AddEdgesVelocity()：对于timedElasticBand类型中的位姿顶点序列-1条边，设置该边的顶点和权重矩阵，添加该边。
clearPlanner():清除图，清除时变弹性带轨迹
computeCurrentCost（）：将图中所有边的error的平方和构成cost，存在支持多条轨迹的重载版本。
extractVelocity（）：计算，提取机器人的速度在本体坐标系下的表示。
Compute the velocity profile of the trajectory（）：计算整条轨迹的速度曲线，以vector矢量方式保存。
getFullTrajectory（）：获得整个轨迹的位姿、速度、时间。将其存储在TrajectoryPointMsg类型的vector中。
isHorizonReductionAppropriate():根据一些情况，判断是否建议更短的Horizon。
registerG2OTypes（）：将为TEB定义的顶点和边注册到g2o::Factory。
例如，这允许用户将内部图形导出到文本文件。访问优化器()了解更多细节。

---

HomotopyClassPlanner
类简介：
本地规划器，探索可选的同伦类，为每个可选类创建一个计划，最后返回当前最佳路径的机器人控件(在每个采样间隔中重复)
等价类(例如同伦)是利用搜索图来研究的。
对几个可能的候选项进行采样/生成，然后进行过滤，以便每个同伦类只保留一个候选项。
相关参考论文：
S. Bhattacharya et al.: Search-based Path Planning with Homotopy Class Constraints, AAAI, 2010
C. Rösmann et al.: Planning of Multiple Robot Trajectories in Distinctive Topologies, ECMR, 2015.

此处先不关注此类，有空再看。

========================================================

TebLocalPlannerROS
类简介：
将teb_localplanner插件化，使之能被集成到ROS导航包中去。
私有属性：
指向来自ros的costmap包装器的指针、指向costmap2d的指针、指向tflistener的指针、指向规划器接口的指针、障碍和viapoint序列、参数配置对象、全局规划的序列、各种mutex、目标位姿、一些状态量bool。
公有接口：
TebLocalPlannerROS ()：构造函数，初始化以上属性。
initialize （）：创建roshandle句柄、创建动态参数配置、创建规划器实例、加载costmap插件、订阅障碍和viapoint信息。
setPlan（）：将ros中全局规划的序列传递到这个类中的全局规划序列中保存。
computeVelocityCommands（）：首先获得机器人当前位姿和速度，去掉当前位置前的全局规划，更新viapoints，判断是否到达全局终点，更新障碍，锁住cfg配置参数，调用planner进行规划，根据footprint检查可行性，获得速度命令，判断是否符合约束，速度命令到转向角转化（carlike），可视化轨迹、障碍等。
isGoalReached（）：返回是否到达目标的状态。

静态成员函数
tfPoseToEigenVector2dTransRot（）：将tf::Pose类型转换为包含平移速度和角速度的Eigen::Vector2d。平移速度(x坐标和y坐标)被组合成一个单一的平移速度(第一个分量)。
makeFootprintFromXMLRPC（）：从给定的XmlRpcValue设置footprint.

保护成员函数
updateObstacleContainerWithCostmap（）：根据costmap占有栅格更新障碍序列。note：All occupied cells will be added as point obstacles.
updateObstacleContainerWithCostmapConverter（）：从costmapConverter中提取并更新到障碍序列。
reconfigureCB（）：允许在线修改cfg参数。
pruneGlobalPlan（）：修改全局规划，全局规划器规划出的路径中机器人已经经过的位姿删去。
transformGlobalPlan（）：根据localcostmap地图大小，将此地图内的全局规划器的规划保存到transformed_plan中。transformed_plan所在的坐标系仍然为global_frames
estimateLocalGoalOrientation（）：估计位姿方向局部目标的位姿。将未来一段的规划的姿态角的平局值返回为估计的局部目标姿态。
saturateVelocity（）：若规划出来的速度超过限制速度，则令其等于最大限制速度。
convertTransRotVelToSteeringAngle（）：将旋转平移速度转换为车转角

========================================================
test_optim_node.cpp主程序