Apollo 3.5 Planning模块源代码分析_apollo 3.0 planning

严正声明：本文系作者davidhopper原创，未经允许，严禁转载！

说明：
1. 本文基于Apollo master分支2019年5月8日的代码（commit 2025a67874ecb3ef333b54079723a8f6738d81b5）分析；
2. 根据Apollo团队的最新开发思路，今后只会保留一个规划算法：PublicRoadPlanner。目前，OpenSpacePlanner算法已从master分支删除，相关算法逻辑已合并到PublicRoadPlanner。其他算法也随时可能被删除。
3. Apollo官方文档阐述了场景规划的概念。

我于2018年1月初写过一篇博客《Apollo Planning模块源代码分析》，当时的Apollo项目基于ROS实现任务调度与模块通信。2019年1月发布的Apollo 3.5基于百度自研的实时计算框架Cyber RT完成任务调度与模块通信，并且Planning模块的内部架构也发生了重大变化。因应上述变化，本文对Planning模块的主要逻辑进行剖析，期望给感兴趣的同学带来帮助。

规划（Planning）模块位于命名空间：apollo::planning，其作用在于构建无人车从起点到终的局部行驶路径，具体而言，就是给定导航地图、导航路径、当前定位点、车辆状态（包括：位置、速度、加速度、底盘）、 周边目标的感知及预测信息（如交通标志和障碍物等），规划模块计算出可供控制模块（Controller）执行的一条安全且舒适的行驶路径。注意，规划模块输出的路径是局部路径而非全局路径。举个简单示例加以说明，假如无人车需从长沙智能驾驶研究院行驶至长沙高铁南站，首先需借助Routing（路由寻径）模块输出全局导航路径，接下来才是规划模块基于全局导航路径进行一小段、一小段具体行驶路径的规划。规划模块内部结构及其与其他模块的交互示意如下图所示。

Apollo 3.5 规划模块内部的主要类成员如下图所示：

该模块的主要执行流程如下图所示：

一、模块主入口

根据我的另一篇博客Apollo 3.5 各功能模块的启动过程解析中的分析，Planning功能模块的启动命令为：

/apollo/bazel-bin/cyber/mainboard -p compute_sched -d /apollo/modules/planning/dag/planning.dag
1

-p compute_sched表明使用配置文件/apollocyber/conf/compute_sched.conf 进行任务调度，该参数可忽略，-d /apollo/modules/planning/dag/planning.dag表明动态加载的是Planning模块。上述启动命令表明，Planning模块的主入口为：/apollo/cyber/mainboard/mainboard.cc：

int main(int argc, char** argv) {
  google::SetUsageMessage("we use this program to load dag and run user apps.");

  // parse the argument
  ModuleArgument module_args;
  module_args.ParseArgument(argc, argv);

  // initialize cyber
  apollo::cyber::Init(argv[0]);

  // start module
  ModuleController controller(module_args);
  if (!controller.Init()) {
    controller.Clear();
    AERROR << "module start error.";
    return -1;
  }

  apollo::cyber::WaitForShutdown();
  controller.Clear();
  AINFO << "exit mainboard.";

  return 0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24

main函数十分简单，首先是解析参数，初始化cyber环境，接下来创建一个ModuleController类对象controller，之后调用controller.Init()启动相关功能模块。最后，进入Cyber RT的消息循环，直到等待cyber::WaitForShutdown()返回，清理资源并退出main函数。ModuleController::Init()函数十分简单，内部调用了ModuleController::LoadAll()函数：

bool ModuleController::LoadAll() {
  const std::string work_root = common::WorkRoot();
  const std::string current_path = common::GetCurrentPath();
  const std::string dag_root_path = common::GetAbsolutePath(work_root, "dag");

  for (auto& dag_conf : args_.GetDAGConfList()) {
    std::string module_path = "";
    if (dag_conf == common::GetFileName(dag_conf)) {
      // case dag conf argument var is a filename
      module_path = common::GetAbsolutePath(dag_root_path, dag_conf);
    } else if (dag_conf[0] == '/') {
      // case dag conf argument var is an absolute path
      module_path = dag_conf;
    } else {
      // case dag conf argument var is a relative path
      module_path = common::GetAbsolutePath(current_path, dag_conf);
      if (!common::PathExists(module_path)) {
        module_path = common::GetAbsolutePath(work_root, dag_conf);
      }
    }
    AINFO << "Start initialize dag: " << module_path;
    if (!LoadModule(module_path)) {
      AERROR << "Failed to load module: " << module_path;
      return false;
    }
  }
  return true;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28

上述函数处理一个dag_conf配置文件循环，读取配置文件中的所有dag_conf，并逐一调用bool ModuleController::LoadModule(const std::string& path)函数加载功能模块。该函数的作用将在第二部分详细阐述。

二、apollo::planning::PlanningComponent类对象的创建过程

Cyber RT使用工厂设计模式创建apollo::planning::PlanningComponent类对象，创建过程涉及到的主要类图如下所示：

2.1 apollo::planning::PlanningComponent类的注册过程

使用工厂模式动态创建apollo::planning::PlanningComponent类对象，应先生成一个与之对应的工厂类apollo::cyber::class_loader::utility::ClassFactory<apollo::planning::PlanningComponent, apollo::cyber::ComponentBase >，并将其加入到工厂集合类 std::map<std::string, utility::AbstractClassFactoryBase*>中，这个就是 apollo::planning::PlanningComponent类的注册过程，下面具体阐述之，主要流程如下所示：

Cyber RT使用宏CYBER_REGISTER_COMPONENT(PlanningComponent)将apollo::planning::PlanningComponent类注册到组件类管理器。查看源代码可知：

#define CYBER_REGISTER_COMPONENT(name) \
  CLASS_LOADER_REGISTER_CLASS(name, apollo::cyber::ComponentBase)
1
2

而后者的定义为：

#define CLASS_LOADER_REGISTER_CLASS(Derived, Base) \
  CLASS_LOADER_REGISTER_CLASS_INTERNAL_1(Derived, Base, __COUNTER__)
1
2

继续展开得到：

#define CLASS_LOADER_REGISTER_CLASS_INTERNAL_1(Derived, Base, UniqueID) \
  CLASS_LOADER_REGISTER_CLASS_INTERNAL(Derived, Base, UniqueID)
1
2

仍然需要进一步展开：

#define CLASS_LOADER_REGISTER_CLASS_INTERNAL(Derived, Base, UniqueID)         \
  namespace {                                                                 \
  struct ProxyType##UniqueID {                                                \
    ProxyType##UniqueID() {                                                   \
      apollo::cyber::class_loader::utility::RegisterClass<Derived, Base>(     \
          #Derived, #Base);                                                   \
    }                                                                         \
  };                                                                          \
  static ProxyType##UniqueID g_register_class_##UniqueID;                     \
  }
1
2
3
4
5
6
7
8
9
10

将PlanningComponent代入上述宏，最终得到：

  namespace {                                                                 
  struct ProxyType__COUNTER__ {                                                
    ProxyType__COUNTER__() {                                                   
      apollo::cyber::class_loader::utility::RegisterClass<PlanningComponent, apollo::cyber::ComponentBase>( 
          "PlanningComponent", "apollo::cyber::ComponentBase");                                                   
    }                                                                         
  };   
  // 通过该静态变量来触发RegisterClass函数，完成PlanningComponent对应的工厂类对象的注册                     
  static ProxyType__COUNTER__ g_register_class___COUNTER__;                     
  }
1
2
3
4
5
6
7
8
9
10

注意两点：第一，上述定义位于namespace apollo::planning内；第二，___COUNTER__是C语言的一个计数器宏，这里仅代表一个占位符，实际展开时可能就是1387之类的数字，亦即ProxyType__COUNTER__实际上应为ProxyType1387之类的命名。上述代码简洁明了，首先定义一个结构体ProxyType1387，该结构体仅包含一个构造函数，在内部调用apollo::cyber::class_loader::utility::RegisterClass<PlanningComponent, apollo::cyber::ComponentBase>注册apollo::cyber::ComponentBase类的派生类PlanningComponent。定义一个静态全局结构体对象ProxyType1387：g_register_class1387，这样就能调用构造函数完成注册。
通过文本搜索可以发现，除了在此处定义了一个包含关键字g_register_class___的静态全局结构体对象外，其他没有任何地方调用这个静态全局结构体对象。为什么要定义这样一个看似多余的变量？因为我们需要调用该结构体的构造函数，再通过构造函数内部的RegisterClass函数来实现PlanningComponent对应的工厂类的注册。如果不定义这个静态全局结构体对象，那么RegisterClass函数就不会被调用，工厂类的注册也就无法完成。
继续观察apollo::cyber::class_loader::utility::RegisterClass函数：

template <typename Derived, typename Base>
void RegisterClass(const std::string& class_name,
                   const std::string& base_class_name) {
  AINFO << "registerclass:" << class_name << "," << base_class_name << ","
        << GetCurLoadingLibraryName();

  // 创建产品类Derived对应的工厂类对象指针
  utility::AbstractClassFactory<Base>* new_class_factrory_obj =
      new utility::ClassFactory<Derived, Base>(class_name, base_class_name);
  // 设置工厂类对象的加载器，内部是一个静态变量，初始值为空指针，会在之后加载动态库调用的LoadLibrary函数中设置正确的对象指针
  new_class_factrory_obj->AddOwnedClassLoader(GetCurActiveClassLoader());
  // 设置工厂类对象的加载库名称，内部是一个静态变量，初始值为空字符串，会在之后加载动态库调用的LoadLibrary函数中设置正确的名称
  new_class_factrory_obj->SetRelativeLibraryPath(GetCurLoadingLibraryName());

  GetClassFactoryMapMapMutex().lock();
  // 获取工厂映射，该映射对应的key是class_name，value是工厂对象指针
  ClassClassFactoryMap& factory_map =
      GetClassFactoryMapByBaseClass(typeid(Base).name());
  factory_map[class_name] = new_class_factrory_obj;
  GetClassFactoryMapMapMutex().unlock();
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21

该函数创建一个模板类utility::ClassFactory<Derived, Base>对象new_class_factrory_obj，为其添加类加载器（new_class_factrory_obj->AddOwnedClassLoader(GetCurActiveClassLoader())），设置加载库的路径名（new_class_factrory_obj->SetRelativeLibraryPath(GetCurLoadingLibraryName())），获取工厂映射factory_map（类型为ClassClassFactoryMap，key为待创建类对象的基类名称字符串，value为创建类对象的工厂类对象指针），并将工厂类对象加入到factory_map统一管理。

GetCurActiveClassLoader()函数内部代码如下：

ClassLoader*& GetCurActiveClassLoaderReference() {
  static ClassLoader* loader = nullptr;
  return loader;
}

ClassLoader* GetCurActiveClassLoader() {
  return (GetCurActiveClassLoaderReference());
}
1
2
3
4
5
6
7
8

可见GetCurActiveClassLoader()函数返回的是一个静态ClassLoader对象指针，该对象指针的初始值是nullptr，在其他地方一定有函数对该值进行了设值。果不其然，在SetCurActiveClassLoader()函数中进行了设值：

void SetCurActiveClassLoader(ClassLoader* loader) {
  ClassLoader*& loader_ref = GetCurActiveClassLoaderReference();
  loader_ref = loader;
}
1
2
3
4

GetCurLoadingLibraryName()函数内部代码如下：

std::string& GetCurLoadingLibraryNameReference() {
  static std::string library_name;
  return library_name;
}

std::string GetCurLoadingLibraryName() {
  return GetCurLoadingLibraryNameReference();
}
1
2
3
4
5
6
7
8

可见GetCurLoadingLibraryName()函数返回的是一个静态std::string对象，该对象的初始值是空字符串，在其他地方一定有函数对该值进行了设值。果不其然，在SetCurLoadingLibraryName()函数中进行了设值：

void SetCurLoadingLibraryName(const std::string& library_name) {
  std::string& library_name_ref = GetCurLoadingLibraryNameReference();
  library_name_ref = library_name;
}
1
2
3
4

最终，在bool LoadLibrary(const std::string& library_path, ClassLoader* loader)函数中（位于cyber/class_loader/utility/class_loader_utility.cc文件）进行了设置类加载器和加载库路径名的调用：

bool LoadLibrary(const std::string& library_path, ClassLoader* loader) {
  // ...

  SharedLibraryPtr shared_library = nullptr;
  static std::recursive_mutex loader_mutex;
  {
    std::lock_guard<std::recursive_mutex> lck(loader_mutex);

    try {
      // 设置类加载器
      SetCurActiveClassLoader(loader);
      // 设置当前加载的库名称
      SetCurLoadingLibraryName(library_path);
      shared_library = SharedLibraryPtr(new SharedLibrary(library_path));
    } catch (const LibraryLoadException& e) {
      SetCurLoadingLibraryName("");
      SetCurActiveClassLoader(nullptr);
      AERROR << "LibraryLoadException: " << e.what();
    } catch (const LibraryAlreadyLoadedException& e) {
      SetCurLoadingLibraryName("");
      SetCurActiveClassLoader(nullptr);
      AERROR << "LibraryAlreadyLoadedException: " << e.what();
    } catch (const SymbolNotFoundException& e) {
      SetCurLoadingLibraryName("");
      SetCurActiveClassLoader(nullptr);
      AERROR << "SymbolNotFoundException: " << e.what();
    }

    SetCurLoadingLibraryName("");
    SetCurActiveClassLoader(nullptr);
  }

  // ...
  return true;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35

GetClassFactoryMapByBaseClass()函数内部代码如下：

using ClassClassFactoryMap =
    std::map<std::string, utility::AbstractClassFactoryBase*>;
using BaseToClassFactoryMapMap = std::map<std::string, ClassClassFactoryMap>;

ClassClassFactoryMap& GetClassFactoryMapByBaseClass(
    const std::string& typeid_base_class_name) {
  BaseToClassFactoryMapMap& factoryMapMap = GetClassFactoryMapMap();
  std::string base_class_name = typeid_base_class_name;
  if (factoryMapMap.find(base_class_name) == factoryMapMap.end()) {
    factoryMapMap[base_class_name] = ClassClassFactoryMap();
  }

  return factoryMapMap[base_class_name];
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14

BaseToClassFactoryMapMap是一个映射，key为待创建对象的基类名称字符串，value为ClassClassFactoryMap对象；ClassClassFactoryMap也是一个映射，key为待创建对象的自身名称字符串，value为待创建对象对应的抽象工厂基类对象指针。GetClassFactoryMapByBaseClass()函数的作用是返回待创建对象对应的ClassClassFactoryMap对象，以便将当前的工厂对象指针塞进映射中。

通过apollo::cyber::class_loader::utility::RegisterClass函数，我们可以清楚地看到，Cyber RT不直接创建产品类对象（例如PlanningComponent），而是使用工厂方法模式来完成产品类对象的创建，至于如何调用工厂对象创建PlanningComponent对象，这是下一节要讨论的内容。

2.2 apollo::planning::PlanningComponent类对象的动态创建过程

第一部分介绍模块主入口时，提及bool ModuleController::LoadModule(const std::string& path)函数，正是该函数动态创建出了apollo::planning::PlanningComponent类对象。创建流程如下图所述：

函数内部调用分析如下：

bool ModuleController::LoadModule(const std::string& path) {
  DagConfig dag_config;
  if (!common::GetProtoFromFile(path, &dag_config)) {
    AERROR << "Get proto failed, file: " << path;
    return false;
  }
  return LoadModule(dag_config);
}
1
2
3
4
5
6
7
8

上述函数从磁盘配置文件读取配置信息，并调用bool ModuleController::LoadModule(const DagConfig& dag_config)函数加载功能模块：

bool ModuleController::LoadModule(const DagConfig& dag_config) {
  const std::string work_root = common::WorkRoot();

  for (auto module_config : dag_config.module_config()) {
    std::string load_path;
    // ...
    class_loader_manager_.LoadLibrary(load_path);
    for (auto& component : module_config.components()) {
      const std::string& class_name = component.class_name();
      std::shared_ptr<ComponentBase> base =
          class_loader_manager_.CreateClassObj<ComponentBase>(class_name);
      if (base == nullptr) {
        return false;
      }

      if (!base->Initialize(component.config())) {
        return false;
      }
      component_list_.emplace_back(std::move(base));
    }

    // ...
  }
  return true;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25

已经知道，PlanningComponent对象是通过class_loader_manager_.CreateClassObj<ComponentBase>(class_name)创建出来的，而class_loader_manager_是一个class_loader::ClassLoaderManager类对象。现在的问题是：class_loader::ClassLoaderManager与2.1节中的工厂类utility::AbstractClassFactory<Base>如何联系起来的？
先看ClassLoaderManager::CreateClassObj函数（位于文件cyber/class_loader/class_loader_manager.h中）：

template <typename Base>
std::shared_ptr<Base> ClassLoaderManager::CreateClassObj(
    const std::string& class_name) {
  std::vector<ClassLoader*> class_loaders = GetAllValidClassLoaders();
  for (auto class_loader : class_loaders) {
    if (class_loader->IsClassValid<Base>(class_name)) {
      return (class_loader->CreateClassObj<Base>(class_name));
    }
  }
  AERROR << "Invalid class name: " << class_name;
  return std::shared_ptr<Base>();
}
1
2
3
4
5
6
7
8
9
10
11
12

上述函数中，从所有class_loaders中找出一个正确的class_loader，并调用class_loader->CreateClassObj<Base>(class_name)（位于文件cyber/class_loader/class_loader.h中）创建功能模块组件类对象：

template <typename Base>
std::shared_ptr<Base> ClassLoader::CreateClassObj(
    const std::string& class_name) {
  if (!IsLibraryLoaded()) {
    LoadLibrary();
  }

  Base* class_object = utility::CreateClassObj<Base>(class_name, this);
  if (nullptr == class_object) {
    AWARN << "CreateClassObj failed, ensure class has been registered. "
          << "classname: " << class_name << ",lib: " << GetLibraryPath();
    return std::shared_ptr<Base>();
  }

  std::lock_guard<std::mutex> lck(classobj_ref_count_mutex_);
  classobj_ref_count_ = classobj_ref_count_ + 1;
  std::shared_ptr<Base> classObjSharePtr(
      class_object, std::bind(&ClassLoader::OnClassObjDeleter<Base>, this,
                              std::placeholders::_1));
  return classObjSharePtr;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21

上述函数继续调用utility::CreateClassObj<Base>(class_name, this)（位于文件cyber/class_loader/utility/class_loader_utility.h中）创建功能模块组件类对象：

template <typename Base>
Base* CreateClassObj(const std::string& class_name, ClassLoader* loader) {
  GetClassFactoryMapMapMutex().lock();
  ClassClassFactoryMap& factoryMap =
      GetClassFactoryMapByBaseClass(typeid(Base).name());
  AbstractClassFactory<Base>* factory = nullptr;
  if (factoryMap.find(class_name) != factoryMap.end()) {
    factory = dynamic_cast<utility::AbstractClassFactory<Base>*>(
        factoryMap[class_name]);
  }
  GetClassFactoryMapMapMutex().unlock();

  Base* classobj = nullptr;
  if (factory && factory->IsOwnedBy(loader)) {
    classobj = factory->CreateObj();
  }

  return classobj;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

上述函数使用factory = dynamic_cast<utility::AbstractClassFactory<Base>*>(factoryMap[class_name]);获取对应的工厂对象指针，至此终于将class_loader::ClassLoaderManager与2.1节中的工厂类utility::AbstractClassFactory<Base>联系起来了。工厂类对象指针找到后，使用classobj = factory->CreateObj();就顺理成章地将PlanningComponent类对象创建出来了。

三、具体规划算法分析

3.1 概述

Apollo 3.5将规划分为两种模式：OnLanePlanning（车道规划，可用于城区及高速公路各种复杂道路）NaviPlanning（导航规划，主要用于高速公路），包含四种具体规划算法：PublicRoadPlanner（即以前的EMPlanner，是Apollo 3.5的主用规划算法）、LatticePlanner（Apollo 3.5的第二重要规划算法，成熟度不足，里面的一些优秀算法思想将被融合到PublicRoadPlanner中，今后该算法将不再维护）、NaviPlanner（主要用于高速公路巡航场景）、RTKPlanner（循迹算法，一般不用。如需循迹，可使用Python脚本程序modules/tools/record_play/rtk_player.py），具体类图如下所示：

根据Apollo团队的最新开发思路，今后只会保留一个规划算法：PublicRoadPlanner，下面对该算法进行具体介绍。

3.2 PublicRoadPlanner规划算法分析

PublicRoadPlanner算法就是Apollo 3.0以下版本中的EMPlanner算法，一般用于OnLanePlanning模式。该算法的架构如下所示（基于百度布道师胡旷老师主讲的《Apollo 3.5自动驾驶开源平台的技术探索》课程思想绘制）：

PublicRoadPlanner算法从Routing模块输出的高精地图Lane序列获得全局导航路径，基于场景(Scenario)的理念进行局部行驶轨迹规划。具体而言，将公共道路行驶划分为BareIntersectionUnprotectedScenario(裸露交叉路口无保护场景，即没有红绿灯及交通标志的交叉路口场景，感谢Apollo美研团队Yifei Jiang老师的答疑)、LaneFollowScenario(跟车场景)、NarrowStreetUTurnScenario(狭窄街道调头场景，暂未实现)、SidePassScenario(侧向通行场景，即前方有停止车辆，借道绕行后再回原车道)、StopSignUnprotectedScenario(停止标志无保护场景)、TrafficLightProtectedScenario(红绿灯保护场景)、TrafficLightUnprotectedLeftTurnScenario(红绿灯无保护左转弯场景)、TrafficLightUnprotectedRightTurnScenario(红绿灯无保护右转弯场景)、PullOverScenario（靠边停车场景）、ValetParkingScenario(泊车场景)等多个场景。
每个场景又包含若干个阶段(Stage)，在每个阶段均使用若干个任务(Task)生成局部行驶轨迹。具体而言，BareIntersectionUnprotectedScenario场景包含BareIntersectionUnprotectedStageApproach、BareIntersectionUnprotectedStageIntersectionCruise两个阶段；LaneFollowScenario场景包含LaneFollowStage一个阶段；SidePassScenario场景包含StageApproachObstacle、StageDetectSafety、StageGeneratePath、StageStopOnWaitPoint、StagePassObstacle、StageBackup六个阶段；StopSignUnprotectedScenario场景包含StopSignUnprotectedStagePreStop、StopSignUnprotectedStageStop、StopSignUnprotectedStageCreep、StopSignUnprotectedStageIntersectionCruise四个阶段；TrafficLightProtectedScenario场景包含TrafficLightProtectedStageApproach、TrafficLightProtectedStageIntersectionCruise两个阶段；TrafficLightUnprotectedLeftTurnScenario场景包含TrafficLightUnprotectedLeftTurnStageStop、TrafficLightUnprotectedLeftTurnStageCreep、TrafficLightUnprotectedLeftTurnStageIntersectionCruise三个阶段；TrafficLightUnprotectedRightTurnScenario场景包含TrafficLightUnprotectedRightTurnStageStop、TrafficLightUnprotectedRightTurnStageCreep、TrafficLightUnprotectedRightTurnStageIntersectionCruise三个阶段；PullOverScenario场景包含PullOverStageApproach一个阶段（尚未开发完毕）；ValetParkingScenario场景包含StageApproachingParkingSpot、StageParking两个阶段。任务分为决策（Decider）与优化（Optimizer ）两类，其中决策类任务包含PathLaneBorrowDecider,
SpeedLimitDecider等（所有决策类任务均包含于modules/planning/tasks/deciders目录），优化类任务包含DpPolyPathOptimizer、DpStSpeedOptimizer等（所有优化类任务均包含于modules/planning/tasks/optimizers目录）。任意一个场景中的任意一个阶段均会利用上述两类任务的若干种。例如，BareIntersectionUnprotectedScenario场景中的BareIntersectionUnprotectedStageApproach阶段使用了PathDecider、SpeedBoundsDecider、DpStSpeedOptimizer、SpeedDecider、SpeedBoundsDecider等决策任务及DpPolyPathOptimizer、DpPolyPathOptimizer等优化任务（见配置文件modules/planning/conf/scenario/bare_intersection_unprotected_config.pb.txt）。
基于场景(Scenario)、阶段(Stage)和任务(Task)的理念进行规划，优点是能合理有效地应对每种场景，易于扩充，并且基于配置文件动态增减场景、阶段及使用的任务，灵活性强；缺点是可能会遗漏一些特殊场景，但可通过不断扩充新的场景加以解决。
该算法涉及到的主要类图如下：

该算法的主要执行流程如下图所示：

可借助GDB调试命令对上述执行流程进行更为深入的理解，例如TrafficLightProtectedScenario场景中TrafficLightProtectedStageApproach阶段的PathLaneBorrowDecider任务的调用堆栈如下，从下往上看，对于任意一个任务的调用流程一目了然：

#0  apollo::planning::PathLaneBorrowDecider::Process (this=0x7f8c28294460, frame=0x7f8c38029f70, 
    reference_line_info=0x7f8c3802b140) at modules/planning/tasks/deciders/path_lane_borrow_decider/path_lane_borrow_decider.cc:39
#1  0x00007f8c0468b7c8 in apollo::planning::Decider::Execute (this=0x7f8c28294460, frame=0x7f8c38029f70, 
    reference_line_info=0x7f8c3802b140) at modules/planning/tasks/deciders/decider.cc:31
#2  0x00007f8c065c4a01 in apollo::planning::scenario::Stage::ExecuteTaskOnReferenceLine (this=0x7f8c28293eb0, 
    planning_start_point=..., frame=0x7f8c38029f70) at modules/planning/scenarios/stage.cc:96
#3  0x00007f8c06e721da in apollo::planning::scenario::traffic_light::TrafficLightProtectedStageApproach::Process (
    this=0x7f8c28293eb0, planning_init_point=..., frame=0x7f8c38029f70) at 
    modules/planning/scenarios/traffic_light/protected/stage_approach.cc:48
#4  0x00007f8c067f1732 in apollo::planning::scenario::Scenario::Process (
    this=0x7f8c2801bf20, planning_init_point=..., frame=0x7f8c38029f70) 
    at modules/planning/scenarios/scenario.cc:76
#5  0x00007f8c186e153a in apollo::planning::PublicRoadPlanner::Plan (
    this=0x23093de0, planning_start_point=..., frame=0x7f8c38029f70, 
    ptr_computed_trajectory=0x7f8b9a5fbed0) at modules/planning/planner/public_road/public_road_planner.cc:51
#6  0x00007f8c19ee5937 in apollo::planning::OnLanePlanning::Plan (
    this=0x237f3b0, current_time_stamp=1557133995.3679764, stitching_trajectory=std::vector of length 1, 
    capacity 1 = {...}, ptr_trajectory_pb=0x7f8b9a5fbed0)  at modules/planning/on_lane_planning.cc:436
#7  0x00007f8c19ee40fa in apollo::planning::OnLanePlanning::RunOnce (
    this=0x237f3b0, local_view=..., ptr_trajectory_pb=0x7f8b9a5fbed0) at modules/planning/on_lane_planning.cc:304
#8  0x00007f8c1ab0d494 in apollo::planning::PlanningComponent::Proc (
    this=0x1d0f310, prediction_obstacles=std::shared_ptr (count 4, weak 0) 0x7f8b840164f8, 
    chassis=std::shared_ptr (count 4, weak 0) 0x7f8b84018a08, 
    localization_estimate=std::shared_ptr (count 4, weak 0) 0x7f8b8400d3b8) at modules/planning/planning_component.cc:134
#9  0x00007f8c1abb46c4 in apollo::cyber::Component<apollo::prediction::PredictionObstacles, 
    apollo::canbus::Chassis, apollo::localization::LocalizationEstimate, apollo::cyber::NullType>::Process (this=0x1d0f310, 
    msg0=std::shared_ptr (count 4, weak 0) 0x7f8b840164f8, msg1=std::shared_ptr (count 4, weak 0) 0x7f8b84018a08, 
    msg2=std::shared_ptr (count 4, weak 0) 0x7f8b8400d3b8) at ./cyber/component/component.h:291
#10 0x00007f8c1aba2698 in apollo::cyber::Component<apollo::prediction::PredictionObstacles, 
    apollo::canbus::Chassis, apollo::localization::LocalizationEstimate, apollo::cyber::NullType>::Initialize(
    apollo::cyber::proto::ComponentConfig const&)::{lambda(std::shared_ptr<apollo::prediction::PredictionObstacles> const&,     
    std::shared_ptr<apollo::canbus::Chassis> const&, std::shared_ptr<apollo::localization::LocalizationEstimate> const&)#2}::operator()
    (std::shared_ptr<apollo::prediction::PredictionObstacles> const&, std::shared_ptr<apollo::canbus::Chassis> const&, 
    std::shared_ptr<apollo::localization::LocalizationEstimate> const&) const (__closure=0x2059a430, 
    msg0=std::shared_ptr (count 4, weak 0) 0x7f8b840164f8, msg1=std::shared_ptr (count 4, weak 0) 0x7f8b84018a08,     
    msg2=std::shared_ptr (count 4, weak 0) 0x7f8b8400d3b8) at ./cyber/component/component.h:378
#11 0x00007f8c1abb4ad2 in apollo::cyber::croutine::RoutineFactory apollo::cyber::croutine::CreateRoutineFactory
    <apollo::prediction::PredictionObstacles, apollo::canbus::Chassis, apollo::localization::LocalizationEstimate, 
    apollo::cyber::Component<apollo::prediction::PredictionObstacles, apollo::canbus::Chassis, 
    apollo::localization::LocalizationEstimate, apollo::cyber::NullType>::Initialize(
    apollo::cyber::proto::ComponentConfig const&)::{lambda(std::shared_ptr<apollo::prediction::PredictionObstacles> const&, 
    std::shared_ptr<apollo::canbus::Chassis> const&, std::shared_ptr<apollo::localization::LocalizationEstimate> const&)#2}&>
    (apollo::cyber::Component<apollo::prediction::PredictionObstacles, apollo::canbus::Chassis, 
    apollo::localization::LocalizationEstimate, apollo::cyber::NullType>::Initialize(apollo::cyber::proto::ComponentConfig const&)::
    {lambda(std::shared_ptr<apollo::prediction::PredictionObstacles> const&, std::shared_ptr<apollo::canbus::Chassis> const&, 
    std::shared_ptr<apollo::localization::LocalizationEstimate> const&)#2}&, 
    std::shared_ptr<apollo::cyber::data::DataVisitor<apollo::prediction::PredictionObstacles, 
    apollo::canbus::Chassis, apollo::localization::LocalizationEstimate, apollo::cyber::NullType> > const&)::
    {lambda()#1}::operator()() const::{lambda()#1}::operator()() const (__closure=0x2059a420) at ./cyber/croutine/routine_factory.h:108
#12 0x00007f8c1ac0466a in std::_Function_handler<void (), apollo::cyber::croutine::RoutineFactory 
apollo::cyber::croutine::CreateRoutineFactory<apollo::prediction::PredictionObstacles, apollo::canbus::Chassis, apollo::localization::LocalizationEstimate, 
apollo::cyber::Component<apollo::prediction::PredictionObstacles, apollo::canbus::Chassis, apollo::localization::LocalizationEstimate, 
apollo::cyber::NullType>::Initialize(apollo::cyber::proto::ComponentConfig const&)::{lambda(std::shared_ptr<apollo::prediction::PredictionObstacles> const&, 
std::shared_ptr<apollo::canbus::Chassis> const&, std::shared_ptr<apollo::localization::LocalizationEstimate> const&)#2}&>
(apollo::cyber::Component<apollo::prediction::PredictionObstacles, apollo::canbus::Chassis, apollo::localization::LocalizationEstimate, 
apollo::cyber::NullType>::Initialize(apollo::cyber::proto::ComponentConfig const&)::{lambda(std::shared_ptr<apollo::prediction::PredictionObstacles> const&, 
std::shared_ptr<apollo::canbus::Chassis> const&, std::shared_ptr<apollo::localization::LocalizationEstimate> const&)#2}&, 
std::shared_ptr<apollo::cyber::data::DataVisitor<apollo::prediction::PredictionObstacles, apollo::canbus::Chassis, apollo::localization::LocalizationEstimate, 
apollo::cyber::NullType> > const&)::{lambda()#1}::operator()() const::{lambda()#1}>::_M_invoke(std::_Any_data const&) (__functor=...) at 
/usr/include/c++/4.8/functional:2071
#13 0x00007f8c5f5b86e8 in std::function<void ()>::operator()() const (this=0x205f1160) at /usr/include/c++/4.8/functional:2471
#14 0x00007f8c57560cbc in apollo::cyber::croutine::CRoutine::Run (this=0x205f1148) at ./cyber/croutine/croutine.h:143
#15 0x00007f8c5755ff55 in apollo::cyber::croutine::(anonymous namespace)::CRoutineEntry (arg=0x205f1148) at cyber/croutine/croutine.cc:43
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63

所有规划算法共用的流程略去不表，与PublicRoadPlanner规划算法相关的有两处，一处是PublicRoadPlanner::Init，另一处是PublicRoadPlanner::Plan。
在PublicRoadPlanner::Init函数内，首先读取配置文件/apollo/modules/planning/conf/planning_config.pb.txt，获取所有支持的场景supported_scenarios，然后调用scenario_manager_.Init(supported_scenarios);对这些场景进行初始化，具体而言就是先调用ScenarioManager::RegisterScenarios函数将配置文件中的所有场景添加到场景管理器对象scenario::ScenarioManager scenario_manager_中，再调用ScenarioManager::CreateScenario函数，生成当前路况对应的场景对象std::unique_ptr<Scenario> current_scenario_。
在PublicRoadPlanner::Plan函数内，首先调用函数ScenarioManager:Update根据实时路况更新当前场景对象std::unique_ptr<Scenario> current_scenario_，接着调用scenario_->Process(planning_start_point, frame)语句实施具体的场景算法。如果Scenario::Process函数的返回值是scenario::Scenario::STATUS_DONE，表明当前场景状态已完成，则再次调用函数ScenarioManager::Update更新当前场景，否则继续处理当前场景并返回。
下面来看场景更新函数ScenarioManager::Update的代码：

void ScenarioManager::Update(const common::TrajectoryPoint& ego_point,
                             const Frame& frame) {
  CHECK(!frame.reference_line_info().empty());
  Observe(frame);
  ScenarioDispatch(ego_point, frame);
}
1
2
3
4
5
6

该函数逻辑很简单，包含两个子函数：ScenarioManager::Observe和ScenarioManager::ScenarioDispatch，其中前者用于更新first_encountered_overlap_map_（车辆沿着参考线行驶首次遇到的道路连接的键值对，key表示道路连接类型，例如：PNC_JUNCTION（用于规划控制模块的交叉路口，是一个由多条道路停止线包围而成的多边形区域，感谢Apollo美研团队Yifei Jiang老师的答疑）、SIGNAL（红绿灯） 、STOP_SIGN（停止标志）、YIELD_SIGN（让行标志），value表示对应的地图元素数据），代码如下所示：

void ScenarioManager::Observe(const Frame& frame) {
  // init first_encountered_overlap_map_
  first_encountered_overlap_map_.clear();
  const auto& reference_line_info = frame.reference_line_info().front();
  const auto& first_encountered_overlaps =
      reference_line_info.FirstEncounteredOverlaps();
  for (const auto& overlap : first_encountered_overlaps) {
    if (overlap.first == ReferenceLineInfo::PNC_JUNCTION ||
        overlap.first == ReferenceLineInfo::SIGNAL ||
        overlap.first == ReferenceLineInfo::STOP_SIGN ||
        overlap.first == ReferenceLineInfo::YIELD_SIGN) {
      first_encountered_overlap_map_[overlap.first] = overlap.second;
    }
  }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

ScenarioManager::ScenarioDispatch使用Strategy设计模式来分派具体的场景，代码如下所示：

void ScenarioManager::ScenarioDispatch(const common::TrajectoryPoint& ego_point,
                                       const Frame& frame) {
  CHECK(!frame.reference_line_info().empty());

  
  // default: LANE_FOLLOW
  ScenarioConfig::ScenarioType scenario_type = default_scenario_type_;

  // check current_scenario (not switchable)
  switch (current_scenario_->scenario_type()) {
    case ScenarioConfig::LANE_FOLLOW:
    case ScenarioConfig::CHANGE_LANE:
    case ScenarioConfig::PULL_OVER:
      break;
    case ScenarioConfig::SIDE_PASS:
    case ScenarioConfig::BARE_INTERSECTION_UNPROTECTED:
    case ScenarioConfig::STOP_SIGN_PROTECTED:
    case ScenarioConfig::STOP_SIGN_UNPROTECTED:
    case ScenarioConfig::TRAFFIC_LIGHT_PROTECTED:
    case ScenarioConfig::TRAFFIC_LIGHT_UNPROTECTED_LEFT_TURN:
    case ScenarioConfig::TRAFFIC_LIGHT_UNPROTECTED_RIGHT_TURN:
    case ScenarioConfig::VALET_PARKING:
      // must continue until finish
      if (current_scenario_->GetStatus() !=
          Scenario::ScenarioStatus::STATUS_DONE) {
        scenario_type = current_scenario_->scenario_type();
      }
      break;
    default:
      break;
  }

  
  // intersection scenarios
  if (scenario_type == default_scenario_type_) {
    hdmap::PathOverlap* traffic_sign_overlap = nullptr;
    hdmap::PathOverlap* pnc_junction_overlap = nullptr;
    ReferenceLineInfo::OverlapType overlap_type;

    const auto& reference_line_info = frame.reference_line_info().front();
    const auto& first_encountered_overlaps =
        reference_line_info.FirstEncounteredOverlaps();
    // note: first_encountered_overlaps already sorted
    for (const auto& overlap : first_encountered_overlaps) {
      if (overlap.first == ReferenceLineInfo::SIGNAL ||
          overlap.first == ReferenceLineInfo::STOP_SIGN ||
          overlap.first == ReferenceLineInfo::YIELD_SIGN) {
        overlap_type = overlap.first;
        traffic_sign_overlap = const_cast<hdmap::PathOverlap*>(&overlap.second);
        break;
      } else if (overlap.first == ReferenceLineInfo::PNC_JUNCTION) {
        pnc_junction_overlap = const_cast<hdmap::PathOverlap*>(&overlap.second);
      }
    }

    if (traffic_sign_overlap) {
      switch (overlap_type) {
        case ReferenceLineInfo::STOP_SIGN:
          if (FLAGS_enable_scenario_stop_sign) {
            scenario_type =
                SelectStopSignScenario(frame, *traffic_sign_overlap);
          }
          break;
        case ReferenceLineInfo::SIGNAL:
          if (FLAGS_enable_scenario_traffic_light) {
            scenario_type =
                SelectTrafficLightScenario(frame, *traffic_sign_overlap);
          }
          break;
        case ReferenceLineInfo::YIELD_SIGN:
          // TODO(all): to be added
          // scenario_type = SelectYieldSignScenario(
          //     frame, *traffic_sign_overlap);
          break;
        default:
          break;
      }
    } else if (pnc_junction_overlap) {
      // bare intersection
      if (FLAGS_enable_scenario_bare_intersection) {
        scenario_type =
            SelectBareIntersectionScenario(frame, *pnc_junction_overlap);
      }
    }
  }

  
  // CHANGE_LANE scenario
  if (scenario_type == default_scenario_type_) {
    scenario_type = SelectChangeLaneScenario(frame);
  }

  
  // SIDE_PASS scenario
  if (scenario_type == default_scenario_type_) {
    scenario_type = SelectSidePassScenario(frame);
  }

  
  // pull-over scenario
  if (scenario_type == default_scenario_type_) {
    if (FLAGS_enable_scenario_pull_over) {
      scenario_type = SelectPullOverScenario(frame);
    }
  }

  
  // VALET_PARKING scenario
  if (scenario_type == default_scenario_type_) {
    scenario_type = SelectValetParkingScenario(frame);
  }

  // Check if it is supported by confs
  if (supported_scenarios_.find(scenario_type) == supported_scenarios_.end()) {
    scenario_type = default_scenario_type_;
  }

  ADEBUG << "select scenario: "
         << ScenarioConfig::ScenarioType_Name(scenario_type);

  // update PlanningContext
  UpdatePlanningContext(frame, scenario_type);

  if (current_scenario_->scenario_type() != scenario_type) {
    current_scenario_ = CreateScenario(scenario_type);
  }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127