Apollo：循迹规划器RTKReplayPlanner

RTK Replay Planner 是基于录制的轨迹进行循迹的 Planner，是 Apollo 比较原始的一种 Planner。需要事先录制好设定的轨迹再来规划行车路线。

录制的轨迹文件格式可以参考 modules/planning/data/garage.csv。

轨迹中包含以下信息：

名称	说明
x,y,z	车辆位置
speed	车速
acceleration	加速度
curvature	曲率
curvature_change_rate	曲率变化率
time	时间戳
theta	航向角
gear	档位
s	路程
throttle	油门
brake	刹车
steering	方向盘转角

RTKReplayPlanner::ReadTrajectoryFile

打开轨迹文件，并将轨迹point存储到complete_rtk_trajectory_成员函数中。

void ReadTrajectoryFile(const std::string &filename);

RTKReplayPlanner::QueryPositionMatchedPoint

/*
参数1：planning起始点
参数2：历史轨迹
返回值：距离start_point的以L2距离度量的最近邻轨迹点。
步骤：
【1】求轨迹的点与start_point的距离的平方距离的最小值
【2】遍历轨迹点，找到相对于起点的最短距离点。
【3】返回最短距离点的index
*/
  std::size_t QueryPositionMatchedPoint(
      const apollo::common::TrajectoryPoint &start_point,
      const std::vector<apollo::common::TrajectoryPoint> &trajectory) const;

RTKReplayPlanner::Plan

输入：

• planning_init_point：planning起始点
• frame：保存一个规划周期内的所有数据。
• ptr_computed_trajectory：路径规划的结果集，800个连续的point存储在ptr_trajectory中，由一系列离散的轨迹点组成。

返回：

• 规划器的状态，是否规划成功

算法的大概步骤：

• 【1】找到与起点start_point最近的轨迹中的点的index。即在历史数据里找到一个点，然后把它看做start_point的近似。
• 【2】在match至轨迹的end之间找到一个end_index，使得end_index不越界，同时又尽可能的长。
• 【3】将区间[first,last)的元素赋值到在【ptr_discretized】vector容器中
• 【4】修改在ptr_discretized_trajectory轨迹集合中的相对时间。以第一个point为0起始点。
• 【5】如果point没有800个点，则直接用最后的一个点填充直到满足800个点。

实现：

• 寻找规划轨迹中的第一个可变道点。

• 如果可变道点不是轨迹末尾（即轨迹中途需要变道），那么重新规划轨迹（修改it，同时还需要判断重新规划的轨迹是不是可行驶＆＆可变道&&变道的距离大于FLAGS_change_lane_min_length，如果不是，那么输出变道规划失败。）

• 如果变道规划失败，或者变道策略有更高的优先级，那么重新规划轨迹