鉴于Apollo planning模块代码量较大，将Apollo/modules/planning路径下的代码逐个击破，先从简单的开始。

对discretized_path.cc/.h/_test.cc进行解析

discretized_path.h主要是声明DiscretizedPath类；

discretized_path.cc主要是DiscretizedPath类的定义，也就是实现；

discretized_path_test.cc是针对DiscretizedPath类功能实现的单元测试，主要利用google gtest库，了解该代码对DiscretizedPath类的使用方法和作用会更加熟悉。

DiscretizedPath类主要是实现离散路径点的存放，能返回其size,路径总长度，以及给定一个纵向位置s能正向(s增大的方向)或能反向插值出中间点的路径信息。

1. discretized_path.cc代码详解

/******************************************************************************
 定义了一个DiscretizedPath离散路径类，实现
 *****************************************************************************/

#include "modules/planning/common/path/discretized_path.h"

#include <algorithm>

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

namespace apollo {
namespace planning {

//使用路径点类,一种数据结构由.proto文件生成的一种数据结构类
//详细参见proto文件定义，modules\common\proto\pnc_point.proto
using apollo::common::PathPoint; 

//DiscretizedPath类构造函数，参数就是路径点的vector容器,其实就是将一系列路径点加载进来
DiscretizedPath::DiscretizedPath(std::vector<PathPoint> path_points)
    : std::vector<PathPoint>(std::move(path_points)) {}

//获取离散点路径的总长度，如果离散点容器为空返回0，否则返回最后一个点的纵向位置station减去第一个
//点的纵向位置station就是路径的总长度
double DiscretizedPath::Length() const {
  if (empty()) {
    return 0.0;
  }
  return back().s() - front().s();
}

//在离散的路径点之间正向线性插值得到给定s的点的信息,输入参数也是某个指定的纵向位置path_s
PathPoint DiscretizedPath::Evaluate(const double path_s) const {
  //是否非空？
  ACHECK(!empty());
  //找到刚好纵向位置小于path_s的点，也就是path_s的下边界点
  auto it_lower = QueryLowerBound(path_s);
  //如果下边界点就是起始点，返回起始点
  if (it_lower == begin()) {
    return front();
  }
  //如果下边界点就是最后一个点就返回最后一个点
  if (it_lower == end()) {
    return back();
  }
  //实际上就是线性插值，用下边界和上一个点线性插值得到给定纵向位置点的路径点信息(x,y,theta,kappa
  //等)
  return common::math::InterpolateUsingLinearApproximation(*(it_lower - 1),
                                                           *it_lower, path_s);
}

//查询下边界函数，输入参数是某个特定的纵向位置值path_s，找到路径点容器中刚好小于path_s的点
std::vector<PathPoint>::const_iterator DiscretizedPath::QueryLowerBound(
    const double path_s) const {
  //这是内嵌了一个函数？找到刚小于path_s的点
  auto func = [](const PathPoint &tp, const double path_s) {
    return tp.s() < path_s;
  };
    //正向遍历路径点，找到刚好不小于path_s的第一个点
  return std::lower_bound(begin(), end(), path_s, func);
}

//在离散的路径点之间逆向线性插值得到给定s的点的信息,输入参数也是某个指定的纵向位置path_s
PathPoint DiscretizedPath::EvaluateReverse(const double path_s) const {
  ACHECK(!empty());
  auto it_upper = QueryUpperBound(path_s);
  if (it_upper == begin()) {
    return front();
  }
  if (it_upper == end()) {
    return back();
  }
  return common::math::InterpolateUsingLinearApproximation(*(it_upper - 1),
                                                           *it_upper, path_s);
}

//查询上边界函数，输入参数是某个特定的纵向位置值path_s，找到路径点容器中刚好大于path_s的点
std::vector<PathPoint>::const_iterator DiscretizedPath::QueryUpperBound(
    const double path_s) const {
  auto func = [](const double path_s, const PathPoint &tp) {
    return tp.s() < path_s;
  };
    //逆向遍历路径点，找到刚好不大于path_s的第一个点
  return std::upper_bound(begin(), end(), path_s, func);
}

}  // namespace planning
}  // namespace apollo

2. discretized_path_test.cc 

然后planning/common/path/下还有个discretized_path_test.cc主要就是Apollo用于单元测试，测试这个DiscretizedPath类能否实现给定s，然后根据离散路径点线性插值出给定s处路径点信息的功能。

介绍下google gtest

玩转Google开源C++单元测试框架Google Test系列(gtest)之一 - 初识gtest

TEST这个宏，它有两个参数，官方的对这两个参数的解释为：[TestCaseName，TestName]

EXPECT_EQ，EXPECT_DOUBLE_EQ判断两个参数是否相等，一个是判断整数，一个是判断浮点数

对代码的解析看代码下方的手写笔记

#include "modules/planning/common/path/discretized_path.h"

#include "cyber/common/log.h"
#include "gtest/gtest.h"
#include "modules/common/util/point_factory.h"

namespace apollo {
namespace planning {

//PathPoint是路径点类
using apollo::common::PathPoint;
//PointFactory是点工厂类，可以直接造路径点速度点数据结构
using apollo::common::util::PointFactory;

//TEST是google gtest库的函数，专门做单元测试
//就是做一个正向的路径点插值测试
TEST(DiscretizedPathTest, basic_test) {
  const double s1 = 0.0;
  const double s2 = s1 + std::sqrt(1.0 + 1.0);
  const double s3 = s2 + std::sqrt(1.0 + 1.0);
  const double s4 = s3 + std::sqrt(1.0 + 1.0);
  PathPoint p1 = PointFactory::ToPathPoint(0.0, 0.0, 0.0, s1);
  PathPoint p2 = PointFactory::ToPathPoint(1.0, 1.0, 0.0, s2);
  PathPoint p3 = PointFactory::ToPathPoint(2.0, 2.0, 0.0, s3);
  PathPoint p4 = PointFactory::ToPathPoint(3.0, 3.0, 0.0, s4);

  std::vector<PathPoint> path_points{p1, p2, p3, p4};

  DiscretizedPath discretized_path(path_points);
  EXPECT_EQ(discretized_path.size(), 4);

  EXPECT_DOUBLE_EQ(discretized_path.Length(), std::sqrt(1.0 + 1.0) * 3.0);

  auto eval_p1 = discretized_path.Evaluate(0.0);
  EXPECT_DOUBLE_EQ(eval_p1.s(), 0.0);
  EXPECT_DOUBLE_EQ(eval_p1.x(), 0.0);
  EXPECT_DOUBLE_EQ(eval_p1.y(), 0.0);

  auto eval_p2 = discretized_path.Evaluate(0.3 * std::sqrt(2.0));
  EXPECT_DOUBLE_EQ(eval_p2.s(), 0.3 * std::sqrt(2.0));
  EXPECT_DOUBLE_EQ(eval_p2.x(), 0.3);
  EXPECT_DOUBLE_EQ(eval_p2.y(), 0.3);

  auto eval_p3 = discretized_path.Evaluate(1.8);
  EXPECT_DOUBLE_EQ(eval_p3.s(), 1.8);
  EXPECT_DOUBLE_EQ(eval_p3.x(), (1.0 + 0.8) / std::sqrt(2));
  EXPECT_DOUBLE_EQ(eval_p3.y(), (1.0 + 0.8) / std::sqrt(2));

  auto eval_p4 = discretized_path.Evaluate(2.5);
  EXPECT_DOUBLE_EQ(eval_p4.s(), 2.5);
  EXPECT_DOUBLE_EQ(eval_p4.x(), (2.0 + 0.5) / std::sqrt(2));
  EXPECT_DOUBLE_EQ(eval_p4.y(), (2.0 + 0.5) / std::sqrt(2));

  discretized_path.clear();
  EXPECT_EQ(discretized_path.size(), 0);
}

TEST(DiscretizedPathTest, reverse_case) {
  const double s1 = 0.0;
  const double s2 = s1 - std::sqrt(1.0 + 1.0);
  const double s3 = s2 - std::sqrt(1.0 + 1.0);
  const double s4 = s3 - std::sqrt(1.0 + 1.0);
  PathPoint p1 = PointFactory::ToPathPoint(0.0, 0.0, 0.0, s1);
  PathPoint p2 = PointFactory::ToPathPoint(1.0, 1.0, 0.0, s2);
  PathPoint p3 = PointFactory::ToPathPoint(2.0, 2.0, 0.0, s3);
  PathPoint p4 = PointFactory::ToPathPoint(3.0, 3.0, 0.0, s4);

  std::vector<PathPoint> path_points{p1, p2, p3, p4};

  DiscretizedPath discretized_path(path_points);
  EXPECT_EQ(discretized_path.size(), 4);

  EXPECT_DOUBLE_EQ(discretized_path.Length(), -std::sqrt(1.0 + 1.0) * 3.0);

  auto eval_p1 = discretized_path.EvaluateReverse(0.0);
  EXPECT_DOUBLE_EQ(eval_p1.s(), 0.0);
  EXPECT_DOUBLE_EQ(eval_p1.x(), 0.0);
  EXPECT_DOUBLE_EQ(eval_p1.y(), 0.0);

  auto eval_p2 = discretized_path.EvaluateReverse(-0.3 * std::sqrt(2.0));
  EXPECT_DOUBLE_EQ(eval_p2.s(), -0.3 * std::sqrt(2.0));
  EXPECT_DOUBLE_EQ(eval_p2.x(), 0.3);
  EXPECT_DOUBLE_EQ(eval_p2.y(), 0.3);

  auto eval_p3 = discretized_path.EvaluateReverse(-1.8);
  EXPECT_DOUBLE_EQ(eval_p3.s(), -1.8);
  EXPECT_DOUBLE_EQ(eval_p3.x(), (1.0 + 0.8) / std::sqrt(2));
  EXPECT_DOUBLE_EQ(eval_p3.y(), (1.0 + 0.8) / std::sqrt(2));

  auto eval_p4 = discretized_path.EvaluateReverse(-2.5);
  EXPECT_DOUBLE_EQ(eval_p4.s(), -2.5);
  EXPECT_DOUBLE_EQ(eval_p4.x(), (2.0 + 0.5) / std::sqrt(2));
  EXPECT_DOUBLE_EQ(eval_p4.y(), (2.0 + 0.5) / std::sqrt(2));

  discretized_path.clear();
  EXPECT_EQ(discretized_path.size(), 0);
}

}  // namespace planning
}  // namespace apollo

 现在把这部分代码截出来单独跑一下单元测试，测试DiscretizedPath类的功能是否实现，顺便掌握一下DiscretizedPath类的使用方法。

3.用CMake跑DiscretizedPath类的单元测试

apollo/modules/planning/common/path/DiscretizedPath类单元测试的cmake实现-C++文档类资源-CSDN下载

代码已经上传，参考上述链接。

测试环境:ubuntu 18.04 c++   cmake编译方式

新建一个测试工程文件夹My_Discrete_path_test

其中包含build空文件夹，CMakeList编译规则, deps存放依赖的apollo其他模块代码全截出来放到这，proto存放单元测试需要用到.proto文件,src里就一个main.cc

 CMakeist里需要定义对google gtest库,absl,gflag,glog,protobuf,eigen库的依赖，你的计算机上也需要安装这些库，然后要修改deps里部分原本apollo的代码依赖头文件路径，保证这个工程文件夹可以与apollo源代码完全分开独立运行。具体实现可以下载第3节开头的链接代码。

然后原本在discretized_path_test.cc 里定义的TEST都放到main.cc里

同时main.cc里主函数如下：

int main(int argc,char **argv)
{
    testing::InitGoogleTest(&argc,argv);
    return RUN_ALL_TESTS();
}

上述代码的意思就是把main.cc里定义的所有TEST全部跑一下，检测程序的输出是否等于预先给定的正确结果，主要是检测在离散路径点中给定一个纵向位置s去里面线性插值，能否得到正确的s,x,y,z

上述都准备好后，进入build文件夹打开终端执行以下命令：

cmake ..

make -j6

./DiscretizedPathTest

运行效果如下:

可以看到正向插值测试，和反向插值测试都PASSED了，测试全部通过。

倘若我修改一下代码：

本来eval_p2就是离散路径点p1p2p3p4正向插值出的点，eval_p2是p1p2p3p4上纵向位置s=0.3*m处的点，由上图可知，

eval_p2点的x应该是0.3，那我现在把main.cc里TEST(原本在discretized_path_test.cc 里定义，拷贝到main.cc里)变成判断eval_p2的x是否为0.4，那肯定TEST会不通过

auto eval_p2 = discretized_path.Evaluate(0.3 * std::sqrt(2.0));

EXPECT_DOUBLE_EQ(eval_p2.s(), 0.3 * std::sqrt(2.0));

//EXPECT_DOUBLE_EQ(eval_p2.x(), 0.3);

EXPECT_DOUBLE_EQ(eval_p2.x(), 0.4);

EXPECT_DOUBLE_EQ(eval_p2.y(), 0.3);

再次运行

果然TEST1-basic_test 就没有通过，原因就是插值出来eval_p2点的x为0.3，而测试预先给定0.4不相等。

同时我们也可以通过单元测试代码discretized_path_test.cc了解到DiscretizedPath类的用法及功能：

1.将路径点构成PathPoint类型数据vector容器{(x0,y0,z0,s0),(x1,y1,z1,s1),...} vector容器名path_points，然后DiscretizedPath类构造函数直接将path_points move到类对象discretized_path里

PathPoint p1 = PointFactory::ToPathPoint(0.0, 0.0, 0.0, s1);
  PathPoint p2 = PointFactory::ToPathPoint(1.0, 1.0, 0.0, s2);
  PathPoint p3 = PointFactory::ToPathPoint(2.0, 2.0, 0.0, s3);
  PathPoint p4 = PointFactory::ToPathPoint(3.0, 3.0, 0.0, s4);

  std::vector<PathPoint> path_points{p1, p2, p3, p4};

  apollo::planning::DiscretizedPath discretized_path(path_points);

2.DiscretizedPath类对象discretized_path返回离散路径点的个数

discretized_path.size()

 3.DiscretizedPath类对象discretized_path返回离散路径总长度

discretized_path.Length()

4.给定纵向位置s在离散路径点序列中正向线性插值出插值点处x,y,z,s

类对象discretized_path调用成员函数.Evaluate(),参数1.8的含义就是插值并查询离散路径上纵向位置为1.8m的路径点的信息

正向插值，即s增大的路径点方向

  auto eval_p3 = discretized_path.Evaluate(1.8);
  EXPECT_DOUBLE_EQ(eval_p3.s(), 1.8);
  EXPECT_DOUBLE_EQ(eval_p3.x(), (1.0 + 0.8) / std::sqrt(2));
  EXPECT_DOUBLE_EQ(eval_p3.y(), (1.0 + 0.8) / std::sqrt(2));

5.给定纵向位置s在离散路径点序列中反向线性插值出插值点处x,y,z,s

类对象discretized_path调用成员函数.EvaluateReverse()，参数-1.8的含义就是插值并查询离散路径上纵向位置为-1.8m的路径点的信息

反向插值，即s减小的路径点方向

  auto eval_p3 = discretized_path.EvaluateReverse(-1.8);
  EXPECT_DOUBLE_EQ(eval_p3.s(), -1.8);
  EXPECT_DOUBLE_EQ(eval_p3.x(), (1.0 + 0.8) / std::sqrt(2));
  EXPECT_DOUBLE_EQ(eval_p3.y(), (1.0 + 0.8) / std::sqrt(2));