本文是对上一篇的补充

TimedElasticBand在timed_elastic_band.h，此类定义了一条时变弹性带的轨迹模型，只有两个protected成员

//Internal container storing the sequence of optimzable pose vertices
PoseSequence pose_vec_;
// Internal container storing the sequence of optimzable timediff vertices
TimeDiffSequence timediff_vec_;


// Container of poses that represent the spatial(空间的) part of the trajectory
typedef std::vector<VertexPose*> PoseSequence;
// Container of time differences that define the temporal(时间的) of the trajectory
typedef std::vector<VertexTimeDiff*> TimeDiffSequence;

类VertexPose是g2o类型，在g2o_types\vertex_pose.h，保存和封装SE2 pose (position and orientation) into a vertex(图顶点)，用于g2o的图优化。 类VertexTimeDiff是g2o类型，在g2o_types\vertex_timediff.h，保存和封装时间差deltaT为一个vertex，用于g2o的图优化.

在TEB初始化的过程中，将起点的Pose与终点的Pose所在的顶点设为固定的，使得g2o不对这两个Pose进行优化。

初始化某一个TEB时，其路线简单的由起点和终点的连线组成。在这条直线上均匀采样点作为待优化的顶点。采样的步长由参数min_samples决定。而 timediff 顶点的初始值为 步长除以参数max_vel_x。

每有一个pose顶点就产生一个time_diff顶点，time_diff顶点实际上是每两个Pose之间所需要的时间。

初始化轨迹 initTrajectoryToGoal

以指定的plan(或者起点和终点)进行插值产生的轨迹

bool TimedElasticBand::initTrajectoryToGoal(
            const std::vector<geometry_msgs::PoseStamped>&  plan, 
            double  max_vel_x, double  max_vel_theta, bool  estimate_orient, 
            int  min_samples, bool  guess_backwards_motion )
{
  if (!isInit()) 	// timediff_vec_  pose_vec_ 都是空
  {
    PoseSE2 start(plan.front().pose);
    PoseSE2 goal(plan.back().pose);
     //   double dt = 0.1;
    addPose(start); // 向 pose_vec_ 添加 起点   此时的 BackPose()是 start
    //StartConf 称为固定约束，第一个参数是 index
    setPoseVertexFixed(0,true);  // pose_vec_.at(index)->setFixed(status);   

    bool backwards = false;
    // 参数 guess_backwards_motion 在这里
    // check if the goal is behind the start pose (w.r.t. start orientation)
    if ( guess_backwards_motion && 
    ( goal.position()-start.position()).dot(start.orientationUnitVec()) < 0)
        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}
            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);
        /* 根据最大线速度和角速度，估计从 BackPose 到 intermediate_pose所花的时间，
          假设匀速运动。也就是说这是估算了一个最小值  */
        double dt = estimateDeltaT(BackPose(), intermediate_pose, max_vel_x, max_vel_theta);
        // intermediate_pose添加到 pose_vec_, 约束false, 它成为BackPose()
        // dt 添加到 timediff_vec_ ,  约束false
        addPoseAndTimeDiff(intermediate_pose,  dt);
    }
    // number of samples 小于参数 min_samples, insert manually
    // -1 的意思是不包含goal point，它在之后添加
    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)
      {
        // 策略: interpolate between the current pose and the goal
        // 只处理了 BackPose() 到 goal 这一段，而且是逐步二分
        PoseSE2 intermediate_pose = PoseSE2::average(BackPose(), goal);
        double dt = estimateDeltaT(BackPose(), intermediate_pose, max_vel_x, max_vel_theta);
        // let the optimier correct the timestep (TODO: better initialization
        addPoseAndTimeDiff( intermediate_pose, dt ); 
      }
    }
    // Now add final state with given orientation
    double dt = estimateDeltaT(BackPose(), goal, max_vel_x, max_vel_theta);
    // goal 也插入 pose_vec_, 成为 BackPose()
    addPoseAndTimeDiff(goal, dt);
    // GoalConf 也成为了固定约束
    setPoseVertexFixed(sizePoses()-1,true); 
  }
  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",
       (unsigned int) sizePoses(), (unsigned int) sizeTimeDiffs());
    return false;
  }
  return true;
}

1. 设置起点并且固定（不允许被优化）。
2. 设置到目标一条直线上平均间隔的点作为初始位姿。
3. 设置初始的时间间隔序列。
4. 设置终点并固定（不允许被优化）。

注意： dt的单位是秒，不是时间戳，是路径点之间的时间差。所有点都插入 pose_vec_，但是除起点外的timestep都插入 timediff_vec_

热启动 updateAndPruneTEB

更新和改变轨迹，即根据现在的位置更新起点（vector中的第一个点），改变终点。调用 teb_.updateAndPruneTEB(start, goal, cfg_->trajectory.min_samples);

// new_start new_goal 的根源都在 transformed_plan
void TimedElasticBand::updateAndPruneTEB(boost::optional<const PoseSE2&> new_start, 
  boost::optional<const PoseSE2&> new_goal,   int min_samples)
{
  // first and simple approach: change only start confs(and virtual start conf for inital velocity)
  // TEST if optimizer can handle this "hard" placement
  if ( new_start && sizePoses()>0 )
  {
    // find nearest state (using L2-norm) in order to prune the trajectory
    // (remove already passed states)
    double dist_cache = (new_start->position()- Pose(0).position()).norm();
    double dist;
    // satisfy min_samples, otherwise max 10 samples
    int lookahead = std::min<int>( sizePoses()-min_samples, 10); 

    int nearest_idx = 0;
    for (int i = 1; i<=lookahead; ++i)
    {
      dist = (new_start->position()- Pose(i).position() ).norm();
      if (dist < dist_cache)
      {
        dist_cache = dist;
        nearest_idx = i;
      }
      else break;
    }
    // prune trajectory at the beginning (and extrapolate 
    //sequences at the end if the horizon is fixed )
    if (nearest_idx>0)
    {
      // nearest_idx is equal to the number of samples to be removed (since it counts from 0 )
      // WARNING delete starting at pose 1, and overwrite the original pose(0) with 
      // new_start, since Pose(0) is fixed during optimization!

      // delete first states such that the closest state is the new first one
      deletePoses(1, nearest_idx);  
      deleteTimeDiffs(1, nearest_idx); // delete corresponding time differences
    }
    // update start
    Pose(0) = *new_start;
  }
  if (new_goal && sizePoses()>0)
  {
    BackPose() = *new_goal;
  }
};

当机器人位置变化时，初始轨迹是删去已经走过的，保留后续的。

作用：

1. 取当前位置为起点，删去已经走过的轨迹，并重新设定终点。
2. 对于位姿与时间序列，删减去新起点之前的位姿和时间，也就是删除两个容器中旧的元素
3. 将新的终点加入序列