1.Apollo Planning 模块总结
1.整体架构
1.1 Planning 的上下游
Planning 模块的输入为：
HMI : 人机交互界面，也就是司机开启了自动驾驶
External Comand：转发的导航命令
Localization : 位置信息
Planning 模块的输出为：
control：输出方向盘和踏板
1.2 Planning 具体运行机制
Planning 模块由 Planning 基础库和 Planning 插件组成，基础库中包含了一些父类方法，比如 scenario.cc 是一种通用类，插件就是该类的子类
Planning component ：是 Planning 模块的外部接口，Init 函数用于初始化组件
TrafficRule ：主要运行 ApplyRule 函数对交通规则进行处理
PublicRoadPlanner : 对开放道路进行规划
Plan()：使用场景机制进行规划
ScenarioManager：Update() 场景选择，切换
Scenario：具体轨迹规划
- planning_interface_base 是 planning 插件的父类接口，也保存了 scenario tasks traffic_rules 的父类文件
- palnner 文件夹中包含几种规划器
- pnc_map 是用来生成参考线的插件
- scenario 是场景插件
- task 是任务插件
- traffic_rule 是交通规则插件
1.3 工程架构
架构图
1.3.1 PlanningComponent::Init()
(1) 定义 Planning 的两种模式
if (FLAGS_use_navigation_mode) {planning_base_ = std::make_unique<NaviPlanning>(injector_);
} else {planning_base_ = std::make_unique<OnLanePlanning>(injector_);
}
一共有两种规划模式：
NaviPlanning 比较简单用于规划高速场景
OnLanePlanning 用于规划城市道路场，泊车
(2) 订阅模块的初始化
在 planing_component.cc 中对规划的输入和输出进行初始化，也就是调用他们的 Reader 和 Writer 方法
1.3.2 PlanningComponent::Proc()
1.4 Senario Stage Task 机制
在不同的 Planner 下有不同的 Scenario，在不同的 Scenario 下又会有不同的动作也就是 Stage ，在不同的 Stage 下又有不同的 task，他们之间相互调用
每个 Senario 使用双层状态机机制，双层状态机 Top Layer 是 Scenario 状态机，BottomLayer 是 Stage 状态机
最外层状态机：
Pull Over 靠边
Parking 泊车
Junction
2.Planning Base
这两个 Planner 主要保存在：
navi_planning 和 on_lane_planning 都继承 PlanningBase，这里以 on_lane_planning 为例
2.1 Init 函数
S1:启动参考线
// 参考线提供器，每 50ms 提供一次参考线
reference_line_provider_ = std::make_unique<ReferenceLineProvider>(injector_->vehicle_state(), reference_line_config);
reference_line_provider_->Start();
S2:创建 Planner
OnLanePlanning 继承于 public PlanningBase 在该类中有一个 std::shared_ptr<Planner> planner_; 的成员变量
Planner 是具体执行规划的类，在 planner 文件夹下 planner 一共有四种
每个 Planner 的作用如下：
- rtk- 根据录制的轨迹来规划行车路线，
- public_road- 开放道路的轨迹规划器，
- lattice- 基于网格算法的轨迹规划器，
- navi- 基于实时相对地图的规划器。
这里根据不同 config 和 injector_ 创建不同的具体 Planner
首先创建具体 Planner
LoadPlanner();
在该函数中根据 config 创建具体的 Planner ，默认是 PublicRoadPlanner
void PlanningBase::LoadPlanner() {// Use PublicRoadPlanner as default Plannerstd::string planner_name = "apollo::planning::PublicRoadPlanner";if ("" != config_.planner()) {planner_name = config_.planner();planner_name = ConfigUtil::GetFullPlanningClassName(planner_name);}planner_ =cyber::plugin_manager::PluginManager::Instance()->CreateInstance<Planner>(planner_name);
}
Planner 的 Init 主要是对 Planner 的 injector 进行赋值
return planner_->Init(injector_, FLAGS_planner_config_path);
virtual apollo::common::Status Init(const std::shared_ptr<DependencyInjector>& injector,const std::string& config_path = "") {injector_ = injector;return common::Status::OK();
}
2.2 RunOnce 函数
S1:初始化 Frame
// 初始化Frame
status = InitFrame(frame_num, stitching_trajectory.back(), vehicle_state);
S2:判断是否符合交通规则
// 判断是否符合交通规则for (auto& ref_line_info : *frame_->mutable_reference_line_info()) {auto traffic_status =traffic_decider_.Execute(frame_.get(), &ref_line_info);if (!traffic_status.ok() || !ref_line_info.IsDrivable()) {ref_line_info.SetDrivable(false);AWARN << "Reference line " << ref_line_info.Lanes().Id()<< " traffic decider failed";}}
S3:Plan
1.调用 OnLanePlanning 的 Plan 方法
// Plan
status = Plan(start_timestamp, stitching_trajectory, ptr_trajectory_pb);
2.在具体的 Plan 函数中，planner 调用自己的 Plan 函数
// 调用具体的 planner 执行
auto status = planner_->Plan(stitching_trajectory.back(), frame_.get(),ptr_trajectory_pb);
3.PublicRoadPlanner
OnLanePlanning 类使用的 planner_ 为 PublicRoadPlanner 这个 Planner 也是默认的 Planner
3.1 配置文件
所有 planner 的配置文件都在目标文件夹下的 conf 子文件夹下，定义了每个 Planner 的 scenario
2.2 PublicRoadPlanner::Init
会对 Senario Manager 进行初始化，也就是创建多个 scenario 放在一个 list 中
scenario_manager_.Init(injector, config_);
这个 planner_ 支持的场景如下：
2.2 Plan
在 Plan 中会对 Senario 进行更新，然后执行
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();if (!scenario_) {return Status(apollo::common::ErrorCode::PLANNING_ERROR,"Unknown 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_plugin_type(scenario_->Name());scenario_debug->set_stage_plugin_type(scenario_->GetStage());scenario_debug->set_msg(scenario_->GetMsg());}// 更新执行的状态if (result.GetScenarioStatus() == ScenarioStatusType::STATUS_DONE) {// only updates scenario manager when previous scenario's status is// STATUS_DONEscenario_manager_.Update(planning_start_point, frame);} else if (result.GetScenarioStatus() == ScenarioStatusType::STATUS_UNKNOWN) {return Status(common::PLANNING_ERROR,result.GetTaskStatus().error_message());}return Status(common::OK, result.GetTaskStatus().error_message());
}
3.Scenario
场景介绍
有如下的 Scenario 场景，所有的 Senario 都用 SenarioManager 进行管理，所有的 Scenario 都会继承 Scenario 类重写里面的方法
在 SenarioManager 中有一个 list 用于存放需要运行的 senario
std::vector<std::shared_ptr<Scenario>> scenario_list_;
3.1 SenarioManager Init
将场景加载到 list 中
3.2 SenarioManager Update
在 scenario.cc 文件中 Process 函数会对 Stage 进行调用，首先先对当前 Stage 进行处理，然后更新 Stage 的状态，然后在创建下一个 Stage
在这个函数中主要是调用 ，判断是否可以进行该 scenario 的场景奇切换
scenario->IsTransferable(current_scenario_.get(), *frame)
3.3 Scenario IsTransferable
这里拿最简单的 Lane_follow 的场景来说，下面是判断是否可以 Lane follow 的条件：
bool LaneFollowScenario::IsTransferable(const Scenario* other_scenario,const Frame& frame) {if (!frame.local_view().planning_command->has_lane_follow_command()) {return false;}if (frame.reference_line_info().empty()) {return false;}if (other_scenario == nullptr) {return true;}return true;
}
lane follow 比较简单，下面是判断是否可以靠边停车的代码：
判断是否可以靠边停车
代码中主要判断 Reference 的点是否与红绿灯，停止等标志有 Overlap
auto first_encountered_overlaps =frame.reference_line_info().front().FirstEncounteredOverlaps();
// 如果处于红绿灯路口、具有停止标志、避让标志，则不允许靠边停车
if (pull_over_scenario) {static constexpr double kDistanceToAvoidJunction = 8.0; // meterfor (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) {const double distance_to = overlap.second.start_s - dest_sl.s();const double distance_passed = dest_sl.s() - overlap.second.end_s;if ((distance_to > 0.0 && distance_to < kDistanceToAvoidJunction) ||(distance_passed > 0.0 &&distance_passed < kDistanceToAvoidJunction)) {pull_over_scenario = false;break;}}}
}
3.4 Scenario::Process
会运行具体的 Stage
auto ret = current_stage_->Process(planning_init_point, frame);
3.5 Enter
场景的进⼊函数继承于基类的Enter()函数，在⾸次进⼊场景前调⽤做⼀些预处理的⼯作，重置场景内变量。 ⽐如，在停⽌标记场景的Enter()函数，⾸先寻找参考线的停⽌标记id保存到上下⽂变量中，然后重置停⽌标记的全局变量。
3.6 Exit
场景的退出函数继承于基类的Exit()函数，在场景切出时会被调⽤，可以⽤来清除⼀些全局变量，⽐如停⽌标记场景的切出函数
bool PullOverScenario::Exit(Frame* frame) {injector_->planning_context()->mutable_planning_status()->clear_pull_over();return true;
}
3.7 Scenario::Init()
场景的初始化需要继承Scenario的Init()函数，场景基类的Init函数主要是从场景插件中加载场景的流⽔线，将加载的 Stage 实例保存到stage_pipeline_map_中
下面以 pull_over_scenario.cc 为例
if (!Scenario::Init(injector, name)) {AERROR << "failed to init scenario" << Name();return false;
}
如果场景⾃身还有配置⽂件，则可以调⽤Scenario::LoadConfig()函数加载场景⾃身的配置⽂件，保存到场景实例上下⽂变量中context_。
if (!Scenario::LoadConfig<ScenarioPullOverConfig>(&context_.scenario_config)) {AERROR << "fail to get config of scenario" << Name();return false;
}
4.Stage
每个场景⼜分为⼀个或者多个阶段(stage)，每个阶段⼜由不同的任务(task)组成，
4.1 配置文件
当场景中存在先后顺序的业务逻辑时，可以将其划分成多个 Stage。⽐如，在红绿灯⽆保护左转场景中可以划分为三个阶段，第⼀个阶段是接近⾏驶到红绿灯停⽌线前的过程，第⼆个是红绿灯为绿灯时慢速观望的过程，第三个是对向直⾏⻋道通畅快速通过的过程
可以根据 Scenario 的 conf 文件中定义不同的 task 文件，
下面以 lane follow stage 为例：
对于 lane follow 来讲他会有 lane follow 的 stage，lane change ，lane borrow 等等的状态
下面是 stop sign unprotected Stage ，在开边停车的 Scenario 中就可以看出
stage: { name: "STOP_SIGN_UNPROTECTED_PRE_STOP" // 车辆到达停止牌之前type: "StopSignUnprotectedStagePreStop"enabled: truetask {name: "LANE_FOLLOW_PATH"type: "LaneFollowPath"}....
}
stage: {name: "STOP_SIGN_UNPROTECTED_STOP" // 到达车辆停止牌处如何处理type: "StopSignUnprotectedStageStop"enabled: truetask {name: "LANE_FOLLOW_PATH"type: "LaneFollowPath"}......
}
stage: {name: "STOP_SIGN_UNPROTECTED_CREEP" // 在停牌处停好车，起步，缓慢前行type: "StopSignUnprotectedStageCreep"enabled: truetask {name: "LANE_FOLLOW_PATH"type: "LaneFollowPath"}.......
}
stage: {name: "STOP_SIGN_UNPROTECTED_INTERSECTION_CRUISE" // 通过了停牌路口，进入路口巡航阶段type: "StopSignUnprotectedStageIntersectionCruise"enabled: truetask {name: "LANE_FOLLOW_PATH"type: "LaneFollowPath"......
}
4.2 Stage 的状态机
RUNNING：表示当前状态正在运⾏，Scenario：将继续维持当前阶段运⾏；
FINISHED：表示当前阶段完成，Scenario将会切⼊下⼀个阶段运⾏；
ERROR：表示当前规划存在严重故障。Scenario 会将其上报，主⻋将会刹停。
4.3 LaneFollowStage::Process
这里会对每一条参考线进行处理，这里会对每条参考线调用 task 方法
result =PlanOnReferenceLine(planning_start_point, frame, &reference_line_info);
4.4 LaneFollowStage::PlanOnReferenceLine
在这个函数中会逐个调用 task 执行
for (auto task : task_list_) {const double start_timestamp = Clock::NowInSeconds();const auto start_planning_perf_timestamp =std::chrono::duration<double>(std::chrono::system_clock::now().time_since_epoch()).count();ret.SetTaskStatus(task->Execute(frame, reference_line_info));
5.Task
task 可以理解为具体步骤，如 path 求解器的定义，boundery 函数的定义，以及速度求解器的定义等等
5.1 配置文件
5.2 Init()
5.3 Excute()
所有的 task 都会继承 Task 父类，每个子类实现 Execute 函数
6.Traffic rule
交通规则插件 TrafficRule 主要是在规划模块执行 Scenario 前对交通规则进⾏处理
6.1 配置文件
配置文件在每个文件夹下的 conf 子文件夹下
6.2 Init()
这里以 Crosswalk 类为例，Init 函数主要是将 conf 文件加载进来
// Load the config this task.
return TrafficRule::LoadConfig<CrosswalkConfig>(&config_);
6.3 Crosswalk::ApplyRule
traffic rule 的运行函数为 applyRule 函数，该函数会将判断结果保存在 reference line 中
...if (!FindCrosswalks(reference_line_info)) {injector_->planning_context()->mutable_planning_status()->clear_crosswalk();return Status::OK();}
...
}
7.参数设置
7.1 全局参数设置
全局参数定义在 plannning.conf 文件夹下
在 plannig_gflags.cc 中进行初始化，使用 FLAGS_XXX 的方式进行调用
