Apollo：Planning源码分析之PlanningComponent

数据流向

可以看到规划(planning)模块的上游是Localization, Prediction, Routing模块，而下游是Control模块。Routing模块先规划出一条导航线路，然后Planning模块根据这条线路做局部优化，如果Planning模块发现短期规划的线路行不通（比如前面修路，或者错过了路口），会触发Routing模块重新规划线路，因此这两个模块的数据流是双向的。
Planning模块的输入在"planning_component.h"中，接口如下:

bool Proc(const std::shared_ptr<prediction::PredictionObstacles>&
                prediction_obstacles,
            const std::shared_ptr<canbus::Chassis>& chassis,
            const std::shared_ptr<localization::LocalizationEstimate>&
                localization_estimate) override;

输入参数为:

• 预测的障碍物信息(prediction_obstacles)
• 车辆底盘(chassis)信息(车辆的速度，加速度，航向角等信息)
• 车辆当前位置(localization_estimate)
• 实际上还有高精度地图信息，不在参数中传入，而是在函数中直接读取的。

Planning模块的输出结果在"PlanningComponent::Proc()"中，为规划好的线路，发布到Control模块订阅的Topic中。
输出结果为：规划好的路径。

planning_writer_->Write(std::make_shared<ADCTrajectory>(adc_trajectory_pb));

模块注册

Planning模块的入口为"planning_component.h"和"planning_component.cc"两个文件，实现的功能如下：

// 订阅和发布消息
std::shared_ptr<cyber::Reader<perception::TrafficLightDetection>>  traffic_light_reader_;
std::shared_ptr<cyber::Reader<routing::RoutingResponse>> routing_reader_;
std::shared_ptr<cyber::Reader<planning::PadMessage>> pad_message_reader_;
std::shared_ptr<cyber::Reader<relative_map::MapMsg>> relative_map_reader_;

std::shared_ptr<cyber::Writer<ADCTrajectory>> planning_writer_;
std::shared_ptr<cyber::Writer<routing::RoutingRequest>> rerouting_writer_;
  
// 在Cyber中注册模块
CYBER_REGISTER_COMPONENT(PlanningComponent)

这些消息的订和发布是在PlanningComponent::Init初始化的，如下：

// 发布规划好的线路
  planning_writer_ = node_->CreateWriter<ADCTrajectory>(
      config_.topic_config().planning_trajectory_topic());
// 发布重新规划请求
  rerouting_writer_ = node_->CreateWriter<RoutingRequest>(
      config_.topic_config().routing_request_topic());

  planning_learning_data_writer_ = node_->CreateWriter<PlanningLearningData>(
      config_.topic_config().planning_learning_data_topic());

接下来我们来看下PlanningComponent是个什么东西

是什么？

• Cyber RT以组件的方式来管理各个模块，组件的实现会基于该框架提供的基类：apollo::cyber::Component。
• Planning模块自然也不例外：planning组件，即PlanningComponent类、对象，其继承自cyber::Component<M0, M1, M2, NullType>类，可在CyberRT中完成注册工作，实现消息响应机制的托管，是工程实现和算法实现之间的桥梁。其实现的类如下：

• 从上面可以看出planning组件是一个支持三个channel输入的消息回调型组件，支持的消息类型分别如下：
  • 预测的障碍物信息：prediction::PredictionObstacles
  • 车辆底盘信息：canbus::Chassis，即车辆的速度，加速度，航向角等信息
  • 车辆当前位置：localization::LocalizationEstimate
• cyberRT系统收到上面任何一个消息后都会调用planning组件的proc函数进行消息响应(与回调型组件对应的是定时器型组件，即cyberRT中的定时器会按照一定频率调用proc函数)

组件工作流：

模块初始化： PlanningComponent::Init

模块初始化调用了PlanningComponent::Init，初始化工作由CyberRT系统的launch命令触发.

planning_base_

• planning_base_是PlanningComponent中最重要的数据成员，实现具体的规划功能。它是planning的入口

• PlanningBase只是一个抽象类，该类如下子类：

  • NaviPlanning，车道规划
  • OnLanePlanning，导航规划，主要的应用场景是开放道路的自动驾驶。

• 在PlanningComponent的实现中，会根据具体的配置选择Planning的入口。其入口是由planning_base_描述的。默认配置是OnLanePlanning

模块初始化实现2种Planning的注册:

在PlanningBase类中，下面这个方法是及其重要的：

/**
* @brief main logic of the planning module,
* runs periodically triggered by timer.
*/
void RunOnce(const LocalView& local_view,
             ADCTrajectory* const trajectory_pb) override;

方法的注释已经说明得很清楚了：这是Planning模块的主体逻辑，会被timer以固定的间隔调用。每次调用就是一个规划周期。很显然，我们需要重点要关注的就是OnLanePlanning::RunOnce方法的逻辑。

重要数据成员

在PlanningComponent中，除了planning_base_之外，还有一些数据成员需要注意

LocalView

从注释中我们可以看出，LocalView包含了所有planning输入所需要的信息,它是所有输入数据的打包，包括planning_component的输入和通过reader获取的输入：

• 障碍物的预测信息
• 车辆底盘信息
• 大致定位信息
• 交通灯信息
• 导航路由信息
• 相对地图信息

组件数据缓存 DependencyInjector

• DependencyInjector：依赖注入器，这是一个过于专业的名词，来自软件设计模式的依赖倒置原则的一种具体实现方式，起到模块解耦作用。
  • “依赖倒置原则(Dependence Inversion Principle)是程序要依赖于抽象接口，不要依赖于具体实现。简单的说就是要求对抽象进行编程，不要对实现进行编程，这样就降低了客户与实现模块间的耦合。”
• DependencyInjector本质上是一个数据缓存中心，叫做DataCacheCenter更为贴切，可以叫做数据缓存器
• DependencyInjector对象内部管理了planning模块工程过程中的实时数据和几乎全部历史数据，以便于规划任务的前后帧之间的承接，以及异常处理的回溯。
• DependencyInjector是以空对象的形式引入到planning组件中，进而引入到planning模块中用来承载中间数据。

DependencyInjector类结构如下：

依赖注入结构中主要有6类成员变量，分别如下：

• PlanningContext planning_context_：负责planning上下文的缓存，比如是否触发重新路由的ReroutingStatus信息
• History history_： 负责障碍物状态的缓存，包括运动状态，决策结果。该数据与routing结果绑定，routing变更后会清理掉历史数据
• FrameHistory 　frame_history_：是一个可索引队列，负责planning的输入，输出等主要信息的缓存，以Frame类进行组织，内部包含LocalView结构体（负责输入数据的融合管理）。与上述的History是不同的是，该缓数据自模块启动后就开始缓存所有的Frame对象，不受routing变动的影响。
• EgoInfo ego_info_：提供车辆动、静信息，即车辆运动状态参数（轨迹、速度、加速度等）和车辆结构参数（长宽高等）
• apollo::common::VehicleStateProvider vehicle_state_：车辆状态提供器，用于获取车辆实时信息
• LearningBasedData learning_based_data_：基于学习的数据，用于学习建模等

https://blog.csdn.net/weixin_39199083/article/details/124641311

小结

这里完成了planner的注册。

大致分为2步，核心代码整理截图如下

• 1、根据配置选择planning实现方式，并调用init初始化
  • Planning_base_是PlanningBase的实例化对象，用来描述planning的执行过程，其中比较重要的两个函数是RunOnce和Plan，OnLanePlanning和NaviPlanning都是PlanningBase的继承类
  • 默认选择的规划模式是OnLanePlanning

• 2、初始化输入输出通道

模块响应：PlanningComponent::Proc

消息响应由CyberRT在接收到关联消息到达后触发。Proc的主要是检查数据，并且执行注册好的Planning，生成路线并且发布。

bool PlanningComponent::Proc(...) {
  // 1. 检查是否需要重新规划线路。
  CheckRerouting();

  // 2. 数据放入local_view_中，并且检查输入数据。
  ...
  
  // 3. 执行注册好的Planning，生成线路。
  planning_base_->RunOnce(local_view_, &adc_trajectory_pb);

  // 4. 发布消息
  planning_writer_->Write(std::make_shared<ADCTrajectory>(adc_trajectory_pb));
}

更进一步，核心代码整理截图如下

数据检查及更新

• （图中3-1）检查是否需要重新规划路线，Routing模块先规划出一条导航线路，然后Planning模块根据这条线路做局部优化，如果Planning模块发现局部规划的路径行不通（比如前面修路，或者错过了路口），会发送rerouting消息触发Routing模块重新规划线路。

• （图中3-2）数据放入local_view中，数据来源包括外部传参和内部缓存数据。并对输入数据local_view进行检查，因为planning是事件触发，只有收集完所有的TOPIC的信息，才能触发执行。（最终local_view会保存到Frame对象中（在RunOnce()））

• 配置强化学习模块，进行数据预处理和数据发布
  
  

• 执行规划任务（图中4），此处会调用PlanningBase子类的RunOnce函数进行一次规划任务，生成规划后的轨迹

• 发布规划结果（图中5），利用已经创建的planning通道发送规划结果

• 更新历史缓存（图中6），将最终轨迹更新到history中（其他缓存在PlanningBase子类中完成的）以备不时之需

通过调用planning_base_->RunOnce()函数来开启决策规划之旅，目前含有两个大的规划器：navi_planning和on_lane_planning，至于选取那个，在程序初始化的时候被配置了

Planning搞起：RunOnce 结构

在这里，主要做了下图中的事：

RunOnce中使用的planner

在RunOnce执行中，调用了planner_->Plan，whose内部包含了：lattice、navi、public_road、rtk_replay四种规划器

lattice::plan

主要七个步骤：
1.将参考线转变为离散地图点
2.计算参考线上初始规划点的匹配点
3.根据匹配点计算Frenet帧的初始状态
4.解析决策，得到规划目标
5.分别生成纵向和横向一维轨迹束
6.评价：首先，根据动态约束条件对一维轨迹的可行性进行评价；其次，评估可行的纵向和横向轨迹对，并根据成本进行排序。
7.返回无碰撞的、符合条件的轨迹
如下图所示：

• 在采用Frenet坐标系后，ST和SL图极为便捷，位于第4部分实现
• 横纵向轨迹束的生成,位于第5部分，分为纵向轨迹、横向轨迹两个实现

纵向轨迹的生成

主要使用了四次和五次多项式，五次多项式可保证一阶导v、二阶导a、三阶导jerk等连续性

横向轨迹的生成

横向轨迹生成用了五次多项式和二次规划两套方法，就像图中的一样…二次规划求解我没看懂，等闲下来搞定它，只知道大致原理不能算懂～额额

轨迹的评价

对于lattice第6部分，看到很多介绍的材料里都有解释，集火在cost函数上，cost函数形式在很多论文里可以直接看到了

轨迹的挑选

在对轨迹进行筛选时，一开始没有看到对轨迹的剔除，后来才看到while循环时，不满足直接continue了…直接保证凡是执行到reference_line_info->SetTrajectory时，就意味着是成功符合的轨迹了，是在下村儿了

小结

planning组件主要完成两项任务：初始化工作Init，消息响应工作Proc

该模块的输入输出如下：

这里再重复一下，planning的输入分为Reader和Process入参的原因在于，planning依赖于Process的三个上游输入，只有同时接到这三个输入，才会触发planning的主逻辑，即是planning正常启动的必要条件。而Reader则不是，其中部分上游还依赖于配置参数是否打开，具体可以查看Apollo的源码。

planning_component.cc代码框架

https://www.codenong.com/cs106106522/