【Apollo源码分析】系列的第六部分【planning】

来源：互联网发布：python assert() 编辑：程序博客网时间：2024/05/29 08:25

【Apollo源码分析】系列的第六部分【planning】

planning模块代码大概有1000行左右。这个模块仍然处于待完善的状态。
和其他模块一样，这里只有框架，除了 RTKReplayPlanner，没有实用的算法。

planning模块的输入：

Localization
Perception (future work)
Prediction (future work)
Decision (future work)
Recorded RTK trajectory

planning模块的输出：

trajectory point

planning/main.cc

照例先看看main.cc

int main(int argc, char **argv) {
  google::InitGoogleLogging(argv[0]);
  google::ParseCommandLineFlags(&argc, &argv, true);
  ros::init(argc, argv, "planning");
  ::apollo::planning::PlanningNode planning_node;
  planning_node.Run();
  return 0;
}
//与#define APOLLO_MAIN(APP) 大同小异。可能后期会改为一致。

planning/common/planning_gflags.h

.h文件和.cc文件，声明和定义planning模块的变量。
主要的变量是：

#include "modules/planning/common/planning_gflags.h"
//发布运动规划planning的频率，单位是HZ。
DEFINE_int32(planning_loop_rate, 5, "Loop rate for planning node"); 
//用RTK定位算法得到的最近一段时间的历史轨迹
DEFINE_string(rtk_trajectory_filename, "modules/planning/data/garage.csv",
              "Loop rate for planning node");
//planning时，用于回退匹配的轨迹节点数量。
DEFINE_uint64(rtk_trajectory_backward, 10,
              "The number of points to be included in RTK trajectory "
              "before the matched point");
//前向匹配的轨迹节点数量
DEFINE_uint64(rtk_trajectory_forward, 800,
              "The number of points to be included in RTK trajectory "
              "after the matched point");
//重新规划planning的阈值，高于此值则重新planning
DEFINE_double(replanning_threshold, 2.0,
              "The threshold of position deviation "
              "that triggers the planner replanning");
//轨迹点之间的时间分辨率/间隔，0.01s
DEFINE_double(trajectory_resolution, 0.01,
              "The time resolution of "
              "output trajectory.");

planning/planner/planner.h

Planner是纯虚基类。有一个公有的Plan()：
纯虚函数。继承它的子类必须重写这个纯虚函数才能实例化。
所有的路径规划的class都继承自这个类。
目的：虚函数指针，运行时决断，父类指针指向子类对象，runtime时调用子类的成员函数。

具体到Planner 而言就是提供一个公有的接口 virtual bool Plan().
使得在运行时可以执行不同的planning算法。

参数1:planning起始点
参数2：路径规划结果集，由一系列离散的轨迹点组成。
目的：根据历史行驶的一系列轨迹节点，
并结合Perception模块+Prediction模块+Decision模块+
Localization地图定位模块，来进行推算未来一段时间的行驶轨迹。
本质上是路径规划算法。
  virtual bool Plan(
      const apollo::common::TrajectoryPoint &start_point,
      std::vector<apollo::common::TrajectoryPoint> *discretized_trajectory) = 0;

planning/planner/rtk_replay_planner.h

RTKReplayPlanner继承自虚基类Planner。
这个类通过从历史轨迹的file文件中读取轨迹点，
并且从这些历史轨迹点中选择连续的800个point组成历史轨迹的结果集合，
输出到ptr_trajectory中。

目的：从历史信息中提取与start_point相关联的轨迹point集合，
为下一个具体的planner类进行路径规划做数据准备。

// 构造函数直接读取历史轨迹节点文件file中的内容.并存储到complete_rtk_trajectory_数据成员中
RTKReplayPlanner();


/*
参数1:planning起始点
参数2：路径规划的结果集，800个连续的point存储在ptr_trajectory中，由一系列离散的轨迹点组成。
ptr_trajectory存储的是历史轨迹节点而不是planning好的未来的轨迹节点。
故本class 命名为ReplayPlanner.
算法的大概步骤：
【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个点。
*/
bool Plan(const apollo::common::TrajectoryPoint &start_point,
std::vector<apollo::common::TrajectoryPoint> *ptr_trajectory) override;

////打开轨迹文件，并将轨迹point存储到complete_rtk_trajectory_成员函数中。
void ReadTrajectoryFile(const std::string &filename);

file内容举例：

csv格式每行的header信息：
x,y,z,speed,acceleration,curvature,curvature_change_rate,time,theta,gear,s,throttle,brake,steering

具体的值：
586385.858607, 4140674.7357, -28.3670201628, 0.216666668653, 0.887545286694, 0.0227670812611, -0.0177396744278, 1496957374.6140, 2.83470068837, 1, 0.00216666668653, 22.0157165527, 13.6934461594, 12.6382980347

/*
参数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;

测试代码:


TEST_F(RTKReplayPlannerTest, ComputeTrajectory) {
  FLAGS_rtk_trajectory_filename = "modules/planning/testdata/garage.csv";
  RTKReplayPlanner planner;
  TrajectoryPoint start_point;
  start_point.set_x(586385.782842);
  start_point.set_y(4140674.76063);
  std::vector<TrajectoryPoint> trajectory;
  bool planning_succeeded = planner.Plan(start_point, &trajectory);
  EXPECT_TRUE(planning_succeeded);
  EXPECT_TRUE(!trajectory.empty());
  //800个点。不够时用最后一个点填充。
  EXPECT_EQ(trajectory.size(), (std::size_t)FLAGS_rtk_trajectory_forward);
  auto first_point = trajectory.front();
  EXPECT_DOUBLE_EQ(first_point.x(), 586385.782841);//第一个匹配点，.csv第20个
  EXPECT_DOUBLE_EQ(first_point.y(), 4140674.76065);
  auto last_point = trajectory.back();
  EXPECT_DOUBLE_EQ(last_point.x(), 586355.063786);//最后一个匹配点，.csv第819个。
  EXPECT_DOUBLE_EQ(last_point.y(), 4140681.98605);
}

TEST_F(RTKReplayPlannerTest, ErrorTest) {
  FLAGS_rtk_trajectory_filename =
      "modules/planning/testdata/garage_no_file.csv";
  RTKReplayPlanner planner;
  FLAGS_rtk_trajectory_filename = "modules/planning/testdata/garage_error.csv";
  RTKReplayPlanner planner_with_error_csv;//file不完整，只有1行数据时，直接返回false
  TrajectoryPoint start_point;
  start_point.set_x(586385.782842);
  start_point.set_y(4140674.76063);
  std::vector<TrajectoryPoint> trajectory;
  EXPECT_TRUE(!planner_with_error_csv.Plan(start_point, &trajectory));
}

planning/planner_factory.h


// PlannerType指定可选的路径规划class类型。
// 目前Apollo只支持由RTK组成的plan路径规划方法，其他方法还没有实现。
enum class PlannerType { RTK_PLANNER, OTHER };

// 工厂方法模式，没有数据成员，只有一个静态成员函数CreateInstance()
class PlannerFactory {
 public:
  PlannerFactory() = delete;
// 参数：路径规划方法。目前只支持PlannerType::RTK_PLANNER类型。
  static std::unique_ptr<Planner> CreateInstance(
      const PlannerType &planner_type);
};

CreateInstance()实现代码：

//  参数：planning路径规划的算法类型
//  返回值：planning路径规划实例对象。
std::unique_ptr<Planner> PlannerFactory::CreateInstance(
    const PlannerType &planner_type) {
  switch (planner_type) {
    case PlannerType::RTK_PLANNER:
    // new 一个实例对象。
      return std::unique_ptr<Planner>(new RTKReplayPlanner());
    //这里可见其他类型的规划方法为空指针实现。
    default:
      return nullptr;
  }
}

planning/planning.h

class Planning 是上层的路径规划方法封装。
数据成员：

std::unique_ptr<Planner> ptr_planner_;//由工厂方法模式创建的路径规划实例。
std::vector<common::TrajectoryPoint> last_trajectory_; //最近一段时间planning的轨迹point集合
double last_header_time_ = 0.0;//最近planning时间

私有成员函数：

/*
计算从上次【预测的】轨迹记录历史得到的起始点的车辆自身point位置
参数:当前时间
返回值：轨迹point+索引
算法步骤：
【1】二分查找，找到【start_time】小于【轨迹point的time+last_header】的第一个值。
【2】返回point+index
*/
std::pair<common::TrajectoryPoint, std::size_t>
  ComputeStartingPointFromLastTrajectory(const double curr_time) const;


/*计算车辆启动时的地理位置所在的轨迹point。
参数1:当前车辆状态(车辆速度，加速度，行驶方向等)
参数2：planning的预测时间段。
算法步骤：
【1】获取当前车辆的位置x,y,z
【2】获取当前车辆的速度，加速度信息
【3】设置point曲率
【4】设置转弯半径
【5】返回当前轨迹point
*/
common::TrajectoryPoint ComputeStartingPointFromVehicleState(
      const common::vehicle_state::VehicleState &vehicle_state,
      const double forward_time) const;

/*
参数1：与起始点match的轨迹点
参数2：需要回退的size，正常情况下回退10个轨迹point
返回值：在match之前的10个轨迹point
  */
std::vector<common::TrajectoryPoint> GetOverheadTrajectory(
      const std::size_t matched_index, const std::size_t buffer_size);

公有成员函数：

/*
   从历史信息+目前的车辆状态(车辆速度，加速度，行驶方向等)进行路径规划，
   结果存储在discretized_trajectory中。
   参数1：目前车辆状态信息
   参数2:是否自动驾驶模式
   参数3:路径规划发布时间
   参数4:路径规划的轨迹point结果集合
 */
  bool Plan(const common::vehicle_state::VehicleState &vehicle_state,
            const bool is_on_auto_mode, const double publish_time,
            std::vector<common::TrajectoryPoint> *discretized_trajectory);
 /*
 算法步骤：
 如果是自动驾驶模式，并且【预测】历史轨迹不为空(已有历史预测信息)
那么:
【1】从【预测的】历史轨迹中寻找与当前时间匹配的point
【2】计算匹配点与当前点的距离的平方差
【3】若小于阈值(2.0m)，则不需要重新进行路径规划,直接从匹配点开始路径规划.
【4】将last_traj修改为*planning_traj以用于下一次路径规划
若非自动驾驶模式/没有历史轨迹点/匹配点距离太远：
【6】从当前车辆状态获取point位置，然后再从当前位置进行路径规划。
【7】保存预测轨迹，然后用于下一次预测
  */

// 重置为初始状态。清空轨迹point的信息。
void Reset();

看到这里读者肯定以为planning模块的路径规划算法已经完成了。有这种看法非常不错，但是我认为他读懂了这个代码的50%。

实际情况是在 Planning类的Plan()成员函数中有这么一句话(而且是2次出现)：

    bool planning_succeeded =
          ptr_planner_->Plan(matched_point, planning_trajectory);
      if (!planning_succeeded) {
        last_trajectory_.clear();
        return false;
      }
以上代码的含义对不熟悉c++的人来说是有一点困难的。
理解这段代码需要了解3个知识：
【1】面向对象设计
【2】继承，封装，多态
【3】设计模式

planning/planning_node.h

PlanningNode是Apollo的 planning模块与ROS通信的Node封装。
class PlanningNode {
 public:
  /*  构造函数，初始化配置文件*/
  PlanningNode();
  /*  析构函数，什么也不做*/
  virtual ~PlanningNode();
  /* 在ROS中按照设置的频率(默认5hz),定时发布路径规划结果。多次调用RunOnce() 。  */
  void Run();
  /*复位，清空，调用Planning类的Reset()成员函数*/
  void Reset();
 private:
//进行一次planning路径规划算法
  void RunOnce();
//序列化轨迹轨迹point到文件
  ADCTrajectory ToTrajectoryPb(
      const double header_time,
      const std::vector<common::TrajectoryPoint> &discretized_trajectory);
//进行路径规划的实例对象      
  Planning planning_;

总结

planning模块分析就到这里了。废话不多说了。
回顾一下：

planning模块的输入：

Localization
Perception (future work)
Prediction (future work)
Decision (future work)
Recorded RTK trajectory

planning模块的输出：

trajectory point

一句话：

目前的planning模块除了能处理由 RTK记录的历史轨迹point 数据，啥也没干不了。【只能回顾过去】，【不能规划未来】。

而基于【过去和现在】并【规划未来】才是planning模块的应有之意。

本文首发于微信公众号slamcode。

注释版源码：源码

阅读全文

1 0