百度apollo自动驾驶planning代码学习-Apollo planning/common/path/FrenetFramePath类代码解析

鉴于Apollo planning模块代码量较大，将Apollo/modules/planning路径下的代码逐个击破，先从简单的开始。最后会对Apollo planning模块进行一个系统的整理总结。

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

frenet_frame_path.h主要是声明FrenetFramePath类；

frenet_frame_path.cc主要是FrenetFramePath类的定义，也就是实现；

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

FrenetFramePath类主要是实现？

主要是实现存储一条Frenet坐标系下的路径path，并在path上找到一个离给定SL边界sl_boundary最近的点，简而言之就是在Frenet SL坐标系下路径上找到距离SL边界最近的点。

1.frenet_frame_path.cc代码详解

这一块代码没有完全看明白，没关系，直接看单元测试代码frenet_frame_path_test.cc，就是一个简单示例说明FrenetFramePath类的用法,参见第2小节

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

#include <algorithm>
#include <limits>

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

namespace apollo {
namespace planning {

///在apollo/modules/common/proto/pnc_point.proto中由message FrenetFramePoint定义
//google protobuf会根据.proto文件将message生成c++类数据结构
// FrenetFramePoint类包含数据成员s,l,dl,ddl
//s是纵向位置，l是横向位置，SL就是沿着道路参考线的坐标系，很好的横纵向解耦
//FrenetFramePoint就是一个Frenet坐标点
using apollo::common::FrenetFramePoint;

//FrenetFramePath类的构造函数，参数是FrenetFramePoint即Frenet坐标点类型的vector容器
//容器vector名为points
//该构造函数的作用就是直接将Frenet坐标点类型的vector容器里的点都move到FrenetFramePath类对象//中，因此FrenetFramePath类就是Frenet坐标路径就是用来存放一系列Frenet坐标点的数据结构
FrenetFramePath::FrenetFramePath(std::vector<FrenetFramePoint> points)
    : std::vector<FrenetFramePoint>(std::move(points)) {}

//获取Frenet坐标下路径的总长度，也是最后一个点s减去第一个点s
double FrenetFramePath::Length() const {
  if (empty()) {
    return 0.0;
  }
  return back().s() - front().s();
}

FrenetFramePoint FrenetFramePath::GetNearestPoint(const SLBoundary& sl) const {
  auto it_lower =
      std::lower_bound(begin(), end(), sl.start_s(), LowerBoundComparator);
  if (it_lower == end()) {
    return back();
  }
  auto it_upper =
      std::upper_bound(it_lower, end(), sl.end_s(), UpperBoundComparator);
  double min_dist = std::numeric_limits<double>::max();
  auto min_it = it_upper;
  for (auto it = it_lower; it != it_upper; ++it) {
    if (it->l() >= sl.start_l() && it->l() <= sl.end_l()) {
      return *it;
    } else if (it->l() > sl.end_l()) {
      double diff = it->l() - sl.end_l();
      if (diff < min_dist) {
        min_dist = diff;
        min_it = it;
      }
    } else {
      double diff = sl.start_l() - it->l();
      if (diff < min_dist) {
        min_dist = diff;
        min_it = it;
      }
    }
  }
  return *min_it;
}

FrenetFramePoint FrenetFramePath::EvaluateByS(const double s) const {
  CHECK_GT(size(), 1U);
  auto it_lower = std::lower_bound(begin(), end(), s, LowerBoundComparator);
  if (it_lower == begin()) {
    return front();
  } else if (it_lower == end()) {
    return back();
  }
  const auto& p0 = *(it_lower - 1);
  const auto s0 = p0.s();
  const auto& p1 = *it_lower;
  const auto s1 = p1.s();

  FrenetFramePoint p;
  p.set_s(s);
  p.set_l(common::math::lerp(p0.l(), s0, p1.l(), s1, s));
  p.set_dl(common::math::lerp(p0.dl(), s0, p1.dl(), s1, s));
  p.set_ddl(common::math::lerp(p0.ddl(), s0, p1.ddl(), s1, s));
  return p;
}

}  // namespace planning
}  // namespace apollo

 2.frenet_frame_path_test.cc代码详解

该类的单元测试代码，详解看下方手写笔记

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

#include "gtest/gtest.h"

namespace apollo {
namespace planning {

using apollo::common::FrenetFramePoint;

class FrenetFramePathTest : public ::testing::Test {
 public:
  void InitFrenetFramePath() {
    std::vector<double> s{1, 2, 3, 4, 5, 6, 7, 8, 9, 10};
    std::vector<double> l{1, 2, 1, 0, -1, -2, -1, 0, 1, 2};
    std::vector<double> dl{1, 0, -1, 0, -1, 0, 1, 1, 1, 0};
    std::vector<double> ddl{0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
    std::vector<FrenetFramePoint> sl_points;
    for (size_t i = 0; i < s.size(); ++i) {
      sl_points.emplace_back();
      FrenetFramePoint& point = sl_points.back();
      point.set_s(s[i]);
      point.set_l(l[i]);
      point.set_dl(dl[i]);
      point.set_ddl(ddl[i]);
    }
    path_.reset(new FrenetFramePath(std::move(sl_points)));
  }
  void SetUp() { InitFrenetFramePath(); }

  std::unique_ptr<FrenetFramePath> path_;
};

TEST_F(FrenetFramePathTest, GetNearestPoint) {
  SLBoundary sl_boundary;
  {
    // at the beginning
    sl_boundary.set_start_s(-2);
    sl_boundary.set_end_s(-1);
    sl_boundary.set_start_l(0);
    sl_boundary.set_end_l(1);
    auto nearest = path_->GetNearestPoint(sl_boundary);
    EXPECT_DOUBLE_EQ(nearest.s(), 1.0);
    EXPECT_DOUBLE_EQ(nearest.l(), 1.0);
    EXPECT_DOUBLE_EQ(nearest.dl(), 1.0);
    EXPECT_DOUBLE_EQ(nearest.ddl(), 0.0);
  }
  {
    // at the end
    sl_boundary.set_start_s(11);
    sl_boundary.set_end_s(12);
    sl_boundary.set_start_l(0);
    sl_boundary.set_end_l(1);
    auto nearest = path_->GetNearestPoint(sl_boundary);
    EXPECT_DOUBLE_EQ(nearest.s(), 10.0);
    EXPECT_DOUBLE_EQ(nearest.l(), 2.0);
    EXPECT_DOUBLE_EQ(nearest.dl(), 0.0);
    EXPECT_DOUBLE_EQ(nearest.ddl(), 0.0);
  }

  {
    // in the middle, left side
    sl_boundary.set_start_s(1);
    sl_boundary.set_end_s(9);
    sl_boundary.set_start_l(3);
    sl_boundary.set_end_l(4);
    auto nearest = path_->GetNearestPoint(sl_boundary);
    EXPECT_DOUBLE_EQ(nearest.s(), 2.0);
    EXPECT_DOUBLE_EQ(nearest.l(), 2.0);
    EXPECT_DOUBLE_EQ(nearest.dl(), 0.0);
    EXPECT_DOUBLE_EQ(nearest.ddl(), 0.0);
  }
  {
    // in the middle, right side
    sl_boundary.set_start_s(1);
    sl_boundary.set_end_s(9);
    sl_boundary.set_start_l(-4);
    sl_boundary.set_end_l(-3);
    auto nearest = path_->GetNearestPoint(sl_boundary);
    EXPECT_DOUBLE_EQ(nearest.s(), 6.0);
    EXPECT_DOUBLE_EQ(nearest.l(), -2.0);
    EXPECT_DOUBLE_EQ(nearest.dl(), 0.0);
    EXPECT_DOUBLE_EQ(nearest.ddl(), 0.0);
  }
  {
    // in the middle, cross
    sl_boundary.set_start_s(1);
    sl_boundary.set_end_s(9);
    sl_boundary.set_start_l(-1);
    sl_boundary.set_end_l(0);
    auto nearest = path_->GetNearestPoint(sl_boundary);
    EXPECT_DOUBLE_EQ(nearest.s(), 4.0);
    EXPECT_DOUBLE_EQ(nearest.l(), 0.0);
    EXPECT_DOUBLE_EQ(nearest.dl(), 0.0);
    EXPECT_DOUBLE_EQ(nearest.ddl(), 0.0);
  }
}

}  // namespace planning
}  // namespace apollo

 主要是通过FrenetFramePath类的成员函数GetNearest(sl_boundary)去Frenet路径上寻找离给定SL边界的最近点，根据单元测试可知通常FrenetFramePath类对象通常定义为指针对象

std::unique_ptr<FrenetFramePath> path_;

FrenetFramePath类也就是用来存放Frenet坐标系路径以及获取路径上距给定SL边界的最近点