Apollo Planning（二）

一、OnLanePlanning

每次Planning会根据以下2个信息作为输入来执行：

1. Planning上下文信息
2. Frame结构体(车辆信息，位置信息等所有规划需要用到的信息，在/planning/common/frame.h中)

1.1 初始化

OnLanePlanning的初始化逻辑在Init中，主要实现分配具体的Planner，启动参考线提供器(reference_line_provider_)，代码分析如下：

Status OnLanePlanning::Init(const PlanningConfig& config) {
  ...
  // 启动参考线提供器，会另启动一个线程，执行一个定时任务，每隔50ms提供一次参考线。
  reference_line_provider_ = std::make_unique(
      injector_->vehicle_state(), hdmap_);
  reference_line_provider_->Start();
  // 为Planning分配具体的Planner。
  planner_ = planner_dispatcher_->DispatchPlanner();
  ...
}
可以看到"DispatchPlanner"在"OnLanePlanning"实例化的时候就指定了。
class OnLanePlanning : public PlanningBase {
 public:
  explicit OnLanePlanning(const std::shared_ptr& injector)
      : PlanningBase(injector) {
    planner_dispatcher_ = std::make_unique();
  }

可以看到"DispatchPlanner"在"OnLanePlanning"实例化的时候就指定了。

class OnLanePlanning : public PlanningBase {
 public:
  explicit OnLanePlanning(const std::shared_ptr& injector)
      : PlanningBase(injector) {
    planner_dispatcher_ = std::make_unique();
}

再看"OnLanePlannerDispatcher"具体的实现，也是根据配置选择具体的"Planner"，默认为"PUBLIC_ROAD"规划器:

// 配置文件planning_config.pb.txt
standard_planning_config {
  planner_type: PUBLIC_ROAD
  planner_public_road_config {
  }
}

OnLanePlannerDispatcher具体实现

// OnLanePlannerDispatcher具体实现
std::unique_ptr OnLanePlannerDispatcher::DispatchPlanner(
    const PlanningConfig& planning_config,
    const std::shared_ptr& injector) {
  return planner_factory_.CreateObject(
      planning_config.standard_planning_config().planner_type(0), injector);
}

1.2 事件触发

OnLanePlanning的主要逻辑在"RunOnce()"中，在Apollo 3.5之前是定时器触发，3.5改为事件触发，即收到对应的消息之后，就触发执行，这样做的好处是增加了实时性

void OnLanePlanning::RunOnce(const LocalView& local_view,
                             ADCTrajectory* const ptr_trajectory_pb) {
  
  // 初始化Frame
  status = InitFrame(frame_num, stitching_trajectory.back(), vehicle_state);
  ...
  
  // 判断是否符合交通规则
  for (auto& ref_line_info : *frame_->mutable_reference_line_info()) {
    TrafficDecider traffic_decider;
    traffic_decider.Init(traffic_rule_configs_);
    auto traffic_status =
        traffic_decider.Execute(frame_.get(), &ref_line_info, injector_);
    if (!traffic_status.ok() || !ref_line_info.IsDrivable()) {
      ref_line_info.SetDrivable(false);
      AWARN << "Reference line " << ref_line_info.Lanes().Id()
            << " traffic decider failed";
    }
  }

  // 执行规划
  status = Plan(start_timestamp, stitching_trajectory, ptr_trajectory_pb);
  ...
}

Status OnLanePlanning::Plan(
    const double current_time_stamp,
    const std::vector& stitching_trajectory,
    ADCTrajectory* const ptr_trajectory_pb) {
  ...
  // 调用具体的Planner(PUBLIC_ROAD)执行
  auto status = planner_->Plan(stitching_trajectory.back(), frame_.get(), ptr_trajectory_pb);  
...
}

上述就是"OnLanePlanning"的执行过程，先是Planner分发器根据配置，选择具体的planner，然后初始化Frame，(PUBLIC_ROAD)planner根据输入帧执行"Plan"方法。

二、Planner

我们先看下Planner目录结构，一共实现了4种Planner：

.
├── BUILD
├── navi_planner_dispatcher.cc
├── navi_planner_dispatcher.h
├── navi_planner_dispatcher_test.cc
├── on_lane_planner_dispatcher.cc
├── on_lane_planner_dispatcher.h
├── on_lane_planner_dispatcher_test.cc
├── planner_dispatcher.cc
├── planner_dispatcher.h
├── planner.h
├── lattice           // lattice planner
├── navi              // navi planner
├── public_road       // public road planner
└── rtk               // rtk planner

可以看到Planner目录分别实现了Planner发布器和具体的Planner，关于发布器我们后面会根据流程图来介绍，这里先介绍一下4种不同的Planner：

1. rtk - 根据录制的轨迹来规划行车路线
2. public_road - 开放道路的轨迹规划器
3. lattice - 基于网格算法的轨迹规划器
4. navi - 基于实时相对地图的规划器

2.1 Planner注册场景

下面我们整理一下planner模块的流程：

 

1. PlanningComponent在cyber中注册
2. 选择Planning
3. 根据不同的Dispatcher，分发Planner

下面我们主要介绍"PublicRoadPlanner"，主要的实现还是在Init和Plan中。

init中主要是初始化场景管理器scenario_manager_。

Status PublicRoadPlanner::Init(const PlanningConfig& config) {
  config_ = config;
  scenario_manager_.Init(config);
  return Status::OK();
}

2.2 Planner运行

接着看"Plan"中的实现：

Status PublicRoadPlanner::Plan(const TrajectoryPoint& planning_start_point, Frame* frame,                               ADCTrajectory* ptr_computed_trajectory){
  // 更新场景，决策当前应该执行什么场景
  scenario_manager_.Update(planning_start_point, *frame);
  // 获取当前场景
  scenario_ = scenario_manager_.mutable_scenario();
  // 执行当前场景的任务
  auto result = scenario_->Process(planning_start_point, frame);
  if (FLAGS_enable_record_debug) {
    auto scenario_debug = ptr_computed_trajectory->mutable_debug()
                              ->mutable_planning_data()
                              ->mutable_scenario();
    scenario_debug->set_scenario_type(scenario_->scenario_type());
    scenario_debug->set_stage_type(scenario_->GetStage());
    scenario_debug->set_msg(scenario_->GetMsg());
  }

  // 当前场景完成
  if (result == scenario::Scenario::STATUS_DONE) {
    // only updates scenario manager when previous scenario's status is
    // STATUS_DONE
    scenario_manager_.Update(planning_start_point, *frame);
  } else if (result == scenario::Scenario::STATUS_UNKNOWN) {
    return Status(common::PLANNING_ERROR, "scenario returned unknown");
  }
  return Status::OK();
}

可以看到"Planner"模块把具体的规划转化成一系列的场景，每次执行规划之前先判断更新当前的场景，然后针对具体的场景去执行。

下面我们先看下"Scenario"模块，然后把这2个模块串起来讲解。

参考文献

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

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