Apollo planning模块pipeline详细分析

1.apollo整体结构

最近利用工作业余时间，抽空学习一下apollo项目的源码。整体来说，apollo的代码量还是挺大，对于入门时间不长的新手来说，逻辑也不是太容易一眼看懂。因此，结合源码以及其他相关理解分析材料，先重点了解了一下apollo中的planning模块也就是规控模块的，把整个planning模块的pipeline大致理清。

本文参考的代码均为apollo 7.0版本，apollo项目github地址上有一张图，反应了整个系统的结构

2.Planning模块框架

planning模块，包含有决策与规划两部分功能。
模块的输入包括：
定位、感知、预测、高精地图、路由、任务管理模块。
输出为：
控制模块可执行的顺滑无碰撞轨迹。

plan模块的整体架构为

整体说来，可以分为三层(参考参考文献4的提法，个人感觉比较合理)
1.可执行代码层。planning模块的入口类为planning_component，·planning_component中会调用合适的planner类，确定相应的车辆意图，执行该意图所需的规划任务并生成规划轨迹。它还将更新未来作业的上下文。

可以看出，planner中包含有lattice/navi/public_road/rtk这几种方式，目前默认的是public_road。

2.app层 每种planner又可以分为多个Scenario。特意查了一下相关资料，Scenario翻译为场景可能比较合适。而每个Scenario又可以分成不同的stage。

我们以bare_intersection_unprotected_scenario为例，该scenario包含有两个stage，一个为approach，一个为cruise。

3.lib层。每个stage，包含有可执行的tasks。Task是Deciders & Optimizers :一组实现决策任务和各种优化的无状态库。优化器负责优化车辆的轨迹和速度。决策者是基于规则的分类决策者，他们建议何时换车道、何时停车、何时爬行(慢速行进)或爬行何时完成。

上面就是task包中的deciders与optimizers。

3.上下游数据pipeline分析

要了解一个模块的逻辑，最好的方式是结合数据流的方向来进行梳理分析。结合代码以及全局的架构图，我们总结了planning模块的输入输出

在planning.component.h文件中，作为入口程序，可以观察到的输入有

 public:
  bool Init() override;

  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;
...

private:
  std::shared_ptr<cyber::Reader<perception::TrafficLightDetection>>
      traffic_light_reader_;
  std::shared_ptr<cyber::Reader<routing::RoutingResponse>> routing_reader_;
  ...
  std::shared_ptr<cyber::Reader<relative_map::MapMsg>> relative_map_reader_;

private部分成员变量，包含有如下输入：
1.traffic_light_reader_ 交通信号灯
2.routing_reader_ 全局导航信息
3.relative_map_reader_ 高清地图
proc方法，包含的输入：
4.prediction_obstacles 预测障碍物大小，轨迹等相关信息
5.chassis 底盘信息
6.localization_estimate 预测的位置信息

而整个模块的输出，在proc方法最后可以看到

  ADCTrajectory adc_trajectory_pb;
  ....
  planning_writer_->Write(adc_trajectory_pb);

最后输出的是ADCTrajectory类型，并通过planning_writer_写入。ADCTrajectory是经过所有逻辑处理的最终结果，包含有车辆行驶所需要的所有信息，这个数据将直接影响到车的自动驾驶行为。

4.代码pipeline之Init方法

前面提到，整个planning模块的入口为planning.component.cc(.h为头文件)，主要包含了两个方法：Init()与Proc()。

首先我们分析Init方法的逻辑。

4.1 选择规划器

bool PlanningComponent::Init() {
  injector_ = std::make_shared<DependencyInjector>();

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

首先根据配置，选择planning类型
1.NaviPlanning，基于地图规划
2.OnLanePlanning，默认规划器，用于public_road。
这两个规划器都有共同基类PlanningBase。

4.2 选择planner

  planning_base_->Init(config_);

规划器选择完毕，然后有planning_base_进行Init()操作。
前面我们提到默认的planning是OnLanePlanning，进入到OnLanePlanning中看一下 Init的操作。

Status OnLanePlanning::Init(const PlanningConfig& config) {
  config_ = config;
  if (!CheckPlanningConfig(config_)) {
    return Status(ErrorCode::PLANNING_ERROR,
                  "planning config error: " + config_.DebugString());
  }

  PlanningBase::Init(config_);

  planner_dispatcher_->Init();
  ...
  // clear planning history
  injector_->history()->Clear();
  // clear planning status
  injector_->planning_context()->mutable_planning_status()->Clear();

  // load map
  hdmap_ = HDMapUtil::BaseMapPtr();
  ACHECK(hdmap_) << "Failed to load map";

  // instantiate reference line provider
  reference_line_provider_ = std::make_unique<ReferenceLineProvider>(
      injector_->vehicle_state(), hdmap_);
  reference_line_provider_->Start();
  // dispatch planner
  planner_ = planner_dispatcher_->DispatchPlanner(config_, injector_);
  ...
  return planner_->Init(config_);
  ...

1.首先通过injector_，将planning的一些历史信息以及状态进行clear。
2.进行load map操作。
3.reference_line_provider_启动，根据导航信息生成的一条或多条道路中心线，即 reference_line
4.planner_dispatcher_进行planner选择。
前面我们提到了四种planner，分别为lattice/navi/public_road/rtk，默认使用的是public_road。
5.代码最后一行是planner_->Init(config_); 比如我们在PublicRoadPlanner中查看，Init代码如下：

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

上面的代码，就是对scenario_manager_进行了注册。

4.3 创建reader/writer

  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_ = node_->CreateReader<TrafficLightDetection>(
      config_.topic_config().traffic_light_detection_topic(),
      [this](const std::shared_ptr<TrafficLightDetection>& traffic_light) {
        ADEBUG << "Received traffic light data: run traffic light callback.";
        std::lock_guard<std::mutex> lock(mutex_);
        traffic_light_.CopyFrom(*traffic_light);
      });

  pad_msg_reader_ = node_->CreateReader<PadMessage>(
      config_.topic_config().planning_pad_topic(),
      [this](const std::shared_ptr<PadMessage>& pad_msg) {
        ADEBUG << "Received pad data: run pad callback.";
        std::lock_guard<std::mutex> lock(mutex_);
        pad_msg_.CopyFrom(*pad_msg);
      });
...
  planning_learning_data_writer_ = node_->CreateWriter<PlanningLearningData>(
      config_.topic_config().planning_learning_data_topic());

  return true;

执行完 planning_base_->Init(config_);以后，开始注册各种reader/writer,为cyber RT框架用于通信的部分，先不做过多关注，至此Init方法结束。

5.Proc方法

5.1 注册local_view

bool PlanningComponent::Proc(
    const std::shared_ptr<prediction::PredictionObstacles>&
        prediction_obstacles,
    const std::shared_ptr<canbus::Chassis>& chassis,
    const std::shared_ptr<localization::LocalizationEstimate>&
        localization_estimate) {
  ACHECK(prediction_obstacles != nullptr);

  // check and process possible rerouting request
  CheckRerouting();

  // process fused input data
  local_view_.prediction_obstacles = prediction_obstacles;
  local_view_.chassis = chassis;
  local_view_.localization_estimate = localization_estimate;
  {
    std::lock_guard<std::mutex> lock(mutex_);
    if (!local_view_.routing ||
        hdmap::PncMap::IsNewRouting(*local_view_.routing, routing_)) {
      local_view_.routing =
          std::make_shared<routing::RoutingResponse>(routing_);
    }
  }
  {
    std::lock_guard<std::mutex> lock(mutex_);
    local_view_.traffic_light =
        std::make_shared<TrafficLightDetection>(traffic_light_);
    local_view_.relative_map = std::make_shared<MapMsg>(relative_map_);
  }
  {
    std::lock_guard<std::mutex> lock(mutex_);
    local_view_.pad_msg = std::make_shared<PadMessage>(pad_msg_);
  }
  {
    std::lock_guard<std::mutex> lock(mutex_);
    local_view_.stories = std::make_shared<Stories>(stories_);
  }
  ...

proc方法，先是对local_view_各种数据进行设置。

查看一下LocalView的源码

/**
 * @struct local_view
 * @brief LocalView contains all necessary data as planning input
 */

struct LocalView {
  std::shared_ptr<prediction::PredictionObstacles> prediction_obstacles;
  std::shared_ptr<canbus::Chassis> chassis;
  std::shared_ptr<localization::LocalizationEstimate> localization_estimate;
  std::shared_ptr<perception::TrafficLightDetection> traffic_light;
  std::shared_ptr<routing::RoutingResponse> routing;
  std::shared_ptr<relative_map::MapMsg> relative_map;
  std::shared_ptr<PadMessage> pad_msg;
  std::shared_ptr<storytelling::Stories> stories;
};

}  // namespace planning
}  // namespace apollo

根据上面的注释，不难看出，planning模块所需的所有input都在该结构体中。

5.2 Planning_base_->RunOnce()

  ADCTrajectory adc_trajectory_pb;
  planning_base_->RunOnce(local_view_, &adc_trajectory_pb);
  ...
  planning_writer_->Write(adc_trajectory_pb);

注册完毕localview以后，重点就是planning_base_->RunOnce，这是planning真正的计算逻辑所在，输入为local_view_与adc_trajectory_pb， 输出为planning_writer_->Write(adc_trajectory_pb)。

6.RunOnce

我们看下on_lane_planning中的RunOnce方法。

void OnLanePlanning::RunOnce(const LocalView& local_view,
                             ADCTrajectory* const ptr_trajectory_pb) {
  // when rerouting, reference line might not be updated. In this case, planning
  // module maintains not-ready until be restarted.
  static bool failed_to_update_reference_line = false;
  local_view_ = local_view;
  const double start_timestamp = Clock::NowInSeconds();
  const double start_system_timestamp =
      std::chrono::duration<double>(
          std::chrono::system_clock::now().time_since_epoch())
          .count();
  ...
   // Update reference line provider and reset pull over if necessary
  reference_line_provider_->UpdateVehicleState(vehicle_state);
  ...
  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);
  for (const auto& p : ptr_trajectory_pb->trajectory_point()) {
    ADEBUG << p.DebugString();
  }
  ...
    if (!status.ok()) {
    status.Save(ptr_trajectory_pb->mutable_header()->mutable_status());
    AERROR << "Planning failed:" << status.ToString();
    if (FLAGS_publish_estop) {
      AERROR << "Planning failed and set estop";
      // "estop" signal check in function "Control::ProduceControlCommand()"
      // estop_ = estop_ || local_view_.trajectory.estop().is_estop();
      // we should add more information to ensure the estop being triggered.
      EStop* estop = ptr_trajectory_pb->mutable_estop();
      estop->set_is_estop(true);
      estop->set_reason(status.error_message());
    }
  }
  ...
    if (FLAGS_enable_planning_smoother) {
      planning_smoother_.Smooth(injector_->frame_history(), frame_.get(),
                                ptr_trajectory_pb);
    }

因为方法代码较长，有比较多的debug相关的msg,因此把代码中核心部分逻辑选出来进行解读。

1.根据注释不难看出，reference_line_provider_会更新reference_line。如果失败，会生成一个停车轨迹。
2.InitFrame方法，主要是生成reference_lines。
3.for循环中有traffic_decider，主要是与交规相关的决策内容。
4.status = Plan(start_timestamp, stitching_trajectory, ptr_trajectory_pb);,plan的主要逻辑，后面我们再讲。
5.规划失败，执行estop()。
6.最后一步，对轨迹进行平滑处理。

7.PublicRoadPlanner::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();
}

上面是PublicRoadPlanner中的Plan方法源码。
主要是包含有两个步骤：
1.scenario_manager_.Update(planning_start_point, *frame);，这一句用来更新当前的场景。
2.auto result = scenario_->Process(planning_start_point, frame)
前面我们已经提到，apollo中，scenario->stage->task是层层递进关系，即驾驶中有许多scenario(场景)，每个场景分为一个或者多个stage(阶段)，每个阶段又是由多个task(任务)组成。

进入到Process方法

Scenario::ScenarioStatus Scenario::Process(
    const common::TrajectoryPoint& planning_init_point, Frame* frame) {
  if (current_stage_ == nullptr) {
    AWARN << "Current stage is a null pointer.";
    return STATUS_UNKNOWN;
  }
  if (current_stage_->stage_type() == ScenarioConfig::NO_STAGE) {
    scenario_status_ = STATUS_DONE;
    return scenario_status_;
  }
  auto ret = current_stage_->Process(planning_init_point, frame);

以LaneFollowStage中的 Process方法为例

Stage::StageStatus LaneFollowStage::Process(
    const TrajectoryPoint& planning_start_point, Frame* frame) {
  bool has_drivable_reference_line = false;

  ADEBUG << "Number of reference lines:\t"
         << frame->mutable_reference_line_info()->size();

  unsigned int count = 0;

  for (auto& reference_line_info : *frame->mutable_reference_line_info()) {
    // TODO(SHU): need refactor
    if (count++ == frame->mutable_reference_line_info()->size()) {
      break;
    }
  ...
  auto cur_status =
        PlanOnReferenceLine(planning_start_point, frame, &reference_line_info);
  ...

for循环中，遍历reference_line_info，当前road的每条lane对应一条reference_line。
然后，调用PlanOnReferenceLine方法。

Status LaneFollowStage::PlanOnReferenceLine(
    const TrajectoryPoint& planning_start_point, Frame* frame,
    ReferenceLineInfo* reference_line_info) {
  if (!reference_line_info->IsChangeLanePath()) {
    reference_line_info->AddCost(kStraightForwardLineCost);
  }
  ADEBUG << "planning start point:" << planning_start_point.DebugString();
  ADEBUG << "Current reference_line_info is IsChangeLanePath: "
         << reference_line_info->IsChangeLanePath();

  auto ret = Status::OK();
  for (auto* task : task_list_) {
    const double start_timestamp = Clock::NowInSeconds();

    ret = task->Execute(frame, reference_line_info);
    ...
      if (!ret.ok()) {
      AERROR << "Failed to run tasks[" << task->Name()
             << "], Error message: " << ret.error_message();
      break;
    }
    ...
    reference_line_info->set_trajectory_type(ADCTrajectory::NORMAL);
  if (!ret.ok()) {
    PlanFallbackTrajectory(planning_start_point, frame, reference_line_info);
  }
  ...
   // determine if there is a destination on reference line.
  double dest_stop_s = -1.0;
  for (const auto* obstacle :
       reference_line_info->path_decision()->obstacles().Items()) {
    if (obstacle->LongitudinalDecision().has_stop() &&
        obstacle->LongitudinalDecision().stop().reason_code() ==
            STOP_REASON_DESTINATION) {
      SLPoint dest_sl = GetStopSL(obstacle->LongitudinalDecision().stop(),
                                  reference_line_info->reference_line());
      dest_stop_s = dest_sl.s();
    }
  }
  ...

通过PlanOnReferenceLine代码不难看出，里面核心的逻辑，就是位于ret = task->Execute(frame, reference_line_info);。如果ret失败了，再进行fallback操作。

8.总结

以上，就是planning模块运行的整个pipeline，包含了读取配置文件选择合适的planner类型，更新scenario类型，再遍历stage执行task的全过程。当然我们上面只是梳理清楚代码运行逻辑，对于具体的算法实践并未做分析，关于具体的算法实现，后面再来进行解读。

参考文献

1.https://github.com/ApolloAuto/apollo
2.https://github.com/ApolloAuto/apollo/blob/master/modules/planning/README_cn.md
3.https://zhuanlan.zhihu.com/p/266116528
4.https://blog.csdn.net/Cxiazaiyu/article/details/109766770