前言
本文记录学习planning模块时的一些笔记,总体流程参照https://zhuanlan.zhihu.com/p/61982682中的流程图,如上图所示。
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为例。
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
位置姿势,包括车头朝向,在地图上的线速度、线加速度和相对位置,角速度、仰角度等std::mutex
是C++11标准中提供的一种互斥锁机制,用于保护共享资源的访问。当多个线程需要同时访问共享资源时,为了避免出现数据竞争等问题,需要使用互斥锁进行同步控制。
std::mutex
是一种基本的互斥锁类型,可以通过lock()
函数锁定互斥锁,通过unlock()
函数释放互斥锁。当一个线程持有互斥锁时,其他线程想要获取该互斥锁将会被阻塞,直到持有该互斥锁的线程将锁释放。
例如,当多个线程需要同时访问同一个共享变量时,可以在访问之前使用std::mutex
进行锁定,保证每个线程都可以安全地访问该变量。例如:
#include
#include
#include
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进行了锁定,保证了线程的安全性。
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_ROAD
planner_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 stitching
status = 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);
make_unique
是一个C++11中的函数模板,用于创建并返回一个独占指针(unique_ptr)
。它的作用是将一个裸指针封装成一个unique_ptr
对象,并确保该对象是唯一持有它所指向的对象。这可以有效避免内存泄漏和多个指针指向同一个对象的问题。
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_DONE
scenario_manager_.Update(planning_start_point, *frame);
} else if (result == scenario::Scenario::STATUS_UNKNOWN) {
// 当前场景失败
return Status(common::PLANNING_ERROR, "scenario returned unknown");
}
return Status::OK();
}
Apollo规划模块的框架以场景调度为基本决策框架,场景调度本质上是一个双层状态机,顶层是基于场景的管理器,底层是一系列小模块的任务组合,由此来构建出Apollo planning的整体框架。
在执行的每一个场景中,又分为一个或多个阶段,称为stage;而每一个stage又包含一个或多个任务,称为task。执行一个场景,最终就是顺序执行不同阶段的不同任务。由此可见,Scenario在Apollo规划决策中占据着重要的地位。
在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;
// 是否启用该阶段配置,默认值为true
optional 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_FOLLOW
default_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());
// 获取当前要处理的overlaps
Observe(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 PlanningContext
UpdatePlanningContext(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_FOLLOW
ScenarioType 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 finish
if (current_scenario_->GetStatus() !=
Scenario::ScenarioStatus::STATUS_DONE) {
scenario_type = current_scenario_->scenario_type();
}
break;
default:
break;
}
}
// ParkAndGo / starting scenario
if (scenario_type == default_scenario_type_) {
if (FLAGS_enable_scenario_park_and_go) {
scenario_type = SelectParkAndGoScenario(frame);
}
}
// intersection scenarios
if (scenario_type == default_scenario_type_) {
scenario_type = SelectInterceptionScenario(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);
}
return scenario_type;
}
首先,确定一个默认的场景类型,即LANE_FOLLOW。然后,判断是否有驾驶员意图,如果有,则将场景类型改为相应的意图场景类型。如果没有,则进入下一步逻辑。接着检查当前的场景类型,如果是LANE_FOLLOW或PULL_OVER,直接break;若是其他场景,且其他场景还未执行完毕,则继续执行当前场景。如果当前场景已完成,则进入下一步逻辑。PARK_AND_GO和PULL_OVER若有相关配置,则执行相应的函数。注意:每次切换场景都从默认场景开始,最后回到默认场景。
由PublicRoadPlanner::Plan
的流程可知,在获取完当前场景之后,会调用Scenario的Process函数,对具体的场景进行处理。接下来便来看看Scenario的Process函数:
modules/planning/scenarios/scenario.cc
Scenario::ScenarioStatus Scenario::Process(
const common::TrajectoryPoint& planning_init_point, Frame* frame) {
// stage类型unknow
if (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。
以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 refactor
if (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);
// 判断规划结果是否OK
if (cur_status.ok()) {
// 如果发生lanechange,判断reference_line的cost
if (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 version
has_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置位true
ADEBUG << "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 fallback
reference_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();
}
ret = task->Execute(frame, reference_line_info);
依次调用task_list中的task。
还是以Lanefollow为例,Task的配置部分在此处。modules/planning/conf/scenario/lane_follow_config.pb.txt
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
[1] Apollo星火计划学习笔记——第七讲自动驾驶规划技术原理1
[2] Apollo Planning决策规划代码详细解析 (1):Scenario选择
[3] apollo介绍之planning模块(四)
[4] 解析百度Apollo之决策规划模块
[5] Apollo规划模块(一):scenario
[6] Apollo星火计划学习笔记——Apollo决策规划技术详解及实现(以交通灯场景检测为例)
[7] Apollo Planning 模块