teb_local_planner 理解

1、算法流程图 

                              

２、代码入口

teb_local_planner_ros.cpp    

bool TebLocalPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)

bool success = planner_->plan(transformed_plan, &robot_vel_, cfg_.goal_tolerance.free_goal_vel);

return optimizeTEB(cfg_->optim.no_inner_iterations, cfg_->optim.no_outer_iterations);

success = buildGraph(weight_multiplier);  //对应上面的Generate hyper-graph

success = optimizeGraph(iterations_innerloop, false); //对应上面的Optimize hyper-graph

TebOptimalPlanner::buildGraph(double weight_multiplier){

    AddTEBVertices(); //增加图的顶点，轨迹的路径点和dt为顶点
    legacy_obstacle_association 
/*已经修改了将轨迹姿势与优化障碍联系起来的策略（参见changelog）。 
您可以通过将此参数设置为true来切换到旧/先前策略。 
旧策略：对于每个障碍，找到最近的TEB姿势; 新策略：对于每个teb姿势，只找到“相关”的障碍。*/
    AddEdgesObstacles();//添加障碍物约束
    AddEdgesDynamicObstacles();//添加动态障碍物约束
    AddEdgesViaPoints();//添加全局路径约束
    AddEdgesVelocity();//添加速度约束
    AddEdgesAcceleration();//添加加速度约束
    AddEdgesTimeOptimal();//添加时间约束
}

//订阅动态障碍物
custom_obst_sub_ = nh.subscribe("obstacles", 1, &TebLocalPlannerROS::customObstacleCB, this);

void TebOptimalPlanner::AddEdgesObstacles(double weight_multiplier);
//与
void TebOptimalPlanner::AddEdgesDynamicObstacles(double weight_multiplier);
//中的obstacles_来自订阅的动态障碍物和代价地图

obstacles_来源于三部分
//来自代价地图
TebLocalPlannerROS::updateObstacleContainerWithCostmap()｛
obstacles_.push_back(ObstaclePtr(new PointObstacle(obs)));
｝
//来自CostmapConverter
void TebLocalPlannerROS::updateObstacleContainerWithCostmapConverter()
//来自用户发布的障碍物消息
void TebLocalPlannerROS::updateObstacleContainerWithCustomObstacles()

include_dynamic_obstacles 参数作用
if (cfg_->obstacles.include_dynamic_obstacles)
    AddEdgesDynamicObstacles();

include_costmap_obstacles 参数作用
void TebLocalPlannerROS::updateObstacleContainerWithCostmap()
{  
  // Add costmap obstacles if desired
  if (cfg_.obstacles.include_costmap_obstacles)
  {
  }
}