Apollo源码分析系列的第六部分planning
Posted slam庖丁解牛
tags:
篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了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模块的应有之意。
另，如果你能为知识付费，我会很高兴 ^-^.
以上是关于Apollo源码分析系列的第六部分planning的主要内容，如果未能解决你的问题，请参考以下文章
自动驾驶 Apollo 源码分析系列，系统监控篇：Monitor模块如何监控硬件
自动驾驶 Apollo 源码分析系列，系统监控篇：Monitor模块如何监控硬件
自动驾驶 Apollo 源码分析系列，系统监控篇：Monitor模块如何监控硬件
开发者说｜ Apollo 源码分析系列感知篇：红绿灯检测和识别