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();//添加时间约束
}