前言
本文记录学习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_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);
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_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();
}
scenario部分见Apollo星火计划学习笔记——Apollo决策规划技术详解及实现(以交通灯场景检测为例)、Apollo星火计划学习笔记——第七讲自动驾驶规划技术原理1以及后续文章
参考文章
[1] apollo介绍之planning模块(四)
[2] Apollo Planning 模块