Apollo 3.5 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
-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;
}
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;
}
上述函数处理一个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，并将其加入到工厂集合类std::map中，这个就是 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)
而后者的定义为：
#define CLASS_LOADER_REGISTER_CLASS(Derived, Base) \
CLASS_LOADER_REGISTER_CLASS_INTERNAL_1(Derived, Base, __COUNTER__)
继续展开得到：
#define CLASS_LOADER_REGISTER_CLASS_INTERNAL_1(Derived, Base, UniqueID) \
CLASS_LOADER_REGISTER_CLASS_INTERNAL(Derived, Base, UniqueID)
仍然需要进一步展开：
#define CLASS_LOADER_REGISTER_CLASS_INTERNAL(Derived, Base, UniqueID) \
namespace { \
struct ProxyType##UniqueID { \
ProxyType##UniqueID() { \
apollo::cyber::class_loader::utility::RegisterClass( \
#Derived, #Base); \
} \
}; \
static ProxyType##UniqueID g_register_class_##UniqueID; \
}
将PlanningComponent代入上述宏，最终得到：
namespace {
struct ProxyType__COUNTER__ {
ProxyType__COUNTER__() {
apollo::cyber::class_loader::utility::RegisterClass<PlanningComponent, apollo::cyber::ComponentBase>(
"PlanningComponent", "apollo::cyber::ComponentBase");
}
};
static ProxyType__COUNTER__ g_register_class___COUNTER__;
}
注意两点：第一，上述定义位于namespace apollo::planning内；第二，___COUNTER__是C语言的一个计数器宏，这里仅代表一个占位符，实际展开时可能就是1387之类的数字，亦即ProxyType__COUNTER__实际上应为ProxyType1387之类的命名。上述代码简洁明了，首先定义一个结构体ProxyType1387，该结构体仅包含一个构造函数，在内部调用apollo::cyber::class_loader::utility::RegisterClass注册apollo::cyber::ComponentBase类的派生类PlanningComponent。并定义一个静态全局结构体ProxyType1387变量：g_register_class1387，这样就能调用构造函数完成注册。
继续观察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();
utility::AbstractClassFactory<Base>* new_class_factrory_obj =
new utility::ClassFactory<Derived, Base>(class_name, base_class_name);
new_class_factrory_obj->AddOwnedClassLoader(GetCurActiveClassLoader());
new_class_factrory_obj->SetRelativeLibraryPath(GetCurLoadingLibraryName());
GetClassFactoryMapMapMutex().lock();
ClassClassFactoryMap& factory_map =
GetClassFactoryMapByBaseClass(typeid(Base).name());
factory_map[class_name] = new_class_factrory_obj;
GetClassFactoryMapMapMutex().unlock();
}
该函数创建一个模板类utility::ClassFactory对象new_class_factrory_obj，为其添加类加载器，设置加载库的路径，最后将工厂类对象加入到ClassClassFactoryMap对象factory_map统一管理。通过该函数，我们可以清楚地看到，Cyber使用工厂方法模式完成产品类（例如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);
}
上述函数从磁盘配置文件读取配置信息，并调用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;
}
已经知道，PlanningComponent对象是通过class_loader_manager_.CreateClassObj创建出来的，而class_loader_manager_是一个class_loader::ClassLoaderManager类对象。现在的问题是：class_loader::ClassLoaderManager与2.1节中的工厂类utility::AbstractClassFactory如何联系起来的？
先看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>();
}
上述函数中，从所有class_loaders中找出一个正确的class_loader，并调用class_loader->CreateClassObj（位于文件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;
}
上述函数继续调用utility::CreateClassObj（位于文件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;
}
上述函数使用factory = dynamic_cast获取对应的工厂对象指针，至此终于将class_loader::ClassLoaderManager与2.1节中的工厂类utility::AbstractClassFactory联系起来了。工厂类对象指针找到后，使用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::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::Initialize(
apollo::cyber::proto::ComponentConfig const&)::{lambda(std::shared_ptr const&,
std::shared_ptr const&, std::shared_ptr const&)#2}::operator()
(std::shared_ptr const&, std::shared_ptr const&,
std::shared_ptr 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
::Initialize(
apollo::cyber::proto::ComponentConfig const&)::{lambda(std::shared_ptr const&,
std::shared_ptr const&, std::shared_ptr const&)#2}&>
(apollo::cyber::Component::Initialize(apollo::cyber::proto::ComponentConfig const&)::
{lambda(std::shared_ptr const&, std::shared_ptr const&,
std::shared_ptr const&)#2}&,
std::shared_ptr > const&)::
{lambda()#1}::operator()() const::{lambda()#1}::operator()() const (__closure=0x2059a420) at ./cyber/croutine/routine_factory.h:108
#12 0x00007f8c1ac0466a in std::_Function_handler::Initialize(apollo::cyber::proto::ComponentConfig const&)::{lambda(std::shared_ptr const&,
std::shared_ptr const&, std::shared_ptr const&)#2}&>
(apollo::cyber::Component::Initialize(apollo::cyber::proto::ComponentConfig const&)::{lambda(std::shared_ptr const&,
std::shared_ptr const&, std::shared_ptr const&)#2}&,
std::shared_ptr > 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::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
所有规划算法共用的流程略去不表，与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。
在PublicRoadPlanner::Plan函数内，首先调用函数ScenarioManager:Update根据实时路况更新当前场景对象std::unique_ptr，接着调用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);
}
该函数逻辑很简单，包含两个子函数：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;
}
}
}
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);
}
}