Apollo planning模块pipeline详细分析
Posted bitcarmanlee
tags:
篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了Apollo planning模块pipeline详细分析相关的知识，希望对你有一定的参考价值。
1.apollo整体结构
最近利用工作业余时间，抽空学习一下apollo项目的源码。整体来说，apollo的代码量还是挺大，对于入门时间不长的新手来说，逻辑也不是太容易一眼看懂。因此，结合源码以及其他相关理解分析材料，先重点了解了一下apollo中的planning模块也就是规控模块的，把整个planning模块的pipeline大致理清。
本文参考的代码均为apollo 7.0版本，apollo项目github地址上有一张图，反应了整个系统的结构
2.Planning模块框架
planning模块，包含有决策与规划两部分功能。
模块的输入包括：
定位、感知、预测、高精地图、路由、任务管理模块。
输出为：
控制模块可执行的顺滑无碰撞轨迹。
plan模块的整体架构为
整体说来，可以分为三层(参考参考文献4的提法，个人感觉比较合理)
1.可执行代码层。planning模块的入口类为planning_component，·planning_component中会调用合适的planner类，确定相应的车辆意图，执行该意图所需的规划任务并生成规划轨迹。它还将更新未来作业的上下文。
可以看出，planner中包含有lattice/navi/public_road/rtk这几种方式，目前默认的是public_road。
2.app层 每种planner又可以分为多个Scenario。特意查了一下相关资料，Scenario翻译为场景可能比较合适。而每个Scenario又可以分成不同的stage。
我们以bare_intersection_unprotected_scenario为例，该scenario包含有两个stage，一个为approach，一个为cruise。
3.lib层。每个stage，包含有可执行的tasks。Task是Deciders & Optimizers :一组实现决策任务和各种优化的无状态库。优化器负责优化车辆的轨迹和速度。决策者是基于规则的分类决策者，他们建议何时换车道、何时停车、何时爬行(慢速行进)或爬行何时完成。
上面就是task包中的deciders与optimizers。
3.上下游数据pipeline分析
要了解一个模块的逻辑，最好的方式是结合数据流的方向来进行梳理分析。结合代码以及全局的架构图，我们总结了planning模块的输入输出
在planning.component.h文件中，作为入口程序，可以观察到的输入有
public:
bool Init() override;
bool Proc(const std::shared_ptr<prediction::PredictionObstacles>&
prediction_obstacles,
const std::shared_ptr<canbus::Chassis>& chassis,
const std::shared_ptr<localization::LocalizationEstimate>&
localization_estimate) override;
...
private:
std::shared_ptr<cyber::Reader<perception::TrafficLightDetection>>
traffic_light_reader_;
std::shared_ptr<cyber::Reader<routing::RoutingResponse>> routing_reader_;
...
std::shared_ptr<cyber::Reader<relative_map::MapMsg>> relative_map_reader_;
private部分成员变量，包含有如下输入：
1.traffic_light_reader_ 交通信号灯
2.routing_reader_ 全局导航信息
3.relative_map_reader_ 高清地图
proc方法，包含的输入：
4.prediction_obstacles 预测障碍物大小，轨迹等相关信息
5.chassis 底盘信息
6.localization_estimate 预测的位置信息
而整个模块的输出，在proc方法最后可以看到
ADCTrajectory adc_trajectory_pb;
....
planning_writer_->Write(adc_trajectory_pb);
最后输出的是ADCTrajectory类型，并通过planning_writer_写入。ADCTrajectory是经过所有逻辑处理的最终结果，包含有车辆行驶所需要的所有信息，这个数据将直接影响到车的自动驾驶行为。
4.代码pipeline之Init方法
前面提到，整个planning模块的入口为planning.component.cc(.h为头文件)，主要包含了两个方法：Init()与Proc()。
首先我们分析Init方法的逻辑。
4.1 选择规划器
bool PlanningComponent::Init()
injector_ = std::make_shared<DependencyInjector>();
if (FLAGS_use_navigation_mode)
planning_base_ = std::make_unique<NaviPlanning>(injector_);
else
planning_base_ = std::make_unique<OnLanePlanning>(injector_);
....
首先根据配置，选择planning类型
1.NaviPlanning，基于地图规划
2.OnLanePlanning，默认规划器，用于public_road。
这两个规划器都有共同基类PlanningBase。
4.2 选择planner
planning_base_->Init(config_);
规划器选择完毕，然后有planning_base_进行Init()操作。
前面我们提到默认的planning是OnLanePlanning，进入到OnLanePlanning中看一下 Init的操作。
Status OnLanePlanning::Init(const PlanningConfig& config)
config_ = config;
if (!CheckPlanningConfig(config_))
return Status(ErrorCode::PLANNING_ERROR,
"planning config error: " + config_.DebugString());
PlanningBase::Init(config_);
planner_dispatcher_->Init();
...
// clear planning history
injector_->history()->Clear();
// clear planning status
injector_->planning_context()->mutable_planning_status()->Clear();
// load map
hdmap_ = HDMapUtil::BaseMapPtr();
ACHECK(hdmap_) << "Failed to load map";
// instantiate reference line provider
reference_line_provider_ = std::make_unique<ReferenceLineProvider>(
injector_->vehicle_state(), hdmap_);
reference_line_provider_->Start();
// dispatch planner
planner_ = planner_dispatcher_->DispatchPlanner(config_, injector_);
...
return planner_->Init(config_);
...
1.首先通过injector_，将planning的一些历史信息以及状态进行clear。
2.进行load map操作。
3.reference_line_provider_启动，根据导航信息生成的一条或多条道路中心线，即 reference_line
4.planner_dispatcher_进行planner选择。
前面我们提到了四种planner，分别为lattice/navi/public_road/rtk，默认使用的是public_road。
5.代码最后一行是planner_->Init(config_); 比如我们在PublicRoadPlanner中查看，Init代码如下：
Status PublicRoadPlanner::Init(const PlanningConfig& config)
config_ = config;
scenario_manager_.Init(config);
return Status::OK();
上面的代码，就是对scenario_manager_进行了注册。
4.3 创建reader/writer
routing_reader_ = node_->CreateReader<RoutingResponse>(
config_.topic_config().routing_response_topic(),
[this](const std::shared_ptr<RoutingResponse>& routing)
AINFO << "Received routing data: run routing callback."
<< routing->header().DebugString();
std::lock_guard<std::mutex> lock(mutex_);
routing_.CopyFrom(*routing);
);
traffic_light_reader_ = node_->CreateReader<TrafficLightDetection>(
config_.topic_config().traffic_light_detection_topic(),
[this](const std::shared_ptr<TrafficLightDetection>& traffic_light)
ADEBUG << "Received traffic light data: run traffic light callback.";
std::lock_guard<std::mutex> lock(mutex_);
traffic_light_.CopyFrom(*traffic_light);
);
pad_msg_reader_ = node_->CreateReader<PadMessage>(
config_.topic_config().planning_pad_topic(),
[this](const std::shared_ptr<PadMessage>& pad_msg)
ADEBUG << "Received pad data: run pad callback.";
std::lock_guard<std::mutex> lock(mutex_);
pad_msg_.CopyFrom(*pad_msg);
);
...
planning_learning_data_writer_ = node_->CreateWriter<PlanningLearningData>(
config_.topic_config().planning_learning_data_topic());
return true;
执行完 planning_base_->Init(config_);以后，开始注册各种reader/writer,为cyber RT框架用于通信的部分，先不做过多关注，至此Init方法结束。
5.Proc方法
5.1 注册local_view
bool PlanningComponent::Proc(
const std::shared_ptr<prediction::PredictionObstacles>&
prediction_obstacles,
const std::shared_ptr<canbus::Chassis>& chassis,
const std::shared_ptr<localization::LocalizationEstimate>&
localization_estimate)
ACHECK(prediction_obstacles != nullptr);
// check and process possible rerouting request
CheckRerouting();
// process fused input data
local_view_.prediction_obstacles = prediction_obstacles;
local_view_.chassis = chassis;
local_view_.localization_estimate = localization_estimate;
std::lock_guard<std::mutex> lock(mutex_);
if (!local_view_.routing ||
hdmap::PncMap::IsNewRouting(*local_view_.routing, routing_))
local_view_.routing =
std::make_shared<routing::RoutingResponse>(routing_);
std::lock_guard<std::mutex> lock(mutex_);
local_view_.traffic_light =
std::make_shared<TrafficLightDetection>(traffic_light_);
local_view_.relative_map = std::make_shared<MapMsg>(relative_map_);
std::lock_guard<std::mutex> lock(mutex_);
local_view_.pad_msg = std::make_shared<PadMessage>(pad_msg_);
std::lock_guard<std::mutex> lock(mutex_);
local_view_.stories = std::make_shared<Stories>(stories_);
...
proc方法，先是对local_view_各种数据进行设置。
查看一下LocalView的源码
/**
* @struct local_view
* @brief LocalView contains all necessary data as planning input
*/
struct LocalView
std::shared_ptr<prediction::PredictionObstacles> prediction_obstacles;
std::shared_ptr<canbus::Chassis> chassis;
std::shared_ptr<localization::LocalizationEstimate> localization_estimate;
std::shared_ptr<perception::TrafficLightDetection> traffic_light;
std::shared_ptr<routing::RoutingResponse> routing;
std::shared_ptr<relative_map::MapMsg> relative_map;
std::shared_ptr<PadMessage> pad_msg;
std::shared_ptr<storytelling::Stories> stories;
;
// namespace planning
// namespace apollo
根据上面的注释，不难看出，planning模块所需的所有input都在该结构体中。
5.2 Planning_base_->RunOnce()
ADCTrajectory adc_trajectory_pb;
planning_base_->RunOnce(local_view_, &adc_trajectory_pb);
...
planning_writer_->Write(adc_trajectory_pb);
注册完毕localview以后，重点就是planning_base_->RunOnce，这是planning真正的计算逻辑所在，输入为local_view_与adc_trajectory_pb， 输出为planning_writer_->Write(adc_trajectory_pb)。
6.RunOnce
我们看下on_lane_planning中的RunOnce方法。
void OnLanePlanning::RunOnce(const LocalView& local_view,
ADCTrajectory* const ptr_trajectory_pb)
// when rerouting, reference line might not be updated. In this case, planning
// module maintains not-ready until be restarted.
static bool failed_to_update_reference_line = false;
local_view_ = local_view;
const double start_timestamp = Clock::NowInSeconds();
const double start_system_timestamp =
std::chrono::duration<double>(
std::chrono::system_clock::now().time_since_epoch())
.count();
...
// Update reference line provider and reset pull over if necessary
reference_line_provider_->UpdateVehicleState(vehicle_state);
...
status = InitFrame(frame_num, stitching_trajectory.back(), vehicle_state);
...
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";
status = Plan(start_timestamp, stitching_trajectory, ptr_trajectory_pb);
for (const auto& p : ptr_trajectory_pb->trajectory_point())
ADEBUG << p.DebugString();
...
if (!status.ok())
status.Save(ptr_trajectory_pb->mutable_header()->mutable_status());
AERROR << "Planning failed:" << status.ToString();
if (FLAGS_publish_estop)
AERROR << "Planning failed and set estop";
// "estop" signal check in function "Control::ProduceControlCommand()"
// estop_ = estop_ || local_view_.trajectory.estop().is_estop();
// we should add more information to ensure the estop being triggered.
EStop* estop = ptr_trajectory_pb->mutable_estop();
estop->set_is_estop(true);
estop->set_reason(status.error_message());
...
if (FLAGS_enable_planning_smoother)
planning_smoother_.Smooth(injector_->frame_history(), frame_.get(),
ptr_trajectory_pb);
因为方法代码较长，有比较多的debug相关的msg,因此把代码中核心部分逻辑选出来进行解读。
1.根据注释不难看出，reference_line_provider_会更新reference_line。如果失败，会生成一个停车轨迹。
2.InitFrame方法，主要是生成reference_lines。
3.for循环中有traffic_decider，主要是与交规相关的决策内容。
4.status = Plan(start_timestamp, stitching_trajectory, ptr_trajectory_pb);,plan的主要逻辑，后面我们再讲。
5.规划失败，执行estop()。
6.最后一步，对轨迹进行平滑处理。
7.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();
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();
上面是PublicRoadPlanner中的Plan方法源码。
主要是包含有两个步骤：
1.scenario_manager_.Update(planning_start_point, *frame);，这一句用来更新当前的场景。
2.auto result = scenario_->Process(planning_start_point, frame)
前面我们已经提到，apollo中，scenario->stage->task是层层递进关系，即驾驶中有许多scenario(场景)，每个场景分为一个或者多个stage(阶段)，每个阶段又是由多个task(任务)组成。
进入到Process方法
Scenario::Scenariostatus Scenario::Process(
const common::TrajectoryPoint& planning_init_point, Frame* frame)
if (current_stage_ == nullptr)
AWARN << "Current stage is a null pointer.";
return STATUS_UNKNOWN;
if (current_stage_->stage_type() == ScenarioConfig::NO_STAGE)
scenario_status_ = STATUS_DONE;
return scenario_status_;
auto ret = current_stage_->Process(planning_init_point, frame);
以LaneFollowStage中的 Process方法为例
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;
.Apollo源码分析系列的第六部分planning
开发者说丨动态规划及其在Apollo项目Planning模块的应用
Azure DevOps Pipelines 中的条件阶段执行
大数据ClickHouse进阶：ClickHouse的explain查询执行计划