Apollo Planning（一）

规划(planning)模块的作用是根据感知预测的结果，当前的车辆信息和路况规划出一条车辆能够行驶的轨迹，这个轨迹会交给控制(control)模块，控制模块通过油门，刹车和方向盘使得车辆按照规划的轨迹运行。

规划模块的轨迹是短期轨迹，即车辆短期内行驶的轨迹，长期的轨迹是routing模块规划出的导航轨迹，即起点到目的地的轨迹，规划模块会先生成导航轨迹，然后根据导航轨迹和路况的情况，沿着短期轨迹行驶，直到目的地。这点也很好理解，我们开车之前先打开导航，然后根据导航行驶，如果前面有车就会减速或者变道，超车，避让行人等，这就是短期轨迹，结合上述方式直到行驶到目的地。

一、Planning输入输出

我们先看下Apollo的数据流向：

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

Planning模块的输入在"planning_component.h"中，接口如下:

输入参数为:

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

实际上还有高精度地图信息，不在参数中传入，而是在函数中直接读取的。

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

输出结果为：规划好的路径。

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

二、Planning整个流程

下图是整个Planning模块的执行过程：

模块的入口是PlanningComponent，在Cyber中注册模块，订阅和发布消息，并且注册对应的Planning类（NaviPlanning / OnLanePlanning）。
Planning的过程为事件触发，即只要收集完成对应TOPIC的消息，就会触发执行，保证实时性。
Planning类主要实现了2个功能，一个是启动ReferenceLineProvider来提供参考线，后面生成的轨迹都是在参考线的基础上做优化。Planning类另外的一个功能是执行Planning主流程。
Planning主流程先是选择对应的Planner，此处用到工厂模式planner_factory_.CreateObject根据配置信息创建Planner（LatticePlanner、NaviPlanner、PublicRoadPlanner、RTKReplayPlanner），这里主要分析PublicRoadPlanner。在配置文件中定义了Planner支持的场景(Scenario)，把规划分为具体的几个场景来执行，每个场景又分为几个阶段(Stage)，每个阶段会执行多个任务(Task)，任务执行完成后，对应的场景就完成了。不同场景间的切换是由一个状态机(ScenarioDispatch)来控制的。规划控制器根据ReferenceLineProvider提供的参考线，在不同的场景下做切换，生成一条车辆可以行驶的轨迹，并且不断重复上述过程直到到达目的地。

接下来我们逐步分析整个planning模块的代码结构。

三、Planning模块入口

3.1模块注册

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_msg_reader_;
  std::shared_ptr<cyber::Reader<relative_map::MapMsg>> relative_map_reader_;
  std::shared_ptr<cyber::Reader<storytelling::Stories>> story_telling_reader_;

  std::shared_ptr<cyber::Writer<ADCTrajectory>> planning_writer_;
  std::shared_ptr<cyber::Writer<routing::RoutingRequest>> rerouting_writer_;
  std::shared_ptr<cyber::Writer<PlanningLearningData>> planning_learning_data_writer_; 

  // 在Cyber中注册模块
  CYBER_REGISTER_COMPONENT(PlanningComponent)

3.2模块初始化

除了注册模块，订阅和发布消息之外，planning_component实现了2个主要函数"init"和"proc"。

Init中实现了模块的初始化：

if (FLAGS_use_navigation_mode) {
    planning_base_ = std::make_unique<NaviPlanning>(injector_);
} else {
    planning_base_ = std::make_unique<OnLanePlanning>(injector_);
}

上面实现了2种Planning的注册，planning模块根据配置选择不同的Planning实现方式， "FLAGS_use_navigation_mode"在Planning模块的conf目录中。在global_flagfile.txt中use_navigation_mode=false，Planning默认情况下的实现是"OnLanePlanning"。下面介绍下这2种Planning的区别。

NaviPlanning - 相对地图规划器；

OnLanePlanning - 主要的应用场景是开放道路的自动驾驶。

"NaviPlanning"和"OnLanePlanning"都继承自同一个基类，并且在PlanningComponent中通过配置选择一个具体的实现进行注册。

PlanningComponent::Init()接下来实现了具体的消息发布和消息订阅，我们只看具体的一个例子：

// 读取routing模块的消息
routing_reader_ = node_->CreateReader<RoutingResponse>(
      config_.topic_config().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_ = ...
// 读取驾驶行为
  pad_msg_reader_ = ...
// 
	story_telling_reader_ =
// 是否使用导航模式
if (FLAGS_use_navigation_mode) {
//  读取相对地图
    relative_map_reader_ = node_->CreateReader<MapMsg>(
        config_.topic_config().relative_map_topic(),
        [this](const std::shared_ptr<MapMsg>& map_message) {
          ADEBUG << "Received relative map data: run relative map callback.";
          std::lock_guard<std::mutex> lock(mutex_);
          relative_map_.CopyFrom(*map_message);
        });
  }// 发布规划好的线路
  planning_writer_ =   node_->CreateWriter<ADCTrajectory>(FLAGS_planning_trajectory_topic);
// 发布重新规划请求
  rerouting_writer_ =      node_->CreateWriter<RoutingRequest>(FLAGS_routing_request_topic);
planning_learning_data_writer_ = node_->CreateWriter<PlanningLearningData>(config_.topic_config().planning_learning_data_topic());

至此，Planning模块的初始化就完成了。

3.3模块运行

Proc的主要是检查数据，并且执行注册好的Planning，生成路线并且发布。

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

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

  // 4. 发布消息
planning_writer_->Write(adc_trajectory_pb);
}

整个"PlanningComponent"的分析就完成了，可以看到"PlanningComponent"是Planning模块的入口，在Apollo3.5引入了Cyber之后，实现了Planning模块在Cyber中的注册，订阅和发布topic消息。同时实现了3种不同的Planning，根据配置选择其中的一种并且运行。

由于默认的Planning是开放道路的OnLanePlanning，我们接下来主要分析这个Planning。

参考文献

apollo介绍之planning模块(四) - 知乎 王方浩，apollo介绍之planning模块(四)

Baidu Apollo代码解析之Open Space Planner中的Hybrid A* - 知乎Baidu Apollo代码解析之Open Space Planner中的Hybrid A*

原文链接：https://blog.csdn.net/whuzhang16/article/details/122544438