Baidu Apollo代码解析之Planning的结构与调用流程（2）

大家好，我已经把CSDN上的博客迁移到了知乎上，欢迎大家在知乎关注我的专栏慢慢悠悠小马车（https://zhuanlan.zhihu.com/duangduangduang）。希望大家可以多多交流，互相学习。

---

Apollo5.0的Planning模块是基于Scenario、Stage、Task这样的层次组织的，针对不同的场景设计不同的算法细节。Scenario指一个特定的问题或场景，Stage指在一个Scenario下的规划方法的粗略步骤，Task指一个具体的处理方法。相应地，一个Scenario包含多个Stage，一个Stage包含多个Task。那我们按照由下向上的顺序展开说。

//Scenario类中包含Stage
std::unordered_map<ScenarioConfig::StageType,
                     const ScenarioConfig::StageConfig*, std::hash<int>>
      stage_config_map_;

//Stage类中包含Task
std::map<TaskConfig::TaskType, std::unique_ptr<Task>> tasks_;
std::vector<Task*> task_list_;

1. Task

在apollo/modules/planning/tasks文件夹中，Task分为4类：deciders，optimizers，rss，smoothers。task.h定义了Task基类，其中重要的是2个Execute()函数。

virtual common::Status Execute(Frame* frame,
                               ReferenceLineInfo* reference_line_info);
virtual common::Status Execute(Frame* frame);

TaskFactory类是为了方便的构造Task对象。

decider.h中定义了Decider类，继承自Task类，对应着继承而来的2个Execute()，分别定义了2个Process()，即Task的执行是通过Decider::Process()运行的。

apollo::common::Status Decider::Execute(
    Frame* frame, ReferenceLineInfo* reference_line_info) {
  Task::Execute(frame, reference_line_info);
  return Process(frame, reference_line_info);
}

apollo::common::Status Decider::Execute(Frame* frame) {
  Task::Execute(frame);
  return Process(frame);
}

不同的Decider如CreepDecider类等，都是继承自Decider类，分别实现自己的Process()，如CreepDecider::Process()。比较特殊的是PathDecider和SpeedDecider类是直接继承自Task类的，因此要实现自己的Execute()。

Optimizer有3种，都继承自Task类，分别是：PathOptimizer，SpeedOptimizer，TrajectoryOptimizer。这3种Optimizer实现了各自的Execute()，并定义了纯虚函数Process()在Execute()中被调用，由子类实现具体的Process()。不同的优化器，如PiecewiseJerkPathOptimizer是继承自PathOptimizer的，实现自己的Process()。

RssDecider继承自Task类，实现了Execute()。

Smoother是个特例，并没有继承Task类，关键函数Smooth()。

2. Stage

scenarios文件夹中包含了多种场景，内部的每个文件夹就是一个scenario的定义和解决。首先看Stage类的定义，主要的处理都在Stage::Process()中（此处是纯虚函数）。

class Stage {
 public:
  ...
  /**
   * @brief Each stage does its business logic inside Process function.
   * If the stage want to transit to a different stage after finish,
   * it should set the type of 'next_stage_'.
   */
  virtual StageStatus Process(
      const common::TrajectoryPoint& planning_init_point, Frame* frame) = 0;
  /**
   * @brief The sequence of tasks inside the stage. These tasks usually will be
   * executed in order.
   */
  const std::vector<Task*>& TaskList() const { return task_list_; }  
  ScenarioConfig::StageType NextStage() const { return next_stage_; }

 protected:
  bool ExecuteTaskOnReferenceLine(
      const common::TrajectoryPoint& planning_start_point, Frame* frame);
  bool ExecuteTaskOnOpenSpace(Frame* frame);
  ...
 protected:
  std::map<TaskConfig::TaskType, std::unique_ptr<Task>> tasks_;
  std::vector<Task*> task_list_;  
  ScenarioConfig::StageType next_stage_;
  ...
};

这里以scenarios/park/valet_parking举例，定义了继承自Scenario类的ValetParkingScenario类，并注册了2个Stage：VALET_PARKING_APPROACHING_PARKING_SPOT和VALET_PARKING_PARKING，分别对应继承自Stage类的StageApproachingParkingSpot类和StageParking类。即想停车，第一步先将车行驶到接近停车位，第二步停入停车位。我们上面提到了一个Stage包含一个Task List，是在哪设定的呢？其实是在apollo/modules/planning/conf/scenario/valet_parking_config.pb.txt中。在StageApproachingParkingSpot::Process()中，会调用Stage::ExecuteTaskOnReferenceLine()，而在Stage::ExecuteTaskOnReferenceLine()中，又会遍历Task List，对每一个Task执行task->Execute()。

3. Scenario 

再看Scenario类的定义。在Scenario::Process()中，通过调用Stage::Process()来处理该stage所包含的task。当该stage处理完成时，就切换到下一个stage。只要当前的stage不是空、有意义，scenario就是“未完成”的状态，从而可以继续执行接下来的Stage。当前的stage是空，则所有的stage处理完成了，scenario才处理完毕。

Scenario::ScenarioStatus Scenario::Process(
    const common::TrajectoryPoint& planning_init_point, Frame* frame) {
  ...
  //在Stage中处理Task List，返回Stage的状态
  auto ret = current_stage_->Process(planning_init_point, frame);
  switch (ret) {
    ...
    case Stage::RUNNING: {
      scenario_status_ = STATUS_PROCESSING;
      break;
    }
    case Stage::FINISHED: {
      auto next_stage = current_stage_->NextStage();      
      ...
      if (current_stage_ != nullptr &&
          current_stage_->stage_type() != ScenarioConfig::NO_STAGE) {
        //只要current_stage_不是空、有意义，scenario_status_就是“未完成”，
        //从而可以继续执行接下来的Stage
        scenario_status_ = STATUS_PROCESSING;
      } else {
        scenario_status_ = STATUS_DONE;
      }
      break;
    }
    ...
  }
  return scenario_status_;
}

这里还是以valet_parking为例，给出函数的调用与实际执行的层次、次序。其他情况类似。

4. ScenarioManager

ScenarioManager类用来管理各个Scenario的判别和切换，通过ScenarioManager::Update() 进而调用 ScenarioManager::ScenarioDispatch() 来改变当前对应的scenario。