TEB算法流程
最近发现了⼀系列关于TEB算法源码讲解的⽂章，⼀共分成了⼗多个篇章，作者描述得很详细，⽂章中还带有中⽂注释的代码，令这些天啃TEB源码弄得头⽪发⿇的我思路清晰了不少，强烈推荐⼤家去看⼀下。广州bim培训
⽂章地址：
下⾯，我也尝试着按照⾃⼰的想法，⼤致地描述下对TEB算法（源码）的浅显理解。
conscience我们要知道ROS官⽅给出的teb_local_planner包中，其实是包含了两种TEB规划器的，⼀个名为optimal_planner ，相对来说⽐较的简单，是本⽂主要阐述的内容。此规划器运⾏后仅会⽣成⼀条路径，在下载后运⾏：
Deactivate parallel planning using the ROS parameter rver (make sure to have a roscore running):
rosparam t /test_optim_node/enable_homotopy_class_planning Fal
Launch test_optim_node in combination with the preconfigured rviz node for visualization:
roslaunch teb_local_planner test_optim_node.launchbarca
在RVIZ界⾯上呈现的，正是此规划器的运⾏效果；
⽽另外⼀种，则名为homotopy_class_planner， 其实就是前⼀种的PLUS版，能规划出多条不同路径，并从中选出最优，适⽤性什么的都要⽐前⼀种好不少，⽬前⼤部分的TEB规划器都是采⽤这⼀种规划。但是算法实现相对复杂⼀点，我⽬前还没进⾏深⼊理解，故不作叙述。最后引⽤⼀下别⼈对这两种规划器的总结
瑞士和瑞典的区别
omotopyClassPlanner是⼀种同时优化多个轨迹的⽅法，由于⽬标函数的⾮凸性(⾮凸函数)会⽣成⼀系列最优的候选轨迹，最终在备选局部解的集合中寻求总体最佳候选轨迹，对应论⽂：《Integrated online trajectory planning and optimization in distinctive topologies》
optimizer本⾝只能找到局部最优轨迹，有时找到的路径invalid，所以⼀般都是⽤HomotopyClassPlanner。
HomotopyClassPlanner类像是多个TebOptimalPlanner类实例的组合。
⾸先，我们把⽬光聚焦到 test_optim_node.cpp，⼀般来说，ROS节点的main函数都是写在这个⽂件，接着按图索骥，就能摸出个所以然。
功能包在node.cpp中，先创建了两个与定时器绑定执⾏的函数，
ros::Timer cycle_timer = n.createTimer(ros::Duration(0.025), CB_mainCycle);
ros::Timer publish_timer = n.createTimer(ros::Duration(0.1), CB_publishCycle);
CB_mainCycle 就是算法的主循环， ⽽ CB_publishCycle 则是更新TEB算法结果显⽰以及障碍物和规划途经点，主程序中，每达到⼀段时间，就会执⾏这两个回调函数。
在node⽂件中main函数的最后，对TEB算法进⾏了实例化：
planner =PlannerInterfacePtr(new TebOptimalPlanner(config,&obst_vector, robot_model, visual,&via_points));
其中，在实例化过程中会创建⼀个TimedElasticBand类的名为teb_对象⾥po_vec_和timediff_vec_对应着g2o
的VertexPo和VertexTimeDiff，轨迹点的坐标以及dt，就是存放在这⾥
//! Container of pos that reprent the spatial part of the trajectory
typedef std::vector<VertexPo*> PoSequence;//这⾥的是typedef，类⾥⾯实际的变量名为po_vec_
//! Container of time differences that define the temporal of the trajectory
typedef std::vector<VertexTimeDiff*> TimeDiffSequence;
⽬光回到 CB_mainCycle 这个函数上
void CB_mainCycle(const ros::TimerEvent& e)
{
planner->plan(PoSE2(-4,0,0),PoSE2(4,0,0));// hardcoded start and goal for testing purpos
}
继续对函数中 “plan” 进⾏深扒。
“plan（）” 先是调⽤teb类中的init相关的函数进⾏初始化，然后调⽤teb_.initTrajectoryToGoal（）初始化轨迹或者热启动
teb_.initTrajectoryToGoal（）流程
（形参为起点终点时）：
1、设置起点并且固定（不允许被优化）。
2、设置到⽬标⼀条直线上平均间隔的点作为初始位姿。
mhs3、设置ds/masvel作为初始时间间隔序列。
4、设置终点并固定。
（形参为位姿序列时）：
1、设置起点并且固定（不允许被优化）。
2、将形参的位姿序列输⼊设置为初始规划位姿。
3、设置ds/masvel作为初始时间间隔序列。
4、设置终点并固定。
然后再调⽤optimizeTEB（）
1、teb_.autoResize（）
2、buildGraph（）;
3、optimized_=ture；代表优化完成。
4、computeCurrentCost（）（只进⾏⼀次）
5、clearGraph（）;
留意到bulidGraph()建⽴超图的过程调⽤的addVertex()传⼊的其实就是teb_对象的PoSequence和timediff_vec_，⽽这两个变量都为指针类型，所以我们可以知道，当执⾏完optimizeTEB（），结果就已经存放在PoSequence中了。
cnn student news这点，从CB_publishCycle ()中的visualize()的publishLocalPlanAndPos()也可以印证
// Visualization loop，位于node.cpp
void CB_publishCycle(const ros::TimerEvent& e)
{
planner->visualize();//显⽰优化后的路径
visual->publishObstacles(obst_vector);//障碍
visual->publishViaPoints(via_points);//途经点
}
//位于optimal_planner.cpp
void TebOptimalPlanner::visualize()
{时装模特
if(!visualization_)
return;
visualization_->publishLocalPlanAndPos(teb_);//显⽰优化后的路径
if(teb_.sizePos()>0)
visualization_->publishRobotFootprintModel(teb_.Po(0),*robot_model_);
if(cfg_->trajectory.publish_feedback)
visualization_->publishFeedbackMessage(*this,*obstacles_);
shuttlecock
}
void TebVisualization::publishLocalPlanAndPos(const TimedElasticBand& teb)const
{
....//略
// fill path msgs with teb configurations
for(int i=0; i < teb.sizePos(); i++)
{
geometry_msgs::PoStamped po;
po.header.frame_id = teb_path.header.frame_id;
po.header.stamp = teb_path.header.stamp;
//就是这⾥，实际上就调⽤了teb的po⽅法，返回的就是对应序列号的po_vec_（PoSE2& Po（）函数）
po.po.position.x = teb.Po(i).x();
examinationquiltpo.po.position.y = teb.Po(i).y();
po.po.position.z = cfg_->hcp.visualize_with_time_as_z_axis_SumOfTimeDiffsUpToIdx(i);
ientation = tf::createQuaternionMsgFromYaw(teb.Po(i).theta());
teb_path.pos.push_back(po);
teb_pos.pos.push_back(po.po);
}
....//略
}
总的来说，就是通过定时器不停地调⽤plan刷新优化路径，同时不断调⽤visualize刷新在RVIZ上的显⽰结果。以上就是我的⼀些粗略见解，如有错误敬请指正。
