概述

SpeedData类继承速度点类vector，本质上也是一个数组容器，用来存放一系列的速度点，并实现以下功能：

将这些速度点按时间升序排序；

在容器末尾增加一个速度点；

从速度点序列按时间t插值得到速度点；

从速度点序列按轨迹长度s插值得到速度点；

获取速度点序列总时间、轨迹总长度；

返回多个速度点信息作为debug调试字符串。

这个SpeedData类用于存放速度规划结果，以及对其进行一些操作。

*速度点SpeedPoint包括v,a,s,t,da等信息，详细定义参见相关proto文件。

1. speed_data.h代码详解

#pragma once

#include <string>
#include <vector>

#include "modules/common/proto/pnc_point.pb.h"

namespace apollo {
namespace planning {

//SpeedData类是继承基类SpeedPoint速度点的vector类，本质上是一个vector容器类
class SpeedData : public std::vector<common::SpeedPoint> {
 public:
  SpeedData() = default;  //默认构造析构函数

  virtual ~SpeedData() = default;

  //带参构造函数用speed_points数组去初始化SpeedData类
  explicit SpeedData(std::vector<common::SpeedPoint> speed_points);

  //在SpeedData类对象末尾插入一个速度点，用s,t,v,a,da
  void AppendSpeedPoint(const double s, const double time, const double v,
                        const double a, const double da);

  //Evaluate通常指插值，用时间去速度规划数组SpeedData类对象里插值出s,v,a,da
  //插值结果存放到函数输入的最后一个参数speed_point里
  bool EvaluateByTime(const double time,
                      common::SpeedPoint* const speed_point) const;

  //Evaluate通常指插值，用纵向位置去速度规划数组SpeedData类对象里插值出t,v,a,da
  //插值结果存放到函数输入的最后一个参数speed_point里
  bool EvaluateByS(const double s, common::SpeedPoint* const speed_point) const;
 
  //返回速度规划的总时间，就是SpeedData类对象里速度点数组最后一个点时间减第一个点时间
  double TotalTime() const;

  //返回速度规划的纵向位置总长度，就是SpeedData类对象里速度点数组最后一个点长度减第一个点长度
  double TotalLength() const;
 
  //返回debug字符串，其实就是获取速度点数组容器SpeedData中前11个点的信息构成字符串来debug
  virtual std::string DebugString() const;
};

}  // namespace planning
}  // namespace apollo

2. speed_data.cc代码详解

#include "modules/planning/common/speed/speed_data.h"

#include <algorithm>
#include <mutex>
#include <utility>

#include "absl/strings/str_cat.h"
#include "absl/strings/str_join.h"
#include "modules/common/math/linear_interpolation.h"
#include "modules/common/util/point_factory.h"
#include "modules/common/util/string_util.h"
#include "modules/common/util/util.h"
#include "modules/planning/common/planning_gflags.h"

namespace apollo {
namespace planning {

//使用了SpeedPoint速度点数据结构
//速度点包含v,a,s,t,da等内容，详细定义参见\apollo\modules\common\proto\pnc_point.proto
using apollo::common::SpeedPoint;

//带参构造函数，输入参数是一个SpeedPoint由多个速度点构成的vector容器对象speed_points
//:冒号后面为类对象初始化方式，直接将参数speed_points move到SpeedData类对象容器中
SpeedData::SpeedData(std::vector<SpeedPoint> speed_points)
    : std::vector<SpeedPoint>(std::move(speed_points)) {
  //然后构造函数对SpeedData这个容器里的对象进行排序
  //sort函数的第3个参数代表排序的条件，即按照速度点的时间升序进行排序
  std::sort(begin(), end(), [](const SpeedPoint& p1, const SpeedPoint& p2) {
    return p1.t() < p2.t();
  });
}

//在SpeedData类容器对象末尾插入一个速度点，这个速度点的t必须大于插入前的最后一个速度点的时间
void SpeedData::AppendSpeedPoint(const double s, const double time,
                                 const double v, const double a,
                                 const double da) {
  static std::mutex mutex_speedpoint;
  UNIQUE_LOCK_MULTITHREAD(mutex_speedpoint);

  if (!empty()) {
    ACHECK(back().t() < time);
  }
  push_back(common::util::PointFactory::ToSpeedPoint(s, time, v, a, da));
}

//根据给定的时间t去SpeedData速度点数组中插值出v,a,s,da，插值结果放入参数speed_point
bool SpeedData::EvaluateByTime(const double t,
                               common::SpeedPoint* const speed_point) const {
  if (size() < 2) {
    return false;
  }
  if (!(front().t() < t + 1.0e-6 && t - 1.0e-6 < back().t())) {
    return false;
  }

  auto comp = [](const common::SpeedPoint& sp, const double t) {
    return sp.t() < t;
  };

  auto it_lower = std::lower_bound(begin(), end(), t, comp);
  if (it_lower == end()) {
    *speed_point = back();
  } else if (it_lower == begin()) {
    *speed_point = front();
  } else {
    const auto& p0 = *(it_lower - 1);
    const auto& p1 = *it_lower;
    double t0 = p0.t();
    double t1 = p1.t();

    speed_point->Clear();
    speed_point->set_s(common::math::lerp(p0.s(), t0, p1.s(), t1, t));
    speed_point->set_t(t);
    if (p0.has_v() && p1.has_v()) {
      speed_point->set_v(common::math::lerp(p0.v(), t0, p1.v(), t1, t));
    }
    if (p0.has_a() && p1.has_a()) {
      speed_point->set_a(common::math::lerp(p0.a(), t0, p1.a(), t1, t));
    }
    if (p0.has_da() && p1.has_da()) {
      speed_point->set_da(common::math::lerp(p0.da(), t0, p1.da(), t1, t));
    }
  }
  return true;
}

//根据给定的纵向位置s去SpeedData速度点数组中插值出v,a,t,da，插值结果放入参数speed_point
bool SpeedData::EvaluateByS(const double s,
                            common::SpeedPoint* const speed_point) const {
  if (size() < 2) {
    return false;
  }
  if (!(front().s() < s + 1.0e-6 && s - 1.0e-6 < back().s())) {
    return false;
  }

  auto comp = [](const common::SpeedPoint& sp, const double s) {
    return sp.s() < s;
  };

  auto it_lower = std::lower_bound(begin(), end(), s, comp);
  if (it_lower == end()) {
    *speed_point = back();
  } else if (it_lower == begin()) {
    *speed_point = front();
  } else {
    const auto& p0 = *(it_lower - 1);
    const auto& p1 = *it_lower;
    double s0 = p0.s();
    double s1 = p1.s();

    speed_point->Clear();
    speed_point->set_s(s);
    speed_point->set_t(common::math::lerp(p0.t(), s0, p1.t(), s1, s));
    if (p0.has_v() && p1.has_v()) {
      speed_point->set_v(common::math::lerp(p0.v(), s0, p1.v(), s1, s));
    }
    if (p0.has_a() && p1.has_a()) {
      speed_point->set_a(common::math::lerp(p0.a(), s0, p1.a(), s1, s));
    }
    if (p0.has_da() && p1.has_da()) {
      speed_point->set_da(common::math::lerp(p0.da(), s0, p1.da(), s1, s));
    }
  }
  return true;
}

//返回速度点数组容器SpeedData涵盖的整个时间长度
double SpeedData::TotalTime() const {
  if (empty()) {
    return 0.0;
  }
  return back().t() - front().t();
}

//返回速度点数组容器SpeedData涵盖的轨迹的总长度
double SpeedData::TotalLength() const {
  if (empty()) {
    return 0.0;
  }
  return back().s() - front().s();
}

//返回debug字符串，其实就是获取速度点数组容器SpeedData中前11个点的信息构成字符串来debug
std::string SpeedData::DebugString() const {
  const auto limit = std::min(
      size(), static_cast<size_t>(FLAGS_trajectory_point_num_for_debug));
      //FLAGS_去\apollo\modules\planning\common\planning_gflags.cc中
      //取出trajectory_point_num_for_debug的值，默认为10,也就是debug时看的速度点个数
  return absl::StrCat(
      "[\n",
      //absl::StrJoin就是将字符串以某种方式连接起来构成一个大字符串
      //StrJoin()函数的参数apollo::common::util::DebugStringFormatter()指字符的连接方式
      //StrJoin()函数的前两个参数begin(),begin+limit代表迭代器的范围
      absl::StrJoin(begin(), begin() + limit, ",\n",
                    apollo::common::util::DebugStringFormatter()),
      "]\n");
}

}  // namespace planning
}  // namespace apollo