百度apollo自动驾驶planninApollo\modules\planning\common\trajectory1d\ConstantDecelerationTrajectory1d类代码解析

概述

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

从名字字面意义来看是 常数减速度1维轨迹类。

从代码来看该类实现的功能非常简单，就是根据车辆初始速度，初始纵向位置s，计算出车辆以加速度a匀减速至0，其末端纵向位置，所需时间，以及在匀减速过程中针对s,v,a,jerk根据时间t的插值。

constant_deceleration_trajectory1d.h

#pragma once

#include <string>

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

namespace apollo {
namespace planning {

//封装了一个常数减速度1维轨迹类
class ConstantDecelerationTrajectory1d : public Curve1d {
 public:
  //带参构造函数，参数初始纵向位置init_s,初始速度init_v，还有减速度的绝对值a
  //结合下面的.cc文件代码解析来看，就是根据给定的init_s，init_v，a从而确定
  //常数减速度1维轨迹类的数据成员end_t_，end_s_
  ConstantDecelerationTrajectory1d(const double init_s, const double init_v,
                                   const double a);
  //默认析构函数
  virtual ~ConstantDecelerationTrajectory1d() = default;

  //返回匀减速至0需要的总时间
  double ParamLength() const override;

  //返回一维匀减速轨迹类的名称字符串，默认为空
  std::string ToString() const override;

  //定义一个抽象的插值模板，参数order为阶数，param基本为时间t
  //order = 0 插值0阶就等效于Evaluate_s
  //order = 1 插值1阶就等效于Evaluate_v
  //order = 2 插值2阶就等效于Evaluate_a
  //order = 3 插值3阶就等效于Evaluate_j
  double Evaluate(const std::uint32_t order, const double param) const override;

 private:
  //Evaluate是Apollo数据结构里的老生常谈——插值，根据时间t求得对应匀减速段的s
  double Evaluate_s(const double t) const;

  //Evaluate是Apollo数据结构里的老生常谈——插值，根据时间t求得对应匀减速段的v
  double Evaluate_v(const double t) const;

  //Evaluate是Apollo数据结构里的老生常谈——插值，根据时间t求得对应匀减速段的a，其实就
  //是一常值
  double Evaluate_a(const double t) const;

  //Evaluate是Apollo数据结构里的老生常谈——插值，根据时间t求得对应匀减速段的jerk
  //既然匀减速段，自然jerk恒为0
  double Evaluate_j(const double t) const;

  //以下就是数据成员初始相对纵向位置，初始速度，减速度绝对值，匀减速至0所需的总时间
  double init_s_;

  double init_v_;

  double deceleration_;

  double end_t_;

  double end_s_;
};

}  // namespace planning
}  // namespace apollo

constant_deceleration_trajectory1d.cc

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

#include <cmath>

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

namespace apollo {
namespace planning {
//带参构造函数，参数初始纵向相对位置init_s,初始速度init_v，还有减速度a
ConstantDecelerationTrajectory1d::ConstantDecelerationTrajectory1d(
    const double init_s, const double init_v, const double a)
    : init_s_(init_s), init_v_(init_v), deceleration_(-a) {
  //可以看到三个参数是用来初始化类数据成员init_s_，init_v_，deceleration_
  //deceleration_是代表减速度的绝对值，是一个正数
  //FLAGS_numerical_epsilon是去modules\planning\common\planning_gflags.cc文件里取
  //出numerical_epsilon的值，也就是1e-6
  if (init_v_ < -FLAGS_numerical_epsilon) {
    //若初始速度小于-0.000001，则报错具有负的初速度
    AERROR << "negative init v = " << init_v_;
  }
  //然后对初始速度取绝对值
  init_v_ = std::fabs(init_v_);
  //检查此时数据成员deceleration_ 是否为正？应为正
  ACHECK(deceleration_ > 0.0);
  //终点时间 = 初始速度 / 减速度绝对值
  end_t_ = init_v_ / deceleration_;
  //终点的相对纵向位置(匀减速走过的距离) = (v^2 - v0^2)/(2a) + s0 高中物理公式
  end_s_ = init_v_ * init_v_ / (2.0 * deceleration_) + init_s_;
}

//Evaluate是Apollo数据结构里的老生常谈——插值，根据时间t求得对应匀减速段的s
double ConstantDecelerationTrajectory1d::Evaluate_s(const double t) const {
  if (t < end_t_) {
    double curr_v = init_v_ - deceleration_ * t;
    double delta_s = (curr_v + init_v_) * t * 0.5;
    return init_s_ + delta_s;
  } else {
    return end_s_;
  }
}
//Evaluate是Apollo数据结构里的老生常谈——插值，根据时间t求得对应匀减速段的v
double ConstantDecelerationTrajectory1d::Evaluate_v(const double t) const {
  if (t < end_t_) {
    return init_v_ - deceleration_ * t;
  } else {
    return 0.0;
  }
}
//Evaluate是Apollo数据结构里的老生常谈——插值，根据时间t求得对应匀减速段的a
double ConstantDecelerationTrajectory1d::Evaluate_a(const double t) const {
  if (t < end_t_) {
    return -deceleration_;
  } else {
    return 0.0;
  }
}

//Evaluate是Apollo数据结构里的老生常谈——插值，根据时间t求得对应匀减速段的jerk
double ConstantDecelerationTrajectory1d::Evaluate_j(const double t) const {
  return 0.0;
}

//返回匀减速至0需要的总时间
double ConstantDecelerationTrajectory1d::ParamLength() const { return end_t_; }

//返回一维匀减速轨迹类的名称字符串，默认为空
std::string ConstantDecelerationTrajectory1d::ToString() const { return ""; }

//定义一个抽象的插值模板，参数order为阶数，param基本为时间t
//order = 0 插值0阶就等效于Evaluate_s
//order = 1 插值1阶就等效于Evaluate_v
//order = 2 插值2阶就等效于Evaluate_a
//order = 3 插值3阶就等效于Evaluate_j
double ConstantDecelerationTrajectory1d::Evaluate(const std::uint32_t order,
                                                  const double param) const {
  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;
}

}  // namespace planning
}  // namespace apollo