P-1 _Apollo3.5:planning模块

概述

参考以下博客并结合自己理解进行整理和修改
https://blog.csdn.net/davidhopper/article/details/79176505#t1
https://paul.pub/apollo-planning/
决策规划模块主要任务:根据导航信息以及车辆的当前状态，在有限的时间范围内，计算出一条合适的轨迹供车辆行驶。

1. 车辆的全局行驶路线由Routing模块提供.
2. 车辆的当前状态包括了:车辆的自身状态(姿态,速度,角速度等等),当前所处的位置,周边环境以及交通状态等等.
3. Planning模块响应速度必须稳定可靠.小于100ms,10hz
4. 合适的轨迹有多个层次含义.轨迹不仅包括行驶路线,还要包括每个时刻车辆的速度 加速度 方向转向等信息.转弯角度.也要考虑乘客体验 规划模块内部与其他模型交互示意如下所示:
   

整体Pipeline

• PncMap：Planning and Control Map。该模块的主要作用是：根据Routing提供的数据，生成Planning模块需要的路径信息。
• Frame：Frame中包含了Planning一次计算循环中需要的所有数据。例如：地图，车辆状态，参考线，障碍物信息等等。ReferenceLine是车辆行驶的参考线，TrafficDecider与交通规则相关.
• EM Planner：Apollo系统中内置了好几个Planner，但目前默认使用的是EM
  Planner用于开放道路.该模块的实现可以说是整个Planning模块的灵魂所在。

Planner 概述

有5个planner

代码概述

输入:

1. 预测的障碍物信息(prediction_obstacles)
2. 车辆底盘(chassis)信息(车辆的速度，加速度，航向角等信息)
3. 车辆当前位置(localization_estimate)

输出:

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

planning整个流程

1. Planning主流程先是选择对应的Planner，我们这里主要分析PublicRoadPlanner
2. 在配置文件中定义了Planner支持的场景(Scenario)，把规划分为具体的几个场景来执行，每个场景又分为几个阶段(Stage)，每个阶段会执行多个任务(Task)，任务执行完成后，对应的场景就完成了。
3. 不同场景间的切换是由一个状态机(ScenarioDispatch)来控制的。规划控制器根据ReferenceLineProvider提供的参考线，在不同的场景下做切换，生成一条车辆可以行驶的轨迹，并且不断重复上述过程直到到达目的地。

模块代码简单分析

模块注册
Planning模块的入口为"planning_component.h"和"planning_component.cc"两个文件:
订阅和发布消息;
在Cyber中注册模块;
模块初始化
除了注册模块，订阅和发布消息之外，planning模块实现了2个主要函数"init"和"proc".
模块初始化实现3种Planning的注册:

Init接下来实现了具体的消息发布和消息订阅

// 读取routing模块的消息
routing_reader_ = node_->CreateReader<RoutingResponse>(
      FLAGS_routing_response_topic,
      [this](const std::shared_ptr<RoutingResponse>& routing) {
        AINFO << "Received routing data: run routing callback."
              << routing->header().DebugString();
        std::lock_guard<std::mutex> lock(mutex_);
        routing_.CopyFrom(*routing);
      });
// 读取红绿灯
  traffic_light_reader_ = ...
// 是否使用导航模式
  if (FLAGS_use_navigation_mode) {
    pad_message_reader_ = ...
// 读取相对地图
    relative_map_reader_ = ...
  }
// 发布规划好的线路
  planning_writer_ =
      node_->CreateWriter<ADCTrajectory>(FLAGS_planning_trajectory_topic);
// 发布重新规划请求
  rerouting_writer_ =
      node_->CreateWriter<RoutingRequest>(FLAGS_routing_request_topic);

模块运行
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));
}