Apollo6.0代码Lattice算法详解——Part1:离散化参考线

0.前置知识

        要理解该部分代码需要了解一下protobuf中定义的PathPoint具体内容, 如下:

message PathPoint {
  // coordinates
  optional double x = 1;
  optional double y = 2;
  optional double z = 3;

  // direction on the x-y plane
  optional double theta = 4;
  // curvature on the x-y planning
  optional double kappa = 5;
  // accumulated distance from beginning of the path
  optional double s = 6;

  // derivative of kappa w.r.t s.
  optional double dkappa = 7;
  // derivative of derivative of kappa w.r.t s.
  optional double ddkappa = 8;
  // The lane ID where the path point is on
  optional string lane_id = 9;

  // derivative of x and y w.r.t parametric parameter t in CosThetareferenceline
  optional double x_derivative = 10;
  optional double y_derivative = 11;
}

        代码位置: modules/common/proto/pnc_point.proto

1.涉及主要函数

        1) 函数名: ToDiscretizedReferenceLine(const std::vector< ReferencePoint >& ref_points)
            函数位置: modules/planning/planner/lattice/lattice_planner.cc
            函数作用: 离散化参考线的点

2.函数关系

        该部分主要涉及函数就一个 ToDiscretizedReferenceLine, 故没有画关系图.

3.函数代码详解

    3.1 lattice_planner.cc中代码部分

  // 离散化参考线的点
  // 1. obtain a reference line and transform it to the PathPoint format.
  auto ptr_reference_line =
      std::make_shared<std::vector<PathPoint>>(ToDiscretizedReferenceLine(
          reference_line_info->reference_line().reference_points()));

    3.2 函数-ToDiscretizedReferenceLine

// 相当于把referenceLine的点放到vector<PathPoint>中去,顺带计算了总长度
std::vector<PathPoint> ToDiscretizedReferenceLine(
    const std::vector<ReferencePoint>& ref_points) {
  double s = 0.0;
  std::vector<PathPoint> path_points;
  for (const auto& ref_point : ref_points) { // 把参考线上的很多点都拿出来放到path_points容器中
    PathPoint path_point;
    // 下面这几个貌似是跟据高精地图算的,B站视频提到的
    path_point.set_x(ref_point.x());
    path_point.set_y(ref_point.y());
    path_point.set_theta(ref_point.heading());
    path_point.set_kappa(ref_point.kappa());
    path_point.set_dkappa(ref_point.dkappa());

    // 如果不是第一次在path_points压入点的话, 累计一下s值
    if (!path_points.empty()) {
      double dx = path_point.x() - path_points.back().x();
      double dy = path_point.y() - path_points.back().y();
      s += std::sqrt(dx * dx + dy * dy);
    }
    path_point.set_s(s);
    path_points.push_back(std::move(path_point));
  }
  return path_points;
}

4.参考资料

         1.参考视频: Lattice.
         2.参考文章: Apollo中Lattice规划器结构梳理.