apollo planning学习-Apollo\modules\planning\common\trajectory1d\PiecewiseAccelerationTrajectory1d类代码详解

概述

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

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

从代码来看该类实现的功能非常简单：
PiecewiseAccelerationTrajectory1d多段匀加速轨迹类对象，储存多段加速度不同的匀加速段终点的s,v,a,j，并增删改匀加速段，以及对于多段匀加速段构成的轨迹根据时间t插值出相应的s,v,a,j

piecewise_acceleration_trajectory1d.h

#pragma once

#include <array>
#include <string>
#include <vector>

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

namespace apollo {
namespace planning {

class PiecewiseAccelerationTrajectory1d : public Curve1d {
 public:
  //PiecewiseAccelerationTrajectory1d带参构造函数
  //参数：起始点纵向相对位置start_s，初速度start_v
  PiecewiseAccelerationTrajectory1d(const double start_s, const double start_v);

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

  //AppendSegment顾名思义就是增加一段匀加速段
  //参数加速度a，持续时间t_duration
  void AppendSegment(const double a, const double t_duration);
  //删除最后一个点，也就是在vector s_,v_,a_,t_里删去最后一段匀加速段的s,v,a,t
  void PopSegment();
  //返回分段匀加速轨迹类对象所覆盖的时间总长度
  double ParamLength() const override;
  //把数据成员v_,a_,s_,t_转化成字符串并连接起来返回
  std::string ToString() const override;
  //Evaluate插值函数
  //参数order阶数，param是时间，根据时间插值s,v,a,jerk
  //s_,v_,a_,t_储存了多段加速度不同的匀加速度段
  double Evaluate(const std::uint32_t order, const double param) const override;
  //多段匀加速段轨迹根据时间t插值对应的是s,v,a,j
  //返回值是一个4维的double数组
  std::array<double, 4> Evaluate(const double t) const;

 private:
  //下面主要是对数据成员根据时间进行插值
  double Evaluate_s(const double t) const;

  double Evaluate_v(const double t) const;

  double Evaluate_a(const double t) const;

  double Evaluate_j(const double t) const;

 private:
  //下面都是类的数据成员，其实就是vector的s,v,a,t
  // accumulated s
  std::vector<double> s_;

  std::vector<double> v_;

  // accumulated t
  std::vector<double> t_;

  std::vector<double> a_;
};

}  // namespace planning
}  // namespace apollo

piecewise_acceleration_trajectory1d.cc

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

#include <algorithm>

#include "absl/strings/str_cat.h"
#include "absl/strings/str_join.h"

#include "cyber/common/log.h"
#include "modules/common/math/linear_interpolation.h"
#include "modules/planning/common/planning_gflags.h"

namespace apollo {
namespace planning {

//PiecewiseAccelerationTrajectory1d带参构造函数
//参数：起始点纵向相对位置start_s，初速度start_v
PiecewiseAccelerationTrajectory1d::PiecewiseAccelerationTrajectory1d(
    const double start_s, const double start_v) {
  //用参数来初始化类数据成员s,v,a,t 
  //其中a,t初始化塞入0.0
  s_.push_back(start_s);
  v_.push_back(start_v);
  a_.push_back(0.0);
  t_.push_back(0.0);
}

//AppendSegment顾名思义就是增加一段匀加速段
//参数加速度a，持续时间t_duration
void PiecewiseAccelerationTrajectory1d::AppendSegment(const double a,
                                                      const double t_duration) {
  //定义初始的纵向相对位置s0为类数据成员s_里的最后一个(上一段的终点位置)
  double s0 = s_.back();
  //定义初速度v0为类数据成员v_里的最后一个(上一段的终点速度)
  double v0 = v_.back();
  //定义初始点的时间t0为类数据成员t_里的最后一个(上一段的终点时间)
  double t0 = t_.back();

  //新增的这段匀加速运动的终点速度v1
  double v1 = v0 + a * t_duration;
  //检查v1 >= 1e-6?  否则报错
  //FLAGS_numerical_epsilon是gflags的用法FLAGS_去
  //modules\planning\common\planning_gflags.cc取出numerical_epsilon的值1e-6
  ACHECK(v1 >= -FLAGS_numerical_epsilon);

  //定义新增的匀加速段走过的纵向距离delta_s 计算公式高中物理公式
  double delta_s = (v0 + v1) * t_duration * 0.5;
  //定义新增的匀加速段终点的纵向相对位置s1
  double s1 = s0 + delta_s;
  //定义新增的匀加速段的终点时间t1
  double t1 = t0 + t_duration;

  //检查新增的匀加速段终点的纵向相对位置s1是否大于初始的纵向相对位置s0，否则报错
  ACHECK(s1 >= s0 - FLAGS_numerical_epsilon);
  //s1取s1,s0里较大的那一个，这个是为了防止一些错误的清空发生
  s1 = std::max(s1, s0);
  //将新增的匀加速段的终点s,v,a,t塞入类数据成员
  //s_,v_,a_,t_都是vector
  s_.push_back(s1); 
  v_.push_back(v1);
  a_.push_back(a);
  t_.push_back(t1);
}

//删除最后一个点，也就是在vector s_,v_,a_,t_里删去最后一段匀加速段的s,v,a,t
void PiecewiseAccelerationTrajectory1d::PopSegment() {
  if (a_.size() > 0) {
    s_.pop_back();
    v_.pop_back();
    a_.pop_back();
    t_.pop_back();
  }
}

//返回分段匀加速轨迹类对象所覆盖的时间总长度
double PiecewiseAccelerationTrajectory1d::ParamLength() const {
  CHECK_GT(t_.size(), 1U);
  return t_.back() - t_.front();
}

//把数据成员v_,a_,s_,t_转化成字符串并连接起来返回
std::string PiecewiseAccelerationTrajectory1d::ToString() const {
  return absl::StrCat(absl::StrJoin(s_, "\t"), absl::StrJoin(t_, "\t"),
                      absl::StrJoin(v_, "\t"), absl::StrJoin(a_, "\t"), "\n");
}

//Evaluate插值函数
//参数order阶数，param是时间，根据时间插值s,v,a,jerk
//s_,v_,a_,t_储存了多段加速度不同的匀加速度段
double PiecewiseAccelerationTrajectory1d::Evaluate(const std::uint32_t order,
                                                   const double param) const {
  CHECK_GT(t_.size(), 1U);
  ACHECK(t_.front() <= param && param <= t_.back());

  switch (order) {
    case 0:
      return Evaluate_s(param);
    case 1:
      return Evaluate_v(param);
    case 2:
      return Evaluate_a(param);
    case 3:
      return Evaluate_j(param);
  }
  return 0.0;
}
//多段匀加速段轨迹根据时间t插值对应的纵向相对位置s
double PiecewiseAccelerationTrajectory1d::Evaluate_s(const double t) const {
  //先根据时间t找到其在t_中所处的index从而确定所处区间然后进行插值
  auto it_lower = std::lower_bound(t_.begin(), t_.end(), t);
  auto index = std::distance(t_.begin(), it_lower);

  double s0 = s_[index - 1];
  double v0 = v_[index - 1];
  double t0 = t_[index - 1];

  double v1 = v_[index];
  double t1 = t_[index];

  double v = common::math::lerp(v0, t0, v1, t1, t);
  double s = (v0 + v) * (t - t0) * 0.5 + s0;
  return s;
}
//多段匀加速段轨迹根据时间t插值对应的速度v
double PiecewiseAccelerationTrajectory1d::Evaluate_v(const double t) const {
  auto it_lower = std::lower_bound(t_.begin(), t_.end(), t);
  auto index = std::distance(t_.begin(), it_lower);

  double v0 = v_[index - 1];
  double t0 = t_[index - 1];

  double v1 = v_[index];
  double t1 = t_[index];

  double v = apollo::common::math::lerp(v0, t0, v1, t1, t);
  return v;
}
//多段匀加速段轨迹根据时间t插值对应的速度a
double PiecewiseAccelerationTrajectory1d::Evaluate_a(const double t) const {
  auto it_lower = std::lower_bound(t_.begin(), t_.end(), t);
  auto index = std::distance(t_.begin(), it_lower);
  return a_[index - 1];
}
//多段匀加速段轨迹根据时间t插值对应的加加速度jerk，因为是多段匀加速度段，jerk=0.0
double PiecewiseAccelerationTrajectory1d::Evaluate_j(const double t) const {
  return 0.0;
}

//多段匀加速段轨迹根据时间t插值对应的是s,v,a,j
//返回值是一个4维的double数组
std::array<double, 4> PiecewiseAccelerationTrajectory1d::Evaluate(
    const double t) const {
  CHECK_GT(t_.size(), 1U);
  ACHECK(t_.front() <= t && t <= t_.back());

  auto it_lower = std::lower_bound(t_.begin(), t_.end(), t);
  auto index = std::distance(t_.begin(), it_lower);

  double s0 = s_[index - 1];
  double v0 = v_[index - 1];
  double t0 = t_[index - 1];

  double v1 = v_[index];
  double t1 = t_[index];

  double v = common::math::lerp(v0, t0, v1, t1, t);
  double s = (v0 + v) * (t - t0) * 0.5 + s0;

  double a = a_[index - 1];
  double j = 0.0;

  return {{s, v, a, j}};
}

}  // namespace planning
}  // namespace apollo