teb优化轨迹流程

概述

teb源码分析中，我大概了解到了teb算法的底层实现过程，涉及到弹性带定义、初始化、g2o优化、各种参数配置。但是我对于teb的轨迹优化流程不甚明了。
弹性带轨迹存放在哪里？何时、如何初始化？何时优化？多久优化一次？如何将costmap的障碍进行表示？movebase多久调用一次teb？怀揣着这些问题，读源码了解调用流程。

movebase对teb_local_planner_ros类功能的调用

tc_是teb_local_planner_ros插件类的实例。
1、tc_->initialize(blp_loader_.getName(config.base_local_planner), &tf_, controller_costmap_ros_);
2、tc_->setPlan(*controller_plan_)
3、tc_->isGoalReached()
4、tc_->computeVelocityCommands(cmd_vel)

teb_local_planner_ros对底层teb算法调用过程

这个类是对ros中局部规划接口的继承，使teb包能够融入到movebase中。
initialize（）对底层teb调用：
cfg_.loadRosParamFromNodeHandle(nh); 加载参数
配置障碍、可视化、获取costmap、获取全局、局部坐标系名称。
costmap_converter_。
获得机器人footprint、订阅自定义障碍、waypoint。

comuputeVelocityCommands（）对底层teb调用：
1、pruneGlobalPlan(*tf_, robot_pose, global_plan_);
2、transformGlobalPlan（）提取局部地图内的全局规划。提取后的计划所在坐标系仍然为global_frame.
3、updateViaPointsContainer()
4、configureBackupModes(transformed_plan, goal_idx);
5、estimateLocalGoalOrientation(global_plan_, goal_point, goal_idx, tf_plan_to_global);
6、updateObstacleContainerWithCostmap();
7、planner_->plan(transformed_plan, &robot_vel_, cfg_.goal_tolerance.free_goal_vel);
8、planner_->isTrajectoryFeasible（）
9、planner_->getVelocityCommand(cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z)
10、saturateVelocity（）;过滤较大的速度
11、 planner_->visualize();
visualization_->publishObstacles(obstacles_);
visualization_->publishViaPoints(via_points_);
visualization_->publishGlobalPlan(global_plan_);

底层teb算法流程

这里涉及到好几个类及其方法，只提及比较重要的：

TimedElasticBand teb_;存放轨迹

plan（）流程：
1、teb_.initTrajectoryToGoal（）初始化轨迹或者热启动。
2、optimizeTEB（）优化轨迹

teb_.initTrajectoryToGoal（）流程（形参为起点终点时）：
1、设置起点并且固定（不允许被优化）。
2、设置到目标一条直线上平均间隔的点作为初始位姿。
3、设置ds/masvel作为初始时间间隔序列。
4、设置终点并固定。

teb_.initTrajectoryToGoal（）流程（形参为位姿序列时）：
1、设置起点并且固定（不允许被优化）。
2、将形参的位姿序列输入设置为初始规划位姿。
3、设置ds/masvel作为初始时间间隔序列。
4、设置终点并固定。

备注：
当只设定一次goal时，第一次轨迹生成是起点终点插值，当机器人位置变化时，初始轨迹是删去已经走过的，保留后续的。
当重新设定较远的goal时，则首先clear轨迹序列，然后重新起点终点插值。
热启动流程：
teb_.updateAndPruneTEB（）流程：
作用：取当前位置为起点，删去已经走过的轨迹，并重新设定终点。
1、删减去位姿与时间序列中在新起点之前的位姿和时间。
2、将新的终点加入序列。

optimizeTEB（）流程：
循环几次下列操作（多次图优化）：
1、teb_.autoResize（）
2、buildGraph（）;
3、optimized_=ture;代表优化完成。
4、computeCurrentCost（）（只进行一次）
5、clearGraph（）;