百度apollo自动驾驶planning代码学习-Apollo\modules\planning\common\trajectory1d\PiecewiseTrajectory1d类代码详解

piecewise_trajectory1d.cc

概述

对piecewise_trajectory1d.cc和.h进行解析，来了解PiecewiseTrajectory1d类的使用。

从名字字面意义来看是 分段1维轨迹类。

从代码来看该类实现的功能还是比较简单的：
可以储存我其他博客之前介绍的
PiecewiseJerkTrajectory1d类
PiecewiseAccelerationTrajectory1d类
ConstantJerkTrajectory1d类
ConstantDecelerationTrajectory1d类
分别是：多段匀加加速度运动1维轨迹(每一段的加加速度不同，但都是匀加加速运动)，多段匀加速运动1维轨迹，匀加加速度运动1维轨迹，匀加速运动1维轨迹。
这四类的轨迹都可以储存到PiecewiseTrajectory1d类对象中，并且可以对轨迹段进行增删改，并根据纵向相对位置等进行插值计算s,t,v,a,j等.

piecewise_trajectory1d.h

#pragma once

#include <memory>
#include <string>
#include <vector>

#include "modules/planning/math/curve1d/curve1d.h"

namespace apollo {
namespace planning {

class PiecewiseTrajectory1d : public Curve1d {
 public:
  //默认构造函数
  PiecewiseTrajectory1d() = default;

  //默认析构函数
  virtual ~PiecewiseTrajectory1d() = default;

  //插值函数，参数order阶数 0阶代表位置插值，1阶代表速度插值...
  //param是纵向相对位置(仅仅是相对本段轨迹的纵向位置，和后面的插值s不一样)，根据时间插
  //值相应的s,t,v,a,j
  double Evaluate(const std::uint32_t order, const double param) const;

  //返回分段1维轨迹类的走过的总长度
  double ParamLength() const;

  //返回轨迹名称，默认为空
  std::string ToString() const;

  //增加一段1维轨迹，参数是1维曲线类对象trajectory
//其可以是我之前博客提到的匀加速，匀加加速度，分段加速，分段加加速轨迹
  void AppendSegment(const std::shared_ptr<Curve1d> trajectory);
  
  //删除PiecewiseTrajectory1d分段1维轨迹类对象里存储的最后一段轨迹
  void PopSegment();

  //返回该分段1维轨迹类对象所包含的轨迹段的数量
  size_t NumOfSegments() const;

 private:
  std::vector<std::shared_ptr<Curve1d>> trajectory_segments_;

  std::vector<double> accumulated_param_lengths_;
};

}  // namespace planning
}  // namespace apollo

piecewise_trajectory1d.cc

#include "modules/planning/common/trajectory1d/piecewise_trajectory1d.h"

#include <cmath>

#include "cyber/common/log.h"

namespace apollo {
namespace planning {
//插值函数，参数order阶数 0阶代表位置插值，1阶代表速度插值...
//param是纵向相对位置，根据时间插值相应的t,v,a,j
double PiecewiseTrajectory1d::Evaluate(const std::uint32_t order,
                                       const double param) const {
  //确定给定的纵向相对位置所处在vector accumulated_param_lengths_里所处区间
  //it_lower就是在数据成员accumulated_param_lengths_里找到刚好小于给定param的
  //元素，it_lower就是这个刚好小于的纵向相对位置
  auto it_lower = std::lower_bound(accumulated_param_lengths_.begin(),
                                   accumulated_param_lengths_.end(), param);
  //检查这个刚好小于的纵向相对位置it_lower < accumulated_param_lengths_里的最后一个
  //元素,就是这个刚好小于的纵向位置it_lower不能大于这个一维轨迹的终点纵向位置。
  ACHECK(it_lower != accumulated_param_lengths_.end());

  size_t index = (size_t)(it_lower - accumulated_param_lengths_.begin());

  double param_overhead = 0.0;
  if (index != 0) {
    param_overhead = accumulated_param_lengths_[index - 1];
  }
  return trajectory_segments_[index]->Evaluate(order, param - param_overhead);
}

//返回分段1维轨迹类的走过的总长度
double PiecewiseTrajectory1d::ParamLength() const {
  if (accumulated_param_lengths_.empty()) {
    return 0.0;
  }
  return accumulated_param_lengths_.back();
}

//返回轨迹名称，默认为空
std::string PiecewiseTrajectory1d::ToString() const { return ""; }

//增加一段1维轨迹，参数是1维曲线类对象trajectory
//其可以是我之前博客提到的匀加速，匀加加速度，分段加速，分段加加速轨迹
void PiecewiseTrajectory1d::AppendSegment(
    const std::shared_ptr<Curve1d> trajectory) {
  if (trajectory_segments_.empty()) {//若数据成员trajectory_segments_是空的
   									 //之前未存放任何轨迹，直接塞入trajectory
    trajectory_segments_.push_back(trajectory);
  } else {//若数据成员trajectory_segments_非空
    //按纵向位置0.0插值s,v,a,j求到本段轨迹起点信息，这个param都是相对本段轨迹的
    //param代表s s=0处代表本段轨迹起点，param是局部参数，但是s1是相对于整个轨迹的
    double s1 = trajectory->Evaluate(0, 0.0);
    double v1 = trajectory->Evaluate(1, 0.0);
    double a1 = trajectory->Evaluate(2, 0.0);
    double j1 = trajectory->Evaluate(3, 0.0);

    //取出类成员trajectory_segments_中最后一段轨迹last_trajectory
    auto last_trajectory = trajectory_segments_.back();
    //类成员trajectory_segments_中最后一段轨迹last_trajectory的长度
    double last_param_length = last_trajectory->ParamLength();
    //根据上一段轨迹的终点相对纵向位置插值上一段终点的s,v,a,j等信息
    double s0 = last_trajectory->Evaluate(0, last_param_length);
    double v0 = last_trajectory->Evaluate(1, last_param_length);
    double a0 = last_trajectory->Evaluate(2, last_param_length);
    double j0 = last_trajectory->Evaluate(3, last_param_length);

    //若上一段轨迹的终点s坐标和本段轨迹的起点的s坐标差值>1e-4，则警告轨迹0阶不连续
    if (std::fabs(s0 - s1) > 1.0e-4) {
      AWARN << "The appended segment is not smooth in order 0";
    }
	//若上一段轨迹的终点v1和本段轨迹的起点的v0差值>1e-4，则警告轨迹1阶不连续
    if (std::fabs(v0 - v1) > 1.0e-4) {
      AWARN << "The appended segment is not smooth in order 1";
    }
    //若上一段轨迹的终点a1和本段轨迹的起点的a0差值>1e-4，则警告轨迹2阶不连续
    if (std::fabs(a0 - a1) > 1.0e-4) {
      AWARN << "The appended segment is not smooth in order 2";
    }
    //若上一段轨迹的终点j1和本段轨迹的起点的j0差值>1e-4，则警告轨迹3阶不连续
    if (std::fabs(j0 - j1) > 1.0e-4) {
      AWARN << "The appended segment is not smooth in order 3";
    }
    //将本段轨迹塞入类数据成员trajectory_segments_里
    //trajectory_segments_是一维轨迹类Curve1d的vector
    //之前博客介绍的匀加速段1维轨迹类，匀加加速1维轨迹类等都是Curve1d的派生类，可以被
    //加到trajectory_segments_里，trajectory_segments_就是用来存储多段轨迹的
    trajectory_segments_.push_back(trajectory);
  }

  //获取本段轨迹的总长度
  double accumulated_param_length = trajectory->ParamLength();
  if (accumulated_param_lengths_.size() > 0) {
    accumulated_param_length += accumulated_param_lengths_.back();
  }
  //在类数据成员accumulated_param_lengths_中新增本段轨迹终点对应的纵向相对位置
  accumulated_param_lengths_.push_back(accumulated_param_length);
}

//删除PiecewiseTrajectory1d分段1维轨迹类对象里存储的最后一段轨迹
void PiecewiseTrajectory1d::PopSegment() {
  if (trajectory_segments_.empty()) {
    return;
  }
  trajectory_segments_.pop_back();
  accumulated_param_lengths_.pop_back();
}

//返回该分段1维轨迹类对象所包含的轨迹段的数量
size_t PiecewiseTrajectory1d::NumOfSegments() const {
  return trajectory_segments_.size();
}

}  // namespace planning
}  // namespace apollo