最近发现了一系列关于TEB算法源码讲解的文章，一共分成了十多个篇章，作者描述得很详细，文章中还带有中文注释的代码，令这些天啃TEB源码弄得头皮发麻的我思路清晰了不少，强烈推荐大家去看一下。

文章地址：TEB算法

下面，我也尝试着按照自己的想法，大致地描述下对TEB算法（源码）的浅显理解。

我们要知道ROS官方给出的teb_local_planner包中，其实是包含了两种TEB规划器的，一个名为optimal_planner ，相对来说比较的简单，是本文主要阐述的内容。此规划器运行后仅会生成一条路径，在下载官方教程包后运行：

Deactivate parallel planning using the ROS parameter server (make sure to have a roscore running):
rosparam set /test_optim_node/enable_homotopy_class_planning False
Launch test_optim_node in combination with the preconfigured rviz node for visualization:
roslaunch teb_local_planner test_optim_node.launch

在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_对象里pose_vec_和timediff_vec_对应着g2o的VertexPose和VertexTimeDiff，轨迹点的坐标以及dt，就是存放在这里

//! Container of poses that represent the spatial part of the trajectory
typedef std::vector<VertexPose*> PoseSequence;	//这里的是typedef，类里面实际的变量名为pose_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(PoseSE2(-4,0,0), PoseSE2(4,0,0)); // hardcoded start and goal for testing purposes
}

继续对函数中 “plan” 进行深扒。
“plan（）” 先是调用teb类中的init相关的函数进行初始化，然后调用teb_.initTrajectoryToGoal（）初始化轨迹或者热启动

teb_.initTrajectoryToGoal（）流程
（形参为起点终点时）：
1、设置起点并且固定（不允许被优化）。
2、设置到目标一条直线上平均间隔的点作为初始位姿。
3、设置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_对象的PoseSequence和timediff_vec_，而这两个变量都为指针类型，所以我们可以知道，当执行完optimizeTEB（），结果就已经存放在PoseSequence中了。
这点，从CB_publishCycle ()中的visualize()的publishLocalPlanAndPoses()也可以印证

// 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_->publishLocalPlanAndPoses(teb_);	//显示优化后的路径
  if (teb_.sizePoses() > 0)
    visualization_->publishRobotFootprintModel(teb_.Pose(0), *robot_model_);
  if (cfg_->trajectory.publish_feedback)
    visualization_->publishFeedbackMessage(*this, *obstacles_);
}

void TebVisualization::publishLocalPlanAndPoses(const TimedElasticBand& teb) const
{ 
   
	....//略
    
    // fill path msgs with teb configurations
    for (int i=0; i < teb.sizePoses(); i++)
    { 
   
      geometry_msgs::PoseStamped pose;
      pose.header.frame_id = teb_path.header.frame_id;
      pose.header.stamp = teb_path.header.stamp;

	//就是这里，实际上就调用了teb的pose方法，返回的就是对应序列号的pose_vec_（PoseSE2& Pose（）函数）
      pose.pose.position.x = teb.Pose(i).x();
      pose.pose.position.y = teb.Pose(i).y();

      pose.pose.position.z = cfg_->hcp.visualize_with_time_as_z_axis_scale*teb.getSumOfTimeDiffsUpToIdx(i);
      pose.pose.orientation = tf::createQuaternionMsgFromYaw(teb.Pose(i).theta());
      teb_path.poses.push_back(pose);
      teb_poses.poses.push_back(pose.pose);
    }
    ....//略
}

总的来说，就是通过定时器不停地调用plan刷新优化路径，同时不断调用visualize刷新在RVIZ上的显示结果。

以上就是我的一些粗略见解，如有错误敬请指正。

今天的文章TEB算法流程分享到此就结束了，感谢您的阅读，如果确实帮到您，您可以动动手指转发给其他人。