TEB局部路径规划 buildGraph

就是用g2o建立各种约束方程

bool TebOptimalPlanner::buildGraph(double weight_multiplier)
{
    
    
  if (!optimizer_->edges().empty() || !optimizer_->vertices().empty())
  {
    
    
    ROS_WARN("Cannot build graph, because it is not empty. Call graphClear()!");
    return false;
  }
  
  // add TEB vertices 添加节点，一种是位姿节点，一种是时间差节点
  AddTEBVertices();
  
  // add Edges (local cost functions) //添加静态障碍物约束，不确定因为我不考虑障碍物
  if (cfg_->obstacles.legacy_obstacle_association)
    AddEdgesObstaclesLegacy(weight_multiplier);
  else
    AddEdgesObstacles(weight_multiplier);
  //增加动态障碍物约束
  if (cfg_->obstacles.include_dynamic_obstacles)
    AddEdgesDynamicObstacles();
  //增加寻迹约束
  AddEdgesViaPoints();
  //增加速度约束
  AddEdgesVelocity();
  //增加加速度约束
  AddEdgesAcceleration();
  //增加时间约束
  AddEdgesTimeOptimal();	
  //增加最短路径约束
  AddEdgesShortestPath();
  //增加动力学约束
  if (cfg_->robot.min_turning_radius == 0 || cfg_->optim.weight_kinematics_turning_radius == 0)
    AddEdgesKinematicsDiffDrive(); // we have a differential drive robot
  else
    AddEdgesKinematicsCarlike(); // we have a carlike robot since the turning radius is bounded from below.

  //增加方向约束，是经常向左还是经常向右，应该是这个意思
  AddEdgesPreferRotDir();
    
  return true;  
}