TEB TebOptimalPlanner::computeVelocityCommands

1 (teb_local_planner_ros.cpp) computeVelocityCommand

整体跳转流程请看link

整个函数主要跳转到plan的流程如下

Created with Raphaël 2.3.0 (teb_local_planner_ros.cpp) bool TebLocalPlannerROS::computeVelocityCommands (geometry_msgs::Twist& cmd_vel) (teb_local_planner_ros.cpp) bool success = planner_->plan (transformed_plan, &robot_vel_, cfg_.goal_tolerance.free_goal_vel) (optimal_planner.cpp) bool TebOptimalPlanner::plan (const std::vector& initial_plan, const geometry_msgs::Twist* start_vel, bool free_goal_vel)

2 (optimal_planner.cpp) TebOptimalPlanner::plan

Created with Raphaël 2.3.0 TebOptimalPlanner::plan teb_.isInit() 是否初始化 teb_.updateAndPruneTEB 路径裁剪(裁剪teb姿态顶点) setVelocityStart 设置初始顶点运行速度 setVelocityGoalFree 设定目标点速度为0 optimizeTEB　优化路径(正餐) teb_.initTrajectoryToGoal: 转换局部路径到teb 位姿态路径 yes no

bool TebOptimalPlanner::plan(const std::vector<geometry_msgs::PoseStamped>& initial_plan, const geometry_msgs::Twist* start_vel, bool free_goal_vel)
{ 
  //2.1 ****** 初始化  teb_.initTrajectoryToGoal 或者 裁剪 teb_.updateAndPruneTEB ******
  ROS_ASSERT_MSG(initialized_, "Call initialize() first.");
  if (!teb_.isInit())
  {
    // init trajectory
    teb_.initTrajectoryToGoal(initial_plan, cfg_->robot.max_vel_x, cfg_->trajectory.global_plan_overwrite_orientation, cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion);
  } 
  else // warm start
  {
    PoseSE2 start_(initial_plan.front().pose);
    PoseSE2 goal_(initial_plan.back().pose);
    if (teb_.sizePoses()>0
        && (goal_.position() - teb_.BackPose().position()).norm() < cfg_->trajectory.force_reinit_new_goal_dist
        && fabs(g2o::normalize_theta(goal_.theta() - teb_.BackPose().theta())) < cfg_->trajectory.force_reinit_new_goal_angular) // actual warm start!
    
    {
      //裁剪已经有的teb路径 裁剪掉node　顶点 更新起点或者终点
      teb_.updateAndPruneTEB(start_, goal_, cfg_->trajectory.min_samples); // update TEB
    }
    else // goal too far away -> reinit
    {
      ROS_DEBUG("New goal: distance to existing goal is higher than the specified threshold. Reinitalizing trajectories.");
      teb_.clearTimedElasticBand();
      teb_.initTrajectoryToGoal(initial_plan, cfg_->robot.max_vel_x, true, cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion);
    }
  }
  //2.2 ******设定路径初始化速度  setVelocityStart  ******
  if (start_vel)
    setVelocityStart(*start_vel);
  //是否允许机器人以非０速度到达终点
  if (free_goal_vel)
    setVelocityGoalFree();
  else
    vel_goal_.first = true; // we just reactivate and use the previously set velocity (should be zero if nothing was modified)
  
  // now optimize
  // 正戏　构建图优化 hyber graph 顶点图 优化路径
  //2.3 ******构建图进行优化 optimizeTEB*******
  return optimizeTEB(cfg_->optim.no_inner_iterations, cfg_->optim.no_outer_iterations);
}

2.1 初始化 teb_.initTrajectoryToGoal

initTrajectoryToGoal 位于　timed_elastic_band.cpp
主要流程如下

Created with Raphaël 2.3.0 initTrajectoryToGoal addPose(start) 添加起点位置 添加是否可以后退的变量 for循环计算每个路径点上的方向 计算每两个姿态之间的时间间隔，该间隔为距离/最大速度 addPoseAndTimeDiff(intermediate_pose, dt); 添加位姿态与期望的时间 如果位姿态点过少，补点 添加最后目标点

bool TimedElasticBand::initTrajectoryToGoal(const std::vector<geometry_msgs::PoseStamped>& plan, double max_vel_x, bool estimate_orient, int min_samples, bool guess_backwards_motion)
{
  
  if (!isInit())
  {
    PoseSE2 start(plan.front().pose);
    PoseSE2 goal(plan.back().pose);

    double dt = 0.1;
    
    //这个时候teb 路径是空的
    //添加起始点
    addPose(start); // add starting point with given orientation
    setPoseVertexFixed(0,true); // StartConf is a fixed constraint during optimization

    //是否允许后退
    bool backwards = false;
    //通过点积运算判断投影方向
    if (guess_backwards_motion && (goal.position()-start.position()).dot(start.orientationUnitVec()) < 0) // check if the goal is behind the start pose (w.r.t. start orientation)
        backwards = true;
    // TODO: dt ~ max_vel_x_backwards for backwards motions
    
    //路径点方向平滑　平滑插入后续路径上的点　提那家两个位姿态点的时间
    for (int i=1; i<(int)plan.size()-1; ++i)
    {
        //路径点方向平滑
        double yaw;
        if (estimate_orient)
        {
            // get yaw from the orientation of the distance vector between pose_{i+1} and pose_{i}
            //这里前提gloabl_plan 路径一定平滑的，如果是A*路径　一定要先平滑方向
            double dx = plan[i+1].pose.position.x - plan[i].pose.position.x;
            double dy = plan[i+1].pose.position.y - plan[i].pose.position.y;
            yaw = std::atan2(dy,dx);
            if (backwards)
                yaw = g2o::normalize_theta(yaw+M_PI);
        }
        else 
        {
            yaw = tf::getYaw(plan[i].pose.orientation);
        }
        
        PoseSE2 intermediate_pose(plan[i].pose.position.x, plan[i].pose.position.y, yaw);

        //设定两个位姿态之间的最佳期望时间　该时间为距离/最大期望速度
        //dt 其实就是两个点之间的路径　除以最大速度，也就是期望的最短时间　初始化进去
        if (max_vel_x > 0) dt = (intermediate_pose.position()-BackPose().position()).norm()/max_vel_x;
        addPoseAndTimeDiff(intermediate_pose, dt);
    }
    
    // if number of samples is not larger than min_samples, insert manually
    // 如果全局路径点数过少，中间需要插值一些点
    if ( sizePoses() < min_samples-1 )
    {
      ROS_DEBUG("initTEBtoGoal(): number of generated samples is less than specified by min_samples. Forcing the insertion of more samples...");
      while (sizePoses() < min_samples-1) // subtract goal point that will be added later
      {
        // simple strategy: interpolate between the current pose and the goal
        PoseSE2 intermediate_pose = PoseSE2::average(BackPose(), goal);
        if (max_vel_x > 0) dt = (intermediate_pose.position()-BackPose().position()).norm()/max_vel_x;
        addPoseAndTimeDiff( intermediate_pose, dt ); // let the optimier correct the timestep (TODO: better initialization
      }
    }
    
    // Now add final state with given orientation
    //设置最后一个点为固定点
    if (max_vel_x > 0) dt = (goal.position()-BackPose().position()).norm()/max_vel_x;
    addPoseAndTimeDiff(goal, dt);
    setPoseVertexFixed(sizePoses()-1,true); // GoalConf is a fixed constraint during optimization
  }
  else // size!=0
  {
    ROS_WARN("Cannot init TEB between given configuration and goal, because TEB vectors are not empty or TEB is already initialized (call this function before adding states yourself)!");
    ROS_WARN("Number of TEB configurations: %d, Number of TEB timediffs: %d", sizePoses(), sizeTimeDiffs());
    return false;
  }
  
  return true;
}

2.2 路径优化 bool TebOptimalPlanner::optimizeTEB

TebOptimalPlanner::optimizeTEB 位于　optimal_planner.cpp 文件内，是整个teb planner 的核心
主要由以下几个步骤组成

Created with Raphaël 2.3.0 optimizeTEB 循环次数是否到了iterations_outerloop 结束 autoResize 如果有时间过长或者过短的点，将teb路径重新autoresize, buildGraph 构建超图 optimizeGraph 图优化超图 computeCurrentCost 计算cost clearGraph clear超图 yes no

buildGraph

TebOptimalPlanner::buildGraph 位于　optimal_planner.cpp 文件内，是整个teb planner 的核心
主要由以下几个步骤组成

Created with Raphaël 2.3.0 buildGraph 添加位姿态和时间间隔顶点 AddTEBVertices(); 添加障碍物边 AddEdgesObstaclesLegacy(weight_multiplier) 或者 AddEdgesObstacles(weight_multiplier); 添加指定经过的点 AddEdgesViaPoints(); 添加速度边 AddEdgesVelocity(); 添加加速度边 AddEdgesAcceleration() 添加时间约束边 AddEdgesTimeOptimal(); 添加最短路径边 AddEdgesShortestPath(); 添加运动学约束边 AddEdgesKinematicsDiffDrive

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;  
}