百度apollo planning代码学习-Apollo\modules\planning\math\curve1d\PiecewiseQuinticSpiralPath类代码详解

概述

PiecewiseQuinticSpiralPath类是apollo planning模块下modules\planning\math\curve1d\piecewise_quintic_spiral_path.cc /.h实现

从类名来看，应该是PiecewiseQuinticSpiralPath是分段五次螺旋路径类？

从代码来看PiecewiseQuinticSpiralPath类主要是实现：
全局搜索此类好像并没有用到？该类仅在piecewise_quintic_spiral_path.cc /.h以及其测试用例里出现，并没有被外部调用？
1.增加一段5次螺旋路径，输入这一段路径长delta_s，终点曲率kappa，终点曲率变化率dkappa，终点处航向角theta，起点的曲率和航向角及s等信息在构造类对象时就已经确定，根据起点终点的信息可以求解到一段螺旋线；
2.插值函数返回螺旋线在s处对应的曲率/曲率变化率/…；
3.获取整个分段五次螺旋线的总长度；
4.根据输入的纵向位置s定位其所属哪一段五次螺旋路径，以便确定起点终点theta，曲率及曲率变化率以及总长度等信息，也就能确定这一段螺旋线的表达式，也还是作为了插值的辅助函数；

简而言之，该类就是输入每一段起点终点曲率航向纵向位置等信息构造一段段五次螺旋线路径，并能实现输入s插值曲率/曲率变化率等信息

*五次螺旋路径，螺旋线，根据起点曲率，曲率变化率，终点曲率，曲率变化率，起点终点航向角，确定一段螺旋曲线
五次螺旋路径参数曲线{(s0,theta0),(s1,theta1)…}
曲率的定义：曲线某点切线方向对弧长的转动率
*五次螺旋路径 曲率是纵向位置s的5次函数

piecewise_quintic_spiral_path.h

#pragma once

#include <string>
#include <vector>

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

namespace apollo {
namespace planning {
//分段五次螺旋路径类公有继承了1维参数曲线类
class PiecewiseQuinticSpiralPath : public Curve1d {
 public:
 //分段五次螺旋路径类构造函数，输入参数航向角theta，曲率kappa,和曲率变化率dkappa
  PiecewiseQuinticSpiralPath(const double theta, const double kappa,
                             const double dkappa);

  virtual ~PiecewiseQuinticSpiralPath() = default;

//增加一段5次螺旋路径，这一段路径长delta_s，终点曲率kappa，终点曲率变化率dkappa，终点处航向角theta，
  void Append(const double theta, const double kappa, const double dkappa,
              const double delta_s);
//插值函数返回螺旋线在s处对应的曲率/曲率变化率/...，取决于输入参数的order是什么
  double Evaluate(const std::uint32_t order, const double param) const override;
//对曲率进行插值？
  double DeriveKappaS(const double s) const;
//获取整个分段五次螺旋线的总长度
  double ParamLength() const override;
//返回曲线的名称？
  std::string ToString() const override { return "PiecewiseQuinticSpiralPath"; }

 private:
 //根据输入的纵向位置s定位其所属哪一段五次螺旋路径，以便确定起点终点theta，曲率及曲率变化率以及总长度等信息，也就能确定这一段螺旋线的表达式
  size_t LocatePiece(const double param) const;

  std::vector<QuinticSpiralPath> pieces_;

  std::vector<double> accumulated_s_;

  double last_theta_ = 0.0;

  double last_kappa_ = 0.0;

  double last_dkappa_ = 0.0;
};

}  // namespace planning
}  // namespace apollo

piecewise_quintic_spiral_path.cc

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

namespace apollo {
namespace planning {
//分段五次螺旋路径类构造函数，输入参数航向角theta，曲率kappa,和曲率变化率dkappa
PiecewiseQuinticSpiralPath::PiecewiseQuinticSpiralPath(const double theta,
                                                       const double kappa,
                                                       const double dkappa)
    : last_theta_(theta), last_kappa_(kappa), last_dkappa_(dkappa) { //输入参数用来初始化类成员,分段五次螺旋路径类起点的航向曲率变化率等
  accumulated_s_.push_back(0.0);//在路径累计长度里塞入0起点从s=0开始，这段五次螺旋路径的起点对应的航向角theta，起点对应的曲率kappa，起点对应的率变化率dkappa
}

//增加一段5次螺旋路径，这一段路径长delta_s，终点曲率kappa，终点曲率变化率dkappa，终点处航向角theta，
void PiecewiseQuinticSpiralPath::Append(const double theta, const double kappa,
                                        const double dkappa,
                                        const double delta_s) {
  //累计弧长在之前的基础上加上这一段的长度delta_s 
  double s = delta_s + accumulated_s_.back();
  //塞入的是这一段终点对应的纵坐标s
  accumulated_s_.push_back(s);
//在分段pieces_里塞入起点终点的曲率等信息
  pieces_.emplace_back(last_theta_, last_kappa_, last_dkappa_, theta, kappa,
                       dkappa, delta_s);
//这一段螺旋曲线塞完后，这一段的终点又变成了下一段的起点
  last_theta_ = theta;
  last_kappa_ = kappa;
  last_dkappa_ = dkappa;
}

//插值函数返回螺旋线在s处对应的曲率/曲率变化率/...，取决于输入参数的order是什么
double PiecewiseQuinticSpiralPath::Evaluate(const std::uint32_t order,
                                            const double s) const {
  auto index = LocatePiece(s);
  return pieces_[index].Evaluate(order, s - accumulated_s_[index]);
}

//对曲率进行插值？
double PiecewiseQuinticSpiralPath::DeriveKappaS(const double s) const {
  auto index = LocatePiece(s);
  const auto& piece = pieces_[index];
  double ratio = (s - accumulated_s_[index]) / piece.ParamLength();
  return piece.DeriveKappaDerivative(QuinticSpiralPath::DELTA_S, ratio);
}

//获取整个分段五次螺旋线的总长度
double PiecewiseQuinticSpiralPath::ParamLength() const {
  return accumulated_s_.back();
}

//根据输入的纵向位置s定位其所属哪一段五次螺旋路径，以便确定起点终点theta，曲率及曲率变化率以及总长度等信息
size_t PiecewiseQuinticSpiralPath::LocatePiece(const double s) const {
  ACHECK(s >= accumulated_s_.front() && s <= accumulated_s_.back());

  auto it_lower =
      std::lower_bound(accumulated_s_.begin(), accumulated_s_.end(), s);

  if (it_lower == accumulated_s_.begin()) {
    return 0;
  } else {
    return std::distance(accumulated_s_.begin(), it_lower) - 1;
  }
}

}  // namespace planning
}  // namespace apollo