算法的基本架构
我们在最开始直接给出规划决策算法架构框图，然后一一介绍每个框图结构的细节：
- 模块的入口是 PlanningComponent，在 Cyber 中注册模块，订阅和发布消息，并且注册对应的 Planning 类。
- Planning 的过程之前是定时器触发，即每隔一段固定的时间执行一次，现已经改为事件触发，即只要收集完成对应 TOPIC 的消息，就会触发执行，这样的好处是提高的实时性。
- Planning 类主要实现了 2 个功能，一个是启动 ReferenceLineProvider 来提供参考线，后面生成的轨迹都是在参考线的基础上做优化，ReferenceLineProvider 启动了一个单独的线程，每隔 50ms 执行一次，和 Planning 主流程并行执行。Planning 类另外的一个功能是执行 Planning 主流程。
- Planning 主流程先是选择对应的 Planner，我们这里主要分析 PublicRoadPlanner，在配置文件中定义了 Planner 支持的场景(Scenario)，把规划分为具体的几个场景来执行，每个场景又分为几个阶段(Stage)，每个阶段会执行多个任务(Task)，任务执行完成后，对应的场景就完成了。不同场景间的切换是由一个状态机(ScenarioDispatch)来控制的。规划控制器根据 ReferenceLineProvider 提供的参考线，在不同的场景下做切换，生成一条车辆可以行驶的轨迹，并且不断重复上述过程直到到达目的地。
PlanningComponent
规划模块的入口是 PlanningComponent。PlanningComponent 的两大核心函数是 Init 和 Proc.
在初始化中，我们可以看到如下的代码，FLAGS_use_navigation_mode 的标志位直接决定了我们等等实例化的 planning_base_ 的具体 planner，到底是 NaviPlanning 还是 OnLanePlanning：
if (FLAGS_use_navigation_mode) {
planning_base_ = std::make_unique<NaviPlanning>(injector_);
} else {
planning_base_ = std::make_unique<OnLanePlanning>(injector_);
}
其实在更早的版本中有更多选项，7.0 为什么简化了呢？
if (FLAGS_open_space_planner_switchable) {
planning_base_ = std::make_unique<OpenSpacePlanning>();
} else {
if (FLAGS_use_navigation_mode) {
planning_base_ = std::make_unique<NaviPlanning>();
} else {
planning_base_ = std::make_unique<OnLanePlanning>();
}
}
而在 Proc(…)函数中，最核心部分则是调用对应 planner 的 RunOnce 函数：
planning_base_->RunOnce(local_view_, &adc_trajectory_pb);
接下来的具体例子中我们都以这个 planing_base_被实例化为 OnLanePlanning 作为前提。
下图是几个规划器的结构。这里我们要讨论的是 PublicRoadPlanner，这里只是简单的给出结构，后面的对应小节会具体介绍函数中如何运行和管理。
OnLanePlanning
RunOnce 函数的三大重要函数是：
// 初始化frame
status = InitFrame(frame_num, stitching_trajectory.back(), vehicle_state);
......
//进行traffic decider
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";
}
}
......
//plan主函数
status = Plan(start_timestamp, stitching_trajectory, ptr_trajectory_pb);
Plan
这个 plan 函数中主要核心则是：
auto status = planner_->Plan(stitching_trajectory.back(), frame_.get(),
ptr_trajectory_pb);
而这个 planner_则是通过 DispatchPlanner 来指定：
// dispatch planner
planner_ = planner_dispatcher_->DispatchPlanner(config_, injector_);
具体来说就是：
std::unique_ptr<Planner> OnLanePlannerDispatcher::DispatchPlanner(
const PlanningConfig& planning_config,
const std::shared_ptr<DependencyInjector>& injector) {
return planner_factory_.CreateObject(
planning_config.standard_planning_config().planner_type(0), injector);
}
//其中这个config是：
standard_planning_config {
planner_type: PUBLIC_ROAD
planner_public_road_config {
}
}
Plan 函数中主要的几个步骤是：
Status PublicRoadPlanner::Plan(const TrajectoryPoint& planning_start_point,
Frame* frame,
ADCTrajectory* ptr_computed_trajectory) {
//更新当前的scenario
scenario_manager_.Update(planning_start_point, *frame);
// 获取当前场景
scenario_ = scenario_manager_.mutable_scenario();
// 执行当前场景的任务
auto result = scenario_->Process(planning_start_point, frame);
。。。。。。
}
Scenario 是 apollo 决策规划算法中的重要概念，apollo 可以应对自动驾驶所面临的不同道路场景，都是通过 Scenario 统一注册与管理；Scenario 通过一个有限状态机来选择不同场景。
更新当前的 scenario
scenario_manager_.Update(planning_start_point, *frame);
我们来看 Update，做了这么几件事：
void ScenarioManager::Update(const common::TrajectoryPoint& ego_point,
const Frame& frame) {
ACHECK(!frame.reference_line_info().empty());
// 保留当前帧
Observe(frame);
// 场景分发
ScenarioDispatch(frame);
}
这里能够提供的场景库包括：
enum ScenarioType {
LANE_FOLLOW = 0; // default scenario
// intersection involved
BARE_INTERSECTION_UNPROTECTED = 2;
STOP_SIGN_PROTECTED = 3;
STOP_SIGN_UNPROTECTED = 4;
TRAFFIC_LIGHT_PROTECTED = 5;
TRAFFIC_LIGHT_UNPROTECTED_LEFT_TURN = 6;
TRAFFIC_LIGHT_UNPROTECTED_RIGHT_TURN = 7;
YIELD_SIGN = 8;
// parking
PULL_OVER = 9;
VALET_PARKING = 10;
EMERGENCY_PULL_OVER = 11;
EMERGENCY_STOP = 12;
// misc
NARROW_STREET_U_TURN = 13;
PARK_AND_GO = 14;
// learning model sample
LEARNING_MODEL_SAMPLE = 15;
// turn around
DEADEND_TURNAROUND = 16;
}
获取当前场景
//获取当前场景
Scenario* mutable_scenario() { return current_scenario_.get(); }
执行当前场景的任务
//通过Process函数进行场景运行
Scenario::ScenarioStatus Scenario::Process(
const common::TrajectoryPoint& planning_init_point, Frame* frame)
场景下有 stage, 运行 stage 的 Process 函数：
auto ret = current_stage_->Process(planning_init_point, frame);
我们拿 LaneFollowStage 为例，则运行他的每个 task:
//首先进入函数：
auto cur_status =
PlanOnReferenceLine(planning_start_point, frame, &reference_line_info);
//然后是在PlanOnReferenceLine函数中遍历每个task
for (auto* task : task_list_) {
const double start_timestamp = Clock::NowInSeconds();
ret = task->Execute(frame, reference_line_info);
。。。。。。
}
以 lane follow 这个 stage 为例，这个 task list 包括：
scenario_type: LANE_FOLLOW
stage_type: LANE_FOLLOW_DEFAULT_STAGE
stage_config: {
stage_type: LANE_FOLLOW_DEFAULT_STAGE
enabled: true
task_type: LANE_CHANGE_DECIDER
task_type: PATH_REUSE_DECIDER
task_type: PATH_LANE_BORROW_DECIDER
task_type: PATH_BOUNDS_DECIDER
task_type: PIECEWISE_JERK_PATH_OPTIMIZER
task_type: PATH_ASSESSMENT_DECIDER
task_type: PATH_DECIDER
task_type: RULE_BASED_STOP_DECIDER
task_type: SPEED_BOUNDS_PRIORI_DECIDER
task_type: SPEED_HEURISTIC_OPTIMIZER
task_type: SPEED_DECIDER
task_type: SPEED_BOUNDS_FINAL_DECIDER
task_type: PIECEWISE_JERK_SPEED_OPTIMIZER
# task_type: PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER
task_type: RSS_DECIDER
