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。