在这里插入图片描述

前言

本文记录学习planning模块时的一些笔记，总体流程参照https://zhuanlan.zhihu.com/p/61982682中的流程图，如上图所示。

planning_component

modules/planning/planning_component.cc
在这里插入图片描述

PlanningComponent::Init部分首先完成规划模式的选择：

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

再完成相应的消息订阅

routing_reader_ = node_->CreateReader<RoutingResponse>(
traffic_light_reader_ = node_->CreateReader<TrafficLightDetection>(
pad_msg_reader_ = node_->CreateReader<PadMessage>(
story_telling_reader_ = node_->CreateReader<Stories>(
relative_map_reader_ = node_->CreateReader<MapMsg>(

最后消息发布

  planning_writer_ = node_->CreateWriter<ADCTrajectory>(rerouting_writer_ = node_->CreateWriter<RoutingRequest>(planning_learning_data_writer_ = node_->CreateWriter<PlanningLearningData>(

PlanningComponent::Proc的主要是检查数据，并且执行注册好的Planning，生成路线并且发布。

bool PlanningComponent::Proc(...) {// 1. 检查是否需要重新规划线路。CheckRerouting();// 2. 数据放入local_view_中，并且检查输入数据。...// 3. 执行注册好的Planning，生成线路。planning_base_->RunOnce(local_view_, &adc_trajectory_pb);// 4. 发布消息planning_writer_->Write(std::make_shared<ADCTrajectory>(adc_trajectory_pb));// 5. 最后记录history->Add(adc_trajectory_pb);
}

RunOnce在NaviPlanning和OnLanePlanning都有，本文以常用的OnLanePlanning为例。

PS1

prediction_obstacles 从channel中获得的指针，类型为 PredictionObstacles，代码文件在 modules/prediction/proto/prediction_obstacle.proto 中，该类型的消息来自预测模块，包含了以下信息：
- Header 头结构：包含了这条消息发布的时刻（以秒为单位），目前位置的模块名，该消息的序列号（每个模块各自维护的）等，几乎每个消息都包含有这个头结构，下面就不提及了。
- PredictionObstacle 预测模块中的若干个障碍物行为：有感知模块中的障碍物（PerceptionObstacle），它包括障碍物的 id，它的三维坐标、速度加速度、边界大小（长高宽）、特殊的形状、类型（行人、非机动车、机动车）、运动轨迹等；还有时间戳，记录 GPS 给出的时刻；还有预测的时间长度；多种可能的运动轨迹；障碍物的运动趋势、优先级等。
- 开始的时间戳：记录预测开始的时刻
- 结束时间戳：预测结束的时刻
- 自动驾驶车辆的运动趋势，停止、正常行驶还是正在变道等
- Scenery 现在的场景
chassis 从channel获得的指针，类型为 Chassis，这条消息直接来自总线 CanBus，代码文件在 modules/canbus/proto/chassis.proto 中，该类包含了很多信息，主要是汽车底盘所给出的机械状态信息，比如：
- 驾驶模式，有手动驾驶、自动驾驶、仅控制方向、仅控制速度以及紧急模式
- 档位情况、引擎转速、车辆速度、里程表、燃油情况、电池电量等
- 刹车、油门踏板的力度，方向盘的旋转角度，车轮转速
- 转向灯、雾灯、大灯的工作情况
localization_estimate 从channel中获得的指针，类型为 LocalizationEstimate，代码文件在 modules/localization/proto/localization.proto 中，这条消息来自定位模块，该类主要包含了：
- Pose 位置姿势，包括车头朝向，在地图上的线速度、线加速度和相对位置，角速度、仰角度等
- 测量上述姿势的时刻
- 车辆已经经过的轨迹点列
- MSF 定位状态与质量

PS2

std::mutex是C++11标准中提供的一种互斥锁机制，用于保护共享资源的访问。当多个线程需要同时访问共享资源时，为了避免出现数据竞争等问题，需要使用互斥锁进行同步控制。

std::mutex是一种基本的互斥锁类型，可以通过lock()函数锁定互斥锁，通过unlock()函数释放互斥锁。当一个线程持有互斥锁时，其他线程想要获取该互斥锁将会被阻塞，直到持有该互斥锁的线程将锁释放。

例如，当多个线程需要同时访问同一个共享变量时，可以在访问之前使用std::mutex进行锁定，保证每个线程都可以安全地访问该变量。例如：

#include <mutex>
#include <thread>
#include <iostream>int shared_data = 0;
std::mutex mutex;void work(int id) {for (int i = 0; i < 10000; ++i) {mutex.lock();shared_data++;mutex.unlock();}std::cout << "Thread " << id << " finished." << std::endl;
}int main() {std::thread t1(work, 1);std::thread t2(work, 2);t1.join();t2.join();std::cout << "Shared data: " << shared_data << std::endl;return 0;
}

在上述例子中，两个线程都会对shared_data变量进行10000次的自增操作，为了避免出现数据竞争，我们在对shared_data进行访问之前都使用了std::mutex进行了锁定，保证了线程的安全性。

on_lane_planning

modules/planning/on_lane_planning.cc
在这里插入图片描述

OnLanePlanning::Init主要实现分配具体的Planner，启动参考线提供器(reference_line_provider_)。启动参考线提供器，会另启动一个线程，执行一个定时任务，每隔50ms提供一次参考线。注意：分配Planner的部分在OnLanePlanning实例化时就已经完成。

分配Planner的实现部分

modules/planning/planner/on_lane_planner_dispatcher.cc
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);
}

可以在modules/planning/conf/planning_config.pb.txt配置文件中看到配置Planner的类型：

standard_planning_config {planner_type: PUBLIC_ROADplanner_public_road_config {}
}

OnLanePlanning 的主逻辑在OnLanePlanning::RunOnce中，内容比较多，有些还没仔细看，大致完成了一下的内容：

void OnLanePlanning::RunOnce(const LocalView& local_view,ADCTrajectory* const ptr_trajectory_pb) {//  更新汽车状态和参考线的状态，如果状态无效，直接返回//  ...  // 是否为出现导航路线变换，如果是 初始化 planner//  加上预估的规划触发的周期 得到 stitchingTrajectory// planning is triggered by prediction data, but we can still use an estimated// cycle time for stitchingstatus = InitFrame(frame_num, stitching_trajectory.back(), vehicle_state);//  判断是否符合交通规则//  开始正在的规划 planner 开始规划status = Plan(start_timestamp, stitching_trajectory, ptr_trajectory_pb);//  记录规划所花费的时间//  ...
}

status = Plan(start_timestamp, stitching_trajectory, ptr_trajectory_pb);中有如下代码，此部分的Plan是指分配的planner的Plan()函数

  auto status = planner_->Plan(stitching_trajectory.back(), frame_.get(),ptr_trajectory_pb);

PS1

make_unique是一个C++11中的函数模板，用于创建并返回一个独占指针（unique_ptr）。它的作用是将一个裸指针封装成一个unique_ptr对象，并确保该对象是唯一持有它所指向的对象。这可以有效避免内存泄漏和多个指针指向同一个对象的问题。

public_road_planner

modules/planning/planner/public_road/public_road_planner.cc
接着来看public_road_planner的实现。

在这里插入图片描述

PublicRoadPlanner::Init实例化一个全局的scenario_manager_对象来进行场景管理。

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

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();// 调用Scenario的Process函数，对具体的场景进行处理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_DONEscenario_manager_.Update(planning_start_point, *frame);} else if (result == scenario::Scenario::STATUS_UNKNOWN) {// 当前场景失败return Status(common::PLANNING_ERROR, "scenario returned unknown");}return Status::OK();
}

Scenario

Introduction

在这里插入图片描述

Apollo规划模块的框架以场景调度为基本决策框架，场景调度本质上是一个双层状态机，顶层是基于场景的管理器，底层是一系列小模块的任务组合，由此来构建出Apollo planning的整体框架。

在这里插入图片描述在执行的每一个场景中，又分为一个或多个阶段，称为stage；而每一个stage又包含一个或多个任务，称为task。执行一个场景，最终就是顺序执行不同阶段的不同任务。由此可见，Scenario在Apollo规划决策中占据着重要的地位。

Scenario 类型

在apollo中，Scenario被定义了十余种类型，在官方文档Planing 2.0综述中有对各个场景的直观描述。例如下图是LANE_FOLLOW场景的示意图。

在这里插入图片描述

在apollo8.0中，可以从modules/planning/proto/planning_config.proto文件中看到关于Scenario的相关类型定义:

message ScenarioConfig {message StageConfig {// 阶段配置// 阶段类型optional StageType stage_type = 1;// 是否启用该阶段配置，默认值为trueoptional bool enabled = 2 [default = true];// 运行时使用的任务列表，按照顺序排列// 决定了这些任务的运行时顺序// 任务类型repeated TaskConfig.TaskType task_type = 3;// 未排序的任务配置列表// 任务配置repeated TaskConfig task_config = 4;}// 场景类型optional ScenarioType scenario_type = 1;// 场景配置，使用oneof是为了确保只有一个字段被设置oneof scenario_config {// 车道跟随场景配置ScenarioLaneFollowConfig lane_follow_config = 2;// 无保护交叉口场景配置ScenarioBareIntersectionUnprotectedConfig bare_intersection_unprotected_config = 3;// 紧急靠边停车场景配置ScenarioEmergencyPullOverConfig emergency_pull_over_config = 4;// 紧急停车场景配置ScenarioEmergencyStopConfig emergency_stop_config = 5;// 学习模型样本场景配置ScenarioLearningModelSampleConfig learning_model_sample_config = 6;// 窄街道U型转弯场景配置ScenarioNarrowStreetUTurnConfig narrow_street_u_turn_config = 7;// 停车即走场景配置ScenarioParkAndGoConfig park_and_go_config = 8;// 靠边停车场景配置ScenarioPullOverConfig pull_over_config = 9;// 无保护停车标志左转场景配置ScenarioStopSignUnprotectedConfig stop_sign_unprotected_config = 10;// 交通信号灯保护左转场景配置ScenarioTrafficLightProtectedConfig traffic_light_protected_config = 11;// 无保护交通信号灯左转场景配置ScenarioTrafficLightUnprotectedLeftTurnConfig traffic_light_unprotected_left_turn_config = 12;// 无保护交通信号灯右转场景配置ScenarioTrafficLightUnprotectedRightTurnConfig traffic_light_unprotected_right_turn_config = 13;// 代客停车场景配置ScenarioValetParkingConfig valet_parking_config = 14;// 让路标志场景配置ScenarioYieldSignConfig yield_sign_config = 15;// 死胡同场景配置（掉头）ScenarioDeadEndTurnAroundConfig deadend_turnaround_config = 18;}// 运行时使用的阶段列表，第一个是默认阶段// 阶段类型repeated StageType stage_type = 16;// 运行时使用的阶段配置列表，未排序// 阶段配置repeated StageConfig stage_config = 17;
}

接着来看看Scenario Manager做了些什么。
modules/planning/scenarios/scenario_manager.cc
在这里插入图片描述

场景注册

// modules/planning/scenarios/scenario_manager.cc
bool ScenarioManager::Init(const PlanningConfig& planning_config) {planning_config_.CopyFrom(planning_config);// 注册场景RegisterScenarios();// 默认为LANE_FOLLOWdefault_scenario_type_ = ScenarioType::LANE_FOLLOW;current_scenario_ = CreateScenario(default_scenario_type_);return true;
}

ScenarioManager负责实际的场景注册，Init函数负责对场景的初始化，首先调用::RegisterScenario这个函数对各个场景配置文件进行读取和注册；在开始创建LANE_FOLLOW作为默认运行场景。

// modules/planning/planner/public_road/public_road_planner.cc
Status PublicRoadPlanner::Init(const PlanningConfig& config) {config_ = config;scenario_manager_.Init(config);return Status::OK();
}

在PublicRoadPlanner初始化时会调用配置文件里的参数来建立这个对象，ScenarioManager会实例化一个全局的scenario_manager_对象来进行场景管理。

场景转换

// 更新场景
void ScenarioManager::Update(const common::TrajectoryPoint& ego_point,const Frame& frame) {ACHECK(!frame.reference_line_info().empty());// 获取当前要处理的overlapsObserve(frame);// 场景分发ScenarioDispatch(frame);
}

ScenarioManager除了负责场景的配置与注册外也负责对场景进行转换。场景转换在ScenarioManager::Update中实现，这个函数有两个输入，车辆当前的状态信息ego_point，和planning循环计算所需要的其他相关信息frame。ScenarioManager::Update首先调用Observe函数，用以获取当前要处理的overlaps.

// 获取当前要处理的overlaps
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;}}
}

在apollo里 overlap 是指地图上任意重合的东西，比如PNC_JUNCTION里是道路之间有相互重合，SIGNAL是信号灯与道路有重合，STOP_SIGN是停止标志与道路有重合，YIELD_SIGN是让行标志与道路有重合。

// 通过一个有限状态机，决定当前的场景
// 会根据配置选择基于规则还是基于学习的决策方法。
void ScenarioManager::ScenarioDispatch(const Frame& frame) {ACHECK(!frame.reference_line_info().empty());ScenarioType scenario_type;int history_points_len = 0;if (injector_->learning_based_data() &&injector_->learning_based_data()->GetLatestLearningDataFrame()) {history_points_len = injector_->learning_based_data()->GetLatestLearningDataFrame()->adc_trajectory_point_size();}if ((planning_config_.learning_mode() == PlanningConfig::E2E ||planning_config_.learning_mode() == PlanningConfig::E2E_TEST) &&history_points_len >= FLAGS_min_past_history_points_len) {scenario_type = ScenarioDispatchLearning();} else {scenario_type = ScenarioDispatchNonLearning(frame);}ADEBUG << "select scenario: " << ScenarioType_Name(scenario_type);// update PlanningContextUpdatePlanningContext(frame, scenario_type);if (current_scenario_->scenario_type() != scenario_type) {current_scenario_ = CreateScenario(scenario_type);}
}

当遇到这些overlaps时，在函数ScenarioDispatch中对其进行具体的处理或者场景切换。ScenarioDispatch会根据配置选择基于规则还是基于学习的决策方法。

接着来看ScenarioDispatchNonLearning函数：

ScenarioType ScenarioManager::ScenarioDispatchNonLearning(const Frame& frame) {// default: LANE_FOLLOWScenarioType scenario_type = default_scenario_type_;// Pad Msg scenario(驾驶员意图？)scenario_type = SelectPadMsgScenario(frame);if (scenario_type == default_scenario_type_) {// check current_scenario (not switchable)switch (current_scenario_->scenario_type()) {case ScenarioType::LANE_FOLLOW:case ScenarioType::PULL_OVER:break;case ScenarioType::BARE_INTERSECTION_UNPROTECTED:case ScenarioType::EMERGENCY_PULL_OVER:case ScenarioType::PARK_AND_GO:case ScenarioType::STOP_SIGN_PROTECTED:case ScenarioType::STOP_SIGN_UNPROTECTED:case ScenarioType::TRAFFIC_LIGHT_PROTECTED:case ScenarioType::TRAFFIC_LIGHT_UNPROTECTED_LEFT_TURN:case ScenarioType::TRAFFIC_LIGHT_UNPROTECTED_RIGHT_TURN:case ScenarioType::VALET_PARKING:case ScenarioType::YIELD_SIGN:// must continue until finishif (current_scenario_->GetStatus() !=Scenario::ScenarioStatus::STATUS_DONE) {scenario_type = current_scenario_->scenario_type();}break;default:break;}}// ParkAndGo / starting scenarioif (scenario_type == default_scenario_type_) {if (FLAGS_enable_scenario_park_and_go) {scenario_type = SelectParkAndGoScenario(frame);}}// intersection scenariosif (scenario_type == default_scenario_type_) {scenario_type = SelectInterceptionScenario(frame);}// pull-over scenarioif (scenario_type == default_scenario_type_) {if (FLAGS_enable_scenario_pull_over) {scenario_type = SelectPullOverScenario(frame);}}// VALET_PARKING scenarioif (scenario_type == default_scenario_type_) {scenario_type = SelectValetParkingScenario(frame);}return scenario_type;
}

首先，确定一个默认的场景类型，即LANE_FOLLOW。然后，判断是否有驾驶员意图，如果有，则将场景类型改为相应的意图场景类型。如果没有，则进入下一步逻辑。接着检查当前的场景类型，如果是LANE_FOLLOW或PULL_OVER，直接break；若是其他场景，且其他场景还未执行完毕，则继续执行当前场景。如果当前场景已完成，则进入下一步逻辑。PARK_AND_GO和PULL_OVER若有相关配置，则执行相应的函数。注意：每次切换场景都从默认场景开始，最后回到默认场景。

Scenario::Process

由PublicRoadPlanner::Plan的流程可知，在获取完当前场景之后，会调用Scenario的Process函数，对具体的场景进行处理。接下来便来看看Scenario的Process函数：
modules/planning/scenarios/scenario.cc

Scenario::ScenarioStatus Scenario::Process(const common::TrajectoryPoint& planning_init_point, Frame* frame) {// stage类型unknowif (current_stage_ == nullptr) {AWARN << "Current stage is a null pointer.";return STATUS_UNKNOWN;}// stage全部执行完成if (current_stage_->stage_type() == StageType::NO_STAGE) {scenario_status_ = STATUS_DONE;return scenario_status_;}// 当前处于某一stage，调用这个stage的Process()函数，处理具体规划逻辑auto ret = current_stage_->Process(planning_init_point, frame);switch (ret) {case Stage::ERROR: {AERROR << "Stage '" << current_stage_->Name() << "' returns error";scenario_status_ = STATUS_UNKNOWN;break;}case Stage::RUNNING: {scenario_status_ = STATUS_PROCESSING;break;}// 读取当前stage完成的状态，并对下一个stage进行处理case Stage::FINISHED: {auto next_stage = current_stage_->NextStage();if (next_stage != current_stage_->stage_type()) {AINFO << "switch stage from " << current_stage_->Name() << " to "<< StageType_Name(next_stage);if (next_stage == StageType::NO_STAGE) {scenario_status_ = STATUS_DONE;return scenario_status_;}if (stage_config_map_.find(next_stage) == stage_config_map_.end()) {AERROR << "Failed to find config for stage: " << next_stage;scenario_status_ = STATUS_UNKNOWN;return scenario_status_;}current_stage_ = CreateStage(*stage_config_map_[next_stage], injector_);if (current_stage_ == nullptr) {AWARN << "Current stage is a null pointer.";return STATUS_UNKNOWN;}}if (current_stage_ != nullptr &&current_stage_->stage_type() != StageType::NO_STAGE) {scenario_status_ = STATUS_PROCESSING;} else {scenario_status_ = STATUS_DONE;}break;}default: {AWARN << "Unexpected Stage return value: " << ret;scenario_status_ = STATUS_UNKNOWN;}}return scenario_status_;
}

Scenario都是顺序执行，只需要判断这一阶段是否结束，然后转到下一个阶段就可以了。CreateStage生成相应的stage，之后再进行stage的process。

Stage

以Lanefollowstage为例进行介绍。
modules/planning/scenarios/lane_follow/lane_follow_stage.cc

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 refactorif (count++ == frame->mutable_reference_line_info()->size()) {break;}ADEBUG << "No: [" << count << "] Reference Line.";ADEBUG << "IsChangeLanePath: " << reference_line_info.IsChangeLanePath();// 找到可用来行驶的参考线，退出循环if (has_drivable_reference_line) {reference_line_info.SetDrivable(false);break;}// 执行LaneFollow的规划auto cur_status =PlanOnReferenceLine(planning_start_point, frame, &reference_line_info);// 判断规划结果是否OKif (cur_status.ok()) {// 如果发生lanechange，判断reference_line的costif (reference_line_info.IsChangeLanePath()) {ADEBUG << "reference line is lane change ref.";ADEBUG << "FLAGS_enable_smarter_lane_change: "<< FLAGS_enable_smarter_lane_change;if (reference_line_info.Cost() < kStraightForwardLineCost &&(LaneChangeDecider::IsClearToChangeLane(&reference_line_info) ||FLAGS_enable_smarter_lane_change)) {// If the path and speed optimization succeed on target lane while// under smart lane-change or IsClearToChangeLane under older versionhas_drivable_reference_line = true;reference_line_info.SetDrivable(true);LaneChangeDecider::UpdatePreparationDistance(true, frame, &reference_line_info, injector_->planning_context());ADEBUG << "\tclear for lane change";} else {LaneChangeDecider::UpdatePreparationDistance(false, frame, &reference_line_info,injector_->planning_context());reference_line_info.SetDrivable(false);ADEBUG << "\tlane change failed";}} else {// 如果没有lanechange，stage执行结果为OK，则has_drivable_reference_line置位trueADEBUG << "reference line is NOT lane change ref.";has_drivable_reference_line = true;}} else {reference_line_info.SetDrivable(false);}}return has_drivable_reference_line ? StageStatus::RUNNING: StageStatus::ERROR;
}

Process主要是查找可供规划的参考线，之后在PlanOnReferenceLine函数中进行具体的执行。同时还对变道的状况进行规划，如果规划成功后，还需要判断目标车道的变道cost，如果cost太高，那么就会舍弃掉这条目标车道的reference_line, 此时放弃变道的规划，继续循环使用原车道的reference_line进行规划。

PlanOnReferenceLine函数

Status LaneFollowStage::PlanOnReferenceLine(const TrajectoryPoint& planning_start_point, Frame* frame,ReferenceLineInfo* reference_line_info) {// 判断是否有lanechange意图，如果否增加当前参考线的cost？有点疑问，增加了变道的可能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();// 顺序执行stage中的任务auto ret = Status::OK();for (auto* task : task_list_) {const double start_timestamp = Clock::NowInSeconds();// 执行每个task的具体逻辑ret = task->Execute(frame, reference_line_info);const double end_timestamp = Clock::NowInSeconds();const double time_diff_ms = (end_timestamp - start_timestamp) * 1000;ADEBUG << "after task[" << task->Name()<< "]:" << reference_line_info->PathSpeedDebugString();ADEBUG << task->Name() << " time spend: " << time_diff_ms << " ms.";RecordDebugInfo(reference_line_info, task->Name(), time_diff_ms);// 如果task执行失败，退出task执行序列，并且记录失败信息if (!ret.ok()) {AERROR << "Failed to run tasks[" << task->Name()<< "], Error message: " << ret.error_message();break;}// TODO(SHU): disable reference line order changes for now// updated reference_line_info, because it is changed in// lane_change_decider by PrioritizeChangeLane().// reference_line_info = &frame->mutable_reference_line_info()->front();// ADEBUG << "Current reference_line_info is IsChangeLanePath: "//        << reference_line_info->IsChangeLanePath();}RecordObstacleDebugInfo(reference_line_info);// check path and speed results for path or speed fallbackreference_line_info->set_trajectory_type(ADCTrajectory::NORMAL);// 如果task执行失败，则使用备用的规划轨迹if (!ret.ok()) {PlanFallbackTrajectory(planning_start_point, frame, reference_line_info);}// 对规划的轨迹进行合成，如果合成失败，返回失败状态DiscretizedTrajectory trajectory;if (!reference_line_info->CombinePathAndSpeedProfile(planning_start_point.relative_time(),planning_start_point.path_point().s(), &trajectory)) {const std::string msg = "Fail to aggregate planning trajectory.";AERROR << msg;return Status(ErrorCode::PLANNING_ERROR, msg);}// 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();}}// 增加障碍物的代价for (const auto* obstacle :reference_line_info->path_decision()->obstacles().Items()) {if (obstacle->IsVirtual()) {continue;}if (!obstacle->IsStatic()) {continue;}if (obstacle->LongitudinalDecision().has_stop()) {bool add_stop_obstacle_cost = false;if (dest_stop_s < 0.0) {add_stop_obstacle_cost = true;} else {SLPoint stop_sl = GetStopSL(obstacle->LongitudinalDecision().stop(),reference_line_info->reference_line());if (stop_sl.s() < dest_stop_s) {add_stop_obstacle_cost = true;}}if (add_stop_obstacle_cost) {static constexpr double kReferenceLineStaticObsCost = 1e3;reference_line_info->AddCost(kReferenceLineStaticObsCost);}}}if (FLAGS_enable_trajectory_check) {if (ConstraintChecker::ValidTrajectory(trajectory) !=ConstraintChecker::Result::VALID) {const std::string msg = "Current planning trajectory is not valid.";AERROR << msg;return Status(ErrorCode::PLANNING_ERROR, msg);}}reference_line_info->SetTrajectory(trajectory);reference_line_info->SetDrivable(true);return Status::OK();
}

Task

还是以Lanefollow为例，Task的配置部分在此处。modules/planning/conf/scenario/lane_follow_config.pb.txt

stage_config: {stage_type: LANE_FOLLOW_DEFAULT_STAGEenabled: truetask_type: LANE_CHANGE_DECIDERtask_type: PATH_REUSE_DECIDERtask_type: PATH_LANE_BORROW_DECIDERtask_type: PATH_BOUNDS_DECIDERtask_type: PIECEWISE_JERK_PATH_OPTIMIZERtask_type: PATH_ASSESSMENT_DECIDERtask_type: PATH_DECIDERtask_type: RULE_BASED_STOP_DECIDERtask_type: SPEED_BOUNDS_PRIORI_DECIDERtask_type: SPEED_HEURISTIC_OPTIMIZERtask_type: SPEED_DECIDERtask_type: SPEED_BOUNDS_FINAL_DECIDERtask_type: PIECEWISE_JERK_SPEED_OPTIMIZER# task_type: PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZERtask_type: RSS_DECIDER

参考以及推荐阅

[1] Apollo星火计划学习笔记——第七讲自动驾驶规划技术原理1
[2] Apollo Planning决策规划代码详细解析 (1)：Scenario选择
[3] apollo介绍之planning模块(四)
[4] 解析百度Apollo之决策规划模块
[5] Apollo规划模块（一）：scenario
[6] Apollo星火计划学习笔记——Apollo决策规划技术详解及实现（以交通灯场景检测为例）
[7] Apollo Planning 模块