Apollo Planning模块源代码分析
严正声明：本文系作者davidhopper原创，未经允许，严禁转载！
阿波罗项目（https://github.com/ApolloAuto/apollo）规划（Planning）模块位于命名空间：apollo::planning，其作用在于构建无人车从起点到终的局部行驶路径，具体而言，就是给定导航地图、导航路径、当前定位点、车辆状态（包括：位置、速度、加速度、底盘）、 周边目标的感知及预测信息（如交通标志和障碍物等），规划模块计算出可供控制模块（Controller）执行的一条安全且舒适的行驶路径。注意，规划模块输出的路径是局部路径而非全局路径。举个简单示例加以说明，假如无人车需从长沙智能驾驶研究院行驶至长沙高铁南站，首先需借助Routing（路由寻径）模块输出全局导航路径，接下来才是规划模块基于全局导航路径进行一小段、一小段具体行驶路径的规划。规划模块内部结构及其与其他模块的交互示意如下图所示。
规划模块内部的主要类成员如下图所示：
该模块的主要执行流程如下图所示：
一、模块主入口
该模块的主入口为：modules/planning/main.cc：
APOLLO_MAIN(apollo::planning::Planning)
该宏展开后为：
int main(int argc, char **argv) {
google::InitGoogleLogging(argv[0]);
google::ParseCommandLineFlags(&argc, &argv, true);
signal(SIGINT, apollo::common::apollo_app_sigint_handler);
apollo::planning::Planning apollo_app_;
ros::init(argc, argv, apollo_app_.Name());
apollo_app_.Spin();
return 0;
}
Main函数完成以下工作：始化Google日志工具，使用Google命令行解析工具解析相关参数，注册接收中止信号“SIGINT”的处理函数：apollo::common::apollo_app_sigint_handler（该函数的功能十分简单，就是收到中止信号“SIGINT”后，调用ros::shutdown()关闭ROS），创建apollo::planning::Planning对象：apollo_app_，初始化ROS环境，调用apollo_app_.Spin()函数开始消息处理循环。
int ApolloApp::Spin() {
ros::AsyncSpinner spinner(callback_thread_num_);
auto status = Init();
if (!status.ok()) {
AERROR << Name() << " Init failed: " << status;
ReportModuleStatus(apollo::hmi::ModuleStatus::UNINITIALIZED);
return -1;
}
ReportModuleStatus(apollo::hmi::ModuleStatus::INITIALIZED);
status = Start();
if (!status.ok()) {
AERROR << Name() << " Start failed: " << status;
ReportModuleStatus(apollo::hmi::ModuleStatus::STOPPED);
return -2;
}
ExportFlags();
ReportModuleStatus(apollo::hmi::ModuleStatus::STARTED);
spinner.start();
ros::waitForShutdown();
Stop();
ReportModuleStatus(apollo::hmi::ModuleStatus::STOPPED);
AINFO << Name() << " exited.";
return 0;
}
apollo::planning::Planning类的继承关系见下图：
基本时序如下图所示：
现在来看ApolloApp::Spin()函数内部，首先创建ros::AsyncSpinner对象spinner，监控用户操作。之后调用虚函数：Init()（实际调用apollo::planning::Planning::Init()）执行初始化。
Status Planning::Init() {
hdmap_ = apollo::hdmap::HDMapUtil::BaseMapPtr();
CHECK(hdmap_) << "Failed to load map file:" << apollo::hdmap::BaseMapFile();
CHECK(apollo::common::util::GetProtoFromFile(FLAGS_planning_config_file,
&config_))
<< "failed to load planning config file " << FLAGS_planning_config_file;
if (!AdapterManager::Initialized()) {
AdapterManager::Init(FLAGS_planning_adapter_config_filename);
}
if (AdapterManager::GetLocalization() == nullptr) {
std::string error_msg("Localization is not registered");
AERROR << error_msg;
return Status(ErrorCode::PLANNING_ERROR, error_msg);
}
if (AdapterManager::GetChassis() == nullptr) {
std::string error_msg("Chassis is not registered");
AERROR << error_msg;
return Status(ErrorCode::PLANNING_ERROR, error_msg);
}
if (AdapterManager::GetRoutingResponse() == nullptr) {
std::string error_msg("RoutingResponse is not registered");
AERROR << error_msg;
return Status(ErrorCode::PLANNING_ERROR, error_msg);
}
if (AdapterManager::GetRoutingRequest() == nullptr) {
std::string error_msg("RoutingRequest is not registered");
AERROR << error_msg;
return Status(ErrorCode::PLANNING_ERROR, error_msg);
}
if (FLAGS_enable_prediction && AdapterManager::GetPrediction() == nullptr) {
std::string error_msg("Enabled prediction, but no prediction not enabled");
AERROR << error_msg;
return Status(ErrorCode::PLANNING_ERROR, error_msg);
}
if (FLAGS_enable_traffic_light &&
AdapterManager::GetTrafficLightDetection() == nullptr) {
std::string error_msg("Traffic Light Detection is not registered");
AERROR << error_msg;
return Status(ErrorCode::PLANNING_ERROR, error_msg);
}
ReferenceLineProvider::instance()->Init(
hdmap_, config_.qp_spline_reference_line_smoother_config());
RegisterPlanners();
planner_ = planner_factory_.CreateObject(config_.planner_type());
if (!planner_) {
return Status(
ErrorCode::PLANNING_ERROR,
"planning is not initialized with config : " + config_.DebugString());
}
return planner_->Init(config_);
}
在apollo::planning::Planning::Init()函数内部，首先获取高精地图指针，之后执行适配器管理者（AdapterManager）对象的初始化状态，接着检查AdapterManager里定位适配器（AdapterManager::GetLocalization()）、底盘适配器（AdapterManager::GetChassis()）、路由寻径响应适配器（AdapterManager::GetRoutingResponse()）、路由寻径请求适配器（AdapterManager::GetRoutingRequest()）的初始化状态，若启用预测（FLAGS_enable_prediction）则继续检查预测适配器（AdapterManager::GetPrediction()）的初始化状态，若启用交通信号灯（FLAGS_enable_traffic_light）则继续检查交通信号灯适配器（AdapterManager::GetTrafficLightDetection()）的初始化状态。接下来，执行参考线提供者（所谓参考线就是一条候选路径）的初始化。此后调用Planning::RegisterPlanners()将RTKReplayPlanner、EMPlanner对象的创建函数注册到计划者工厂类对象planner_factory_中。另外通过读取配置文件中给定的计划者类型，使用planner_factory_.CreateObject(config_.planner_type())动态生成所需的计划者对象（查询配置文件得知，实际生成EMPlanner对象）。最后执行计划者对象的初始化：planner_->Init(config_)。apollo::planning::Planning::Init()函数内，适配器管理者的初始化过程值得深入探讨：
AdapterManager::Init(FLAGS_planning_adapter_config_filename);
首先研究初始化传入的参数：FLAGS_planning_adapter_config_filename实际上是利用Google开源库gflags宏：
DEFINE_string(planning_adapter_config_filename,
"modules/planning/conf/adapter.conf",
"The adapter configuration file");
定义出的一个字符串变量，该变量的缺省值为：“modules/planning/conf/adapter.conf”，关于该变量的描述信息为： “The adapter configuration file”。配置文件：modules/planning/conf/adapter.conf的内容如下：
config {
type: LOCALIZATION
mode: RECEIVE_ONLY
message_history_limit: 1
}
config {
type: CHASSIS
mode: RECEIVE_ONLY
message_history_limit: 1
}
config {
type: ROUTING_RESPONSE
mode: RECEIVE_ONLY
message_history_limit: 1
}
config {
type: ROUTING_REQUEST
mode: PUBLISH_ONLY
message_history_limit: 1
}
config {
type: PREDICTION
mode: RECEIVE_ONLY
message_history_limit: 10
}
config {
type: PLANNING_TRAJECTORY
mode: PUBLISH_ONLY
message_history_limit: 1
}
config {
type: TRAFFIC_LIGHT_DETECTION
mode: RECEIVE_ONLY
message_history_limit: 1
}
is_ros: true
该文件表明，Planning模块配置以下几种类型的适配器：LOCALIZATION（仅接收）、CHASSIS（仅接收）、ROUTING_RESPONSE（仅接收）、ROUTING_REQUEST（仅发布）、PREDICTION（仅接收）、PLANNING_TRAJECTORY（仅发布）、TRAFFIC_LIGHT_DETECTION（仅接收），且使用ROS环境。
现在进入AdapterManager::Init（const std::string &adapter_config_filename）内部一探究竟：
void AdapterManager::Init(const std::string &adapter_config_filename) {
// Parse config file
AdapterManagerConfig configs;
CHECK(util::GetProtoFromFile(adapter_config_filename, &configs))
<< "Unable to parse adapter config file " << adapter_config_filename;
AINFO << "Init AdapterManger config:" << configs.DebugString();
Init(configs);
}
顺藤摸瓜，继续深入AdapterManager::Init(const AdapterManagerConfig &configs)内部，前面我们通过查看配置文件adapter.conf得知，此次配置类别为：LOCALIZATION、CHASSIS、ROUTING_RESPONSE、ROUTING_REQUEST、PREDICTION、PLANNING_TRAJECTORY、TRAFFIC_LIGHT_DETECTION。
void AdapterManager::Init(const AdapterManagerConfig &configs) {
if (Initialized()) {
return;
}
instance()->initialized_ = true;
if (configs.is_ros()) {
instance()->node_handle_.reset(new ros::NodeHandle());
}
for (const auto &config : configs.config()) {
switch (config.type()) {
// 省略不相关的配置类别
...
case AdapterConfig::CHASSIS:
EnableChassis(FLAGS_chassis_topic, config);
break;
case AdapterConfig::LOCALIZATION:
EnableLocalization(FLAGS_localization_topic, config);
break;
// 省略不相关的配置类别
...
case AdapterConfig::TRAFFIC_LIGHT_DETECTION:
EnableTrafficLightDetection(FLAGS_traffic_light_detection_topic,
config);
// 省略不相关的配置类别
...
case AdapterConfig::ROUTING_REQUEST:
EnableRoutingRequest(FLAGS_routing_request_topic, config);
break;
case AdapterConfig::ROUTING_RESPONSE:
EnableRoutingResponse(FLAGS_routing_response_topic, config);
break;
case AdapterConfig::PLANNING_TRAJECTORY:
EnablePlanning(FLAGS_planning_trajectory_topic, config);
break;
case AdapterConfig::PREDICTION:
EnablePrediction(FLAGS_prediction_topic, config);
break;
// 省略不相关的配置类别
...
default:
AERROR << "Unknown adapter config type!";
break;
}
上述代码中的FLAGS_chassis_topic、FLAGS_localization_topic、FLAGS_traffic_light_detection_topic、FLAGS_routing_request_topic、FLAGS_routing_response_topic、FLAGS_planning_trajectory_topic、FLAGS_prediction_topic是利用DEFINE_string宏定义出来的几个字符串变量，其缺省值分别为：
“/apollo/canbus/chassis”
“/apollo/localization/pose”
“/apollo/perception/traffic_light”
“/apollo/routing_request”
“/apollo/routing_response”
“/apollo/prediction”
代码中出现了几个函数：
EnableChassis、
EnableLocalization、
EnableTrafficLightDetection、
EnableRoutingRequest、
EnableRoutingResponse、
EnablePlanning、
EnablePrediction，
但我们找遍AdapterManager类内部，也无法发现它们的定义，这是从哪里冒出来的呢？其实这是宏
REGISTER_ADAPTER(Chassis)、
REGISTER_ADAPTER(Localization)、
REGISTER_ADAPTER (TrafficLightDetection)、
REGISTER_ADAPTER(RoutingRequest)、
REGISTER_ADAPTER (RoutingResponse)、
REGISTER_ADAPTER(Planning)、
REGISTER_ADAPTER(Prediction)
的杰作。
下面以REGISTER_ADAPTER(Planning)为例进行分析：命名空间Apollo::common::adapter内的适配器管理者类AdapterManager使用宏REGISTER_ADAPTER (Planning)注册了一个PlanningAdapter类对象及与其相关的变量、函数。这个PlanningAdapter类是何方神圣？请看文件：modules/common/adapters/message_adapters.h内的这条语句：
using PlanningAdapter = Adapter<planning::ADCTrajectory>;
原来PlanningAdapter类是模板类Adapter的一个别名。
再来看宏REGISTER_ADAPTER(Planning)展开后的实际代码，是不是找到了我们关注的函数：EnablePlanning(const std::string &topic_name, const AdapterConfig &config)？分析该函数内部可知其功能有两个：一是订阅接收消息FLAGS_planning_trajectory_topic，并绑定该消息的处理函数为PlanningAdapter::OnReceive；二是将匿名函数（或称Lambda表达式，不熟悉的同学可查阅C++11标准）[this]() { Planning_->Observe(); }作为规划适配器PlanningAdapter的观察函数存储到函数对象数组：std::vector内部，提供给相关代码调用。
public:
static void EnablePlanning(const std::string &topic_name,
const AdapterConfig &config) {
CHECK(config.message_history_limit() > 0)
<< "Message history limit must be greater than 0";
instance()->InternalEnablePlanning(topic_name, config);
}
static PlanningAdapter *GetPlanning() {
return instance()->InternalGetPlanning();
}
static AdapterConfig &GetPlanningConfig() {
return instance()->Planningconfig_;
}
static bool FeedPlanningFile(const std::string &proto_file) {
if (!instance()->Planning_) {
AERROR << "Initialize adapter before feeding protobuf";
return false;
}
return GetPlanning()->FeedFile(proto_file);
}
static void PublishPlanning(const PlanningAdapter::DataType &data) {
instance()->InternalPublishPlanning(data);
}
template <typename T>
static void FillPlanningHeader(const std::string &module_name, T *data) {
static_assert(std::is_same<PlanningAdapter::DataType, T>::value,
"Data type must be the same with adapter's type!");
instance()->Planning_->FillHeader(module_name, data);
}
static void AddPlanningCallback(PlanningAdapter::Callback callback) {
CHECK(instance()->Planning_)
<< "Initialize adapter before setting callback";
instance()->Planning_->AddCallback(callback);
}
template <class T>
static void AddPlanningCallback(
void (T::*fp)(const PlanningAdapter::Dssage_history_limit()));
if (config.mode() != AdapterConfig::PUBLISH_ONLY && IsRos()) {
Planningsubscriber_ =
node_handle_->subscribe(topic_name, config.message_history_limit(),
&PlanningAdapter::OnReceive, Planning_.get());
}
if (config.mode() != AdapterConfig::RECEIVE_ONLY && IsRos()) {
Planningpublisher_ = node_handle_->advertise<PlanningAdapter::DataType>(
topic_name, config.message_history_limit(), config.latch());
}
observers_.push_back([this]() { Planning_->Observe(); });
Planningconfig_ = config;
}
PlanningAdapter *InternalGetPlanning() { return Planning_.get(); }
void InternalPublishPlanning(const PlanningAdapter::DataType &data) {
/* Only publish ROS msg if node handle is initialized. */
if (IsRos()) {
if (!Planningpublisher_.getTopic().empty()) {
Planningpublisher_.publish(data);
} else {
AERROR << "Planning" << " is not valid.";
}
} else {
/* For non-ROS mode, just triggers the callback. */
if (Planning_) {
Planning_->OnReceive(data);
} else {
AERROR << "Planning" << " is null.";
}
}
Planning_->SetLatestPublished(data);
}ataType &data), T *obj) {
AddPlanningCallback(std::bind(fp, obj, std::placeholders::_1));
}
template <class T>
static void AddPlanningCallback(
void (T::*fp)(const PlanningAdapter::DataType &data)) {
AddPlanningCallback(fp);
}
/* Returns false if there's no callback to pop out, true otherwise. */
static bool PopPlanningCallback() {
return instance()->Planning_->PopCallback();
}
private:
std::unique_ptr<PlanningAdapter> Planning_;
ros::Publisher Planningpublisher_;
ros::Subscriber Planningsubscriber_;
AdapterConfig Planningconfig_;
void InternalEnablePlanning(const std::string &topic_name,
const AdapterConfig &config) {
Planning_.reset(
new PlanningAdapter("Planning", topic_name, config.mePublishssage_history_limit()));
if (config.mode() != AdapterConfig::PUBLISH_ONLY && IsRos()) {
Planningsubscriber_ =
node_handle_->subscribe(topic_name, config.message_history_limit(),
&PlanningAdapter::OnReceive, Planning_.get());
}
if (config.mode() != AdapterConfig::RECEIVE_ONLY && IsRos()) {
Planningpublisher_ = node_handle_->advertise<PlanningAdapter::DataType>(
topic_name, config.message_history_limit(), config.latch());
}
observers_.push_back([this]() { Planning_->Observe(); });
Planningconfig_ = config;
}
PlanningAdapter *InternalGetPlanning() { return Planning_.get(); }
void InternalPublishPlanning(const PlanningAdapter::DataType &data) {
/* Only publish ROS msg if node handle is initialized. */
if (IsRos()) {
if (!Planningpublisher_.getTopic().empty()) {
Planningpublisher_.publish(data);
} else {
AERROR << "Planning" << " is not valid.";
}
} else {
/* For non-ROS mode, just triggers the callback. */
if (Planning_) {
Planning_->OnReceive(data);
} else {
AERROR << "Planning" << " is null.";
}
}
Planning_->SetLatestPublished(data);
}
现在重新回到ApolloApp::Spin()函数的分析。初始化完成后继续调用虚函数：Start()（实际调用apollo::planning::Planning::Start()）开始运行；之后使用spinner来监听各种信号，并调用相关处理函数；最后，在收到ros::waitForShutdown()信号后，调用Stop()（实际调用apollo::planning::Planning::Stop()）完成资源清理退出。调试信息的输出贯穿于整个函数过程。
二、适配器管理者类（AdapterManager）分析
AdapterManager类不属于规划模块，但它是所有消息适配器的管理者，这里对其作一初步分析。顾名思义，AdapterManager是所有适配器的管理容器。什么是适配器，Apollo项目为什么要使用适配器？第一个问题很好理解，借用网上经常提到的一个例子，中国电压标准是220V，美国电压标准是110V，中国电器要在美国使用，必须借助一个变压器，这个变压器就是适配器。另外一个例子是，中国插座标准是扁孔，欧洲插座标准是圆孔，中国的电源插头到欧洲使用，必须借助一个插孔转接器，这个转接器也是适配器。以上只是对适配器的形象描述，C++中用到的适配器，自然是来自于鼎鼎有名的GOF名著《Design Patterns: Elements of Reusable Object-Oriented Software》中提到的Adapter设计模式。关于该模式的描述，网上有一堆文章，此处不再赘述。关于第二个问题，Apollo项目各模块内部的消息传递使用Google Protocol Buffer格式，各模块之间的底层消息传递目前使用ROS机制，今后可能会替换为百度自研消息机制。不管Apollo项目各模块之间的底层消息传递机制如何改变，我们必须确保各模块内部的消息格式不变，即使用Google Protocol Buffer格式，否则会因为底层消息传递机制的修改，导致各模块内部涉及消息处理的代码被迫修改，这既严重破坏各模块代码的独立性和正确性，也严重影响项目开发效率。这就是Apollo创建众多Adapter及其管理者AdapterManager的根本原因。
Apollo 2.0版本目前定义了以下适配器：
using ChassisAdapter = Adapter<::apollo::canbus::Chassis>;
using ChassisDetailAdapter = Adapter<::apollo::canbus::ChassisDetail>;
using ControlCommandAdapter = Adapter<control::ControlCommand>;
using GpsAdapter = Adapter<apollo::localization::Gps>;
using ImuAdapter = Adapter<localization::Imu>;
using RawImuAdapter = Adapter<apollo::drivers::gnss::Imu>;
using LocalizationAdapter = Adapter<apollo::localization::LocalizationEstimate>;
using MonitorAdapter = Adapter<apollo::common::monitor::MonitorMessage>;
using PadAdapter = Adapter<control::PadMessage>;
using PerceptionObstaclesAdapter = Adapter<perception::PerceptionObstacles>;
using PlanningAdapter = Adapter<planning::ADCTrajectory>;
using PointCloudAdapter = Adapter<::sensor_msgs::PointCloud2>;
using ImageShortAdapter = Adapter<::sensor_msgs::Image>;
using ImageLongAdapter = Adapter<::sensor_msgs::Image>;
using PredictionAdapter = Adapter<prediction::PredictionObstacles>;
using DriveEventAdapter = Adapter<DriveEvent>;
using TrafficLightDetectionAdapter = Adapter<perception::TrafficLightDetection>;
using RoutingRequestAdapter = Adapter<routing::RoutingRequest>;
using RoutingResponseAdapter = Adapter<routing::RoutingResponse>;
using RelativeOdometryAdapter =
Adapter<calibration::republish_msg::RelativeOdometry>;
using InsStatAdapter = Adapter<drivers::gnss::InsStat>;
using InsStatusAdapter = Adapter<gnss_status::InsStatus>;
using GnssStatusAdapter = Adapter<gnss_status::GnssStatus>;
using SystemStatusAdapter = Adapter<apollo::monitor::SystemStatus>;
using MobileyeAdapter = Adapter<drivers::Mobileye>;
using DelphiESRAdapter = Adapter<drivers::DelphiESR>;
using ContiRadarAdapter = Adapter<drivers::ContiRadar>;
using CompressedImageAdapter = Adapter<sensor_msgs::CompressedImage>;
using GnssRtkObsAdapter = Adapter<apollo::drivers::gnss::EpochObservation>;
using GnssRtkEphAdapter = Adapter<apollo::drivers::gnss::GnssEphemeris>;
using GnssBestPoseAdapter = Adapter<apollo::drivers::gnss::GnssBestPose>;
using LocalizationMsfGnssAdapter =
Adapter<apollo::localization::LocalizationEstimate>;
using LocalizationMsfLidarAdapter =
Adapter<apollo::localization::LocalizationEstimate>;
using LocalizationMsfSinsPvaAdapter =
Adapter<apollo::localization::IntegSinsPva>;
注意，AdapterManager类是使用宏DECLARE_SINGLETON(AdapterManager)定义的单实例类，获取该类对象请使用AdapterManager::instance()。
（一）数据成员
AdapterManager类的数据成员除了包含上述各个适配器对象及其关联的消息发布、订阅对象外，还有如下重要成员：
- ROS节点句柄（
std::unique_ptr）
内置的ROS节点句柄。 - 观测回调函数数组（
std::vector）
各个适配器提供的回调函数数组。 - 初始化标志（
bool initialized_）
记录该类是否完成初始化。
（二）重要成员函数
- Init函数
前文已描述，就是完成各类适配器的注册。 - Observe函数
调用各适配器提供的回调函数获取当前各模块的观测数据。 - CreateTimer函数
创建一个定时器，在定时器内定时调用传入的回调函数。例如在Planning模块Planning::Start函数中，创建一个定时器来周期性地执行规划任务。
Status Planning::Start() {
timer_ = AdapterManager::CreateTimer(
ros::Duration(1.0 / FLAGS_planning_loop_rate), &Planning::OnTimer, this);
ReferenceLineProvider::instance()->Start();
AINFO << "Planning started";
return Status::OK();
}
三、规划类（Planning）分析
Planning类是规划模块的主要类，它将GPS和IMU提供的信息作为输入，处理后生成规划信息（包括路径和速度信息），提供给控制模块使用。
（一）数据成员
该类数据成员包括：规划者工厂类对象（planner_factory_）、规划配置类对象（config_）、高精地图类对象指针（hdmap_）、帧类对象指针（frame_）、规划者类对象指针（planner_）、可发布轨迹类对象指针（last_publishable_trajectory_）、计时器对象（timer_）。注意frame_、planner_和last_publishable_trajectory_使用C++标准库的唯一性智能指针unique_ptr包装，其优点在于：一是不用人工管理内存；二是上述指针由Planning类对象专享，确保数据的唯一性。
- 规划者工厂类对象（
apollo::common::util::Factory）
该对象的职能是注册所有的规划者类（目前注册了两个：RTK重放规划者RTKReplayPlanner，用于轨迹回放，无实际用途；最大期望规划者EMPlanner，可实用），并根据配置信息生成合适的规划者类对象（当前配置文件使用EMPlanner）。 - 规划配置类对象（
PlanningConfig config_）
该对象的职能是读取配置文件信息，给规划类对象提供合适的参数。 - 高精地图类对象指针（
hdmap::HDMap* hdmap_）
该指针用于保存高精地图，给规划类对象提供导航地图及车辆定位点等信息。 - 帧类对象指针（
std::unique_ptr frame_）
该指针用于保存帧（Frame）对象，规划类对象所有的决策都在该对象上完成。 - 规划者类对象指针（
std::unique_ptr）planner_
该指针用于保存具体规划者对象。 - 可发布轨迹类对象指针（
std::unique_ptr）last_publishable_trajectory_
该指针用于记录规划路径的位置和速度信息。 - 计时器对象（
ros::Timer timer_）
该对象用于定期监控各类操作用户，调用相关处理函数。
（二）重要成员函数
该类主要包含三个重载函数：Init、Start、Stop（均为基类ApolloApp的虚函数）和几个功能函数：InitFrame、RunOnce、Plan，其他函数不太重要，如感兴趣可直接查看源代码。
- Init函数
该函数完成以下工作：获取高精地图；读取配置文件获取相关参数；使用适配器管理者（apollo::common::adapterAdapterManager，Apollo项目内的所有模块如定位、底盘、预测、控制、规划等全部归其管理，类似于一个家庭管家）对象获取规划模块所需的定位、底盘、路径、交通信号灯探测、周边障碍物预测等信息；获取参考线（基于导航路径和当前位置计算得到，提供给Planning类决策的候选轨迹线）；注册具体的规划者类（目前注册RTKReplayPlanner及EMPlanner）；使用工厂创建模式生成具体的规划者类对象（通过查询配置文件获知，实际生成了EMPlanner类对象）；最后完成规划者类对象的初始化。 - Start函数
Start函数很简单，主要完成两个工作：一是启动参考线信息的获取；二是创建一个定时器，按给定间隔调用Planning::OnTimer函数，Planning::OnTimer函数的功能更为简单，即调用Planning::RunOnce完成实际的规划工作。 - Stop函数
Stop函数的功能也很简单：一是结束参考线信息的获取；二是清空可发布轨迹类对象指针、帧类对象指针、规划者类对象指针。 - RunOnce函数
现在来观察RunOnce函数完成的具体工作。该函数首先调用AdapterManager类的相关函数来获取规划所需的所需的定位、底盘信息；接着基于上述信息，调用common::VehicleStateProvider::instance()->Update函数更新车辆自身的状态；之后调用Planning::InitFrame函数初始化规划帧；如果一切正常，就调用Planning::Plan函数执行实际的规划；最后调用Planning::PublishPlanningPb函数将计算出的规划路径发布给控制模块。另外，整个函数针对各种可能出现的错误均做出相应处理。 - InitFrame函数
该函数功能比较简单，首先调用Frame::reset函数重新创建一个Frame类对象（如果之前不存在，直接创建；若已经存在一个，则先删除后创建），然后设置预测信息，最后调用Frame::Init函数完成当前帧的初始化。 - Plan函数
Plan函数是规划模块最核心的函数，用来完成实际的路径规划。该函数针对Frame类对象指针frame_提供的多条候选参考线信息：reference_line_info，调用规划者对象的Plan函数（planner_->Plan，当前实现就是调用EMPlanner::Plan函数）决策得到可用参考线信息集（可能不止一条）；接着调用Frame::FindDriveReferenceLineInfo函数从可用参考线信息集获取最优的参考线信息：best_reference_line；最后调用ReferenceLineInfo::ExportDecision函数，将best_reference_line转化可供控制模块使用的驾驶决策，包括：路径、速度等。整个函数存在很多输出调试信息的语句，注意不要被他们所干扰，影响对核心思路的理解。
四、期望最大规划者类（EMPlanner）分析
EMPlanner类是具体的规划实施类，它基于高精地图、导航路径及障碍物信息作出实际的驾驶决策，包括路径、速度等方面。首先使用DP（动态规划）方法确定初始的路径和速度，再利用QP（二次规划）方法进一步优化路径和速度，以得到一条更平滑的轨迹，既满足舒适性，又方便车辆操纵。EMPlanner类的继承关系如下图所示：
（一）数据成员
该类数据成员包括：任务工厂类对象（task_factory_）、任务类对象数组（tasks_）。
- 任务工厂类对象（
apollo::common::util::Factory）
该对象的负责是注册所有的任务类，并根据配置信息生成合适的任务类对象。 - 任务类对象数组（
std::vector）
该数组的职能是保存各个任务类对象的智能指针。
（二）重要成员函数
该类主要包含两个重载函数：Init、Plan（基类Planner的虚函数）和两个功能函数：GenerateInitSpeedProfile、GenerateSpeedHotStart，其他函数是辅助函数，如感兴趣可直接查看源代码。
- Init函数
该函数完成以下工作：注册各种任务类（目前已注册：交通决策器TrafficDecider、动态规划多项式路径优化器DpPolyPathOptimizer、路径决策器PathDecider、动态规划ST坐标速度优化器DpStSpeedOptimizer、速度决策器SpeedDecider、二次规划样条路径优化器QpSplinePathOptimizer、二次规划样条ST坐标速度优化器QpSplineStSpeedOptimizer）；使用工厂创建模式生成具体的任务类对象（通过查询配置文件modules/planning/conf/planning_config.pb.txt获知，任务类型包括： TRAFFIC_DECIDER、 DP_POLY_PATH_OPTIMIZER、PATH_DECIDER、DP_ST_SPEED_OPTIMIZER、SPEED_DECIDER、QP_SPLINE_ST_SPEED_OPTIMIZER，实际生成的任务对象分别为：TrafficDecider、DpPolyPathOptimizer、PathDecider 、DpStSpeedOptimizer、SpeedDecider、QpSplineStSpeedOptimizer类对象。注意：任务对象的创建可根据修改配置文件动态调整）；最后完成各种任务类对象的初始化。决策和优化器的继承关系图如下所示：
- Plan函数
该函数首先对当前参考线信息（reference_line_info，即候选路径）作出判断，若其非向前直行，则加入10000的代价（cost）；接下来，基于起始点和当前参考线信息，调用EMPlanner::GenerateInitSpeedProfile函数生成初始速度，若失败则继续调用EMPlanner::GenerateSpeedHotStart生成初始速度；之后是整个函数的核心，针对当前帧frame和参考线信息reference_line_info，逐一调用任务类对象数组tasks_里的优化器（当前配置文件按照以下顺序使用以下决策或优化器：TrafficDecider、DpPolyPathOptimizer、PathDecider 、DpStSpeedOptimizer、SpeedDecider、QpSplineStSpeedOptimizer）进行最优估计，计算当前参考线的最优路径和速度。若针对当前参考线无法计算出最优路径和速度，则将其代价值设为无穷大，表示当前参考线无法使用；若对当前参考线可计算出最优路径和速度，则调用ReferenceLineInfo::CombinePathAndSpeedProfile函数合并路径和速度信息，最后调用ReferenceLineInfo::setTrajectory函数更新当前参考线的轨迹，返回给上一级Planning::Plan函数使用。整个函数存在很多输出调试信息的语句，注意不要被他们所干扰，影响对核心思路的理解。 - GenerateInitSpeedProfile函数
该函数看上去很复杂，实际思想比较简单：首先获取上一帧last_frame的参考线信息last_reference_line_info；之后，对当前参考线信息reference_line_info调用ReferenceLineInfo::IsStartFrom函数来判断其是否起始于上一帧参考线信息last_reference_line_info，若不是直接返回，否则继续；接着，基于上一帧参考线的速度信息生成当前帧参考线的速度信息，注意这里用到了(x, y)坐标到(s, l)坐标的转换。 - GenerateSpeedHotStart函数
该函数很简单，就是将初始点的速度限制在[5.0，FLAGS_planning_upper_speed_limit]范围内，并为当前帧参考线的初始速度信息。
五、帧类（Frame）分析
Frame类用于存储规划模块用到的各类信息，包括起始点、车辆状态、参考线信息列表（即多条候选路径）、驾驶参考线信息（即实际执行的驾驶路径）、障碍物预测等。所有决策都基于当前帧进行。
（一）数据成员
- 帧序号（
uint32_t sequence_num_）
记录当前帧的序号。 - 高精地图类对象指针（
hdmap::HDMap* hdmap_）
保存高精地图，给规划类对象提供导航地图及车辆定位点等信息。 - 轨迹起始点（
common::TrajectoryPoint planning_start_point_）
保存路径的起始点。 - 车辆状态类对象（
common::VehicleState vehicle_state_）
存储车辆状态信息，包括位置、时间戳、姿态、速度、加速度、底盘及方位等。 - 参考线信息列表（
std::list）reference_line_info_
存储多条候选路径。 - 驾驶参考线信息指针（
cconst ReferenceLineInfo *drive_reference_line_info_）
记录实际执行驾驶路径。 - 预测障碍物类对象（
prediction::PredictionObstacles prediction_）
存储车辆当前驾驶模式下，预测出现的障碍物。 - 障碍物列表（
prediction::PredictionObstacles prediction_）
存储车辆周边所有的障碍物。
（二）重要成员函数
该类主要包含如下成员函数：Init、FindDriveReferenceLineInfo，其他函数是辅助函数，如感兴趣可直接查看源代码。
- Init函数
该函数被Planning::InitFrame函数调用，用于完成当前帧各类信息的初始化，包括获取高精地图、车辆状态，创建预测障碍、目的地障碍，查找碰撞障碍，初始化参考线信息等。 - FindDriveReferenceLineInfo函数
该函数从可用的参考线信息列表中找到代价（cost）值最低的一条参考线信息，作为最终的驾驶路径。
六、参考线提供者类（ReferenceLineProvider）分析
ReferenceLineProvider类是规划模块中参考线（即候选路径）的提供和管理者，它以Routing模块输出的高精度路径（从起点到终点的Lane片段集）为输入数据，经优化处理后生成平滑的参考线集给后续的动作规划子模块使用。ReferenceLineProvider类是采用C++ 11标准实现的多线程类，每次调用ReferenceLineProvider::Start()函数后，该类就会在内部开启一个新线程，执行参考线的优化。ReferenceLineProvider类是使用宏DECLARE_SINGLETON(ReferenceLineProvider)定义的单实例类，获取该类对象请使用ReferenceLineProvider::instance()。
（一）数据成员
该类数据成员包括：是否初始化（is_initialized_）、是否停止（is_stop_）、线程类对象智能指针（thread_）、参考线平滑类对象智能指针（smoother_）、二维样条求解类对象智能指针（spline_solver_）、二次规划样条参考线平滑配置类对象指针（smoother_config_）、规划控制地图互斥体类对象（pnc_map_mutex_）、规划控制地图类对象智能指针（pnc_map_）、车辆状态互斥体类对象（vehicle_state_mutex_）、车辆状态类对象（vehicle_state_）、路由寻径互斥体类对象（routing_mutex_）、路由寻径响应类对象（routing_）、是否具备路由（has_routing_）、历史片段互斥体类对象（segment_history_mutex_）、历史片段无序映射表类对象（segment_history_）、历史片段ID列表类对象（segment_history_id_）、参考线互斥体类对象（reference_lines_mutex_）、参考线列表类对象（reference_lines_）、路由片段列表类对象（route_segments_）、上次计算时间（last_calculation_time_）。注意该类的指针使用C++标准库的唯一性智能指针unique_ptr包装，其优点在于：一是不用人工管理内存；二是上述指针由ReferenceLineProvider类对象专享，确保数据的唯一性。
- 是否初始化（
bool is_initialized_）
初始化是否完成标志。 - 是否停止（
bool is_stop_）
记录当前线程是否停止。 - 线程类对象智能指针（
std::unique_ptr_）
后台线程的管理者。 - 参考线平滑类对象智能指针（
std::unique_ptr）smoother_
执行参考线平滑任务的类对象。 - 二维样条求解类对象智能指针（
std::unique_ptr）spline_solver_
执行二维样条求解任务的类对象。 - 二次规划样条参考线平滑配置类对象指针（
QpSplineReferenceLineSmootherConfig smoother_config_）
读取二次规划样条参考线平滑配置的类对象。 - 规划控制地图互斥体类对象（
std::mutex pnc_map_mutex_）
允许规划控制地图在多线程环境下安全使用的互斥体类对象。 - 规划控制地图类对象智能指针（
std::unique_ptr）
规划控制模块使用的地图数据类对象。 - 车辆状态互斥体类对象（
std::mutex vehicle_state_mutex_）
允许车辆状态在多线程环境下安全使用的互斥体类对象。 - 车辆状态类对象（
common::VehicleState vehicle_state_）
当前的车辆状态。 - 路由寻径互斥体类对象（
std::mutex routing_mutex_）
允许路由寻径响应在多线程环境下安全使用的互斥体类对象。 - 路由寻径响应类对象（
routing::RoutingResponse routing_）
Routing模块输出的路由寻径数据。 - 是否具备路由（
bool has_routing_）
从起点到终点是否具备路由。 - 历史片段互斥体类对象（
std::mutex segment_history_mutex_）
允许历史片段在多线程环境下安全使用的互斥体类对象。 - 历史片段无序映射表类对象（
std::unordered_map）
存储多条历史路径片段。 - 历史片段ID列表类对象（
std::list）
用于检索历史路径片段的ID。 - 参考线互斥体类对象（std::mutex reference_lines_mutex_）
允许参考线在多线程环境下安全使用的互斥体类对象。 - 参考线列表类对象（
std::list）reference_lines_
存储多条候选路径。 - 路由片段列表类对象（
std::list）
存储多条路由片段。 - 上次计算时间（
double last_calculation_time_）
上次计算平滑参考线耗时。
（二）重要成员函数
该类的重要成员函数包括：Init、Start、Stop、GenerateThread、CreateReferenceLine、UpdateReferenceLine，其他函数都是辅助性的功能函数，如感兴趣可直接查看源代码。
- Init函数
创建hdmap::PncMap类对象，清空segment_history_，若启用螺旋参考线（FLAGS_enable_spiral_reference_line），则将参考线平滑类对象（smoother_）创建为SpiralReferenceLineSmoother类对象，否则创建为QpSplineReferenceLineSmoother类对象。将初始化标志is_initialized_置为true。 - Start函数
如果初始化标志is_initialized_置为false，直接返回false。若启用参考线提供者线程（FLAGS_enable_reference_line_provider_thread），则以ReferenceLineProvider:: GenerateThread为入口函数创建一个新线程并返回true，否则直接返回true，不做任何实际工作。 - Stop函数
将线程停止标志is_stop_置为true，判断当前线程是否完成，若未完，则调用thread_->join()强制完成线程任务。 - GenerateThread函数
这是ReferenceLineProvider类线程的入口函数。如果停止标志（is_stop_）置为true，首先调用std::this_thread::yield和std::this_thread::sleep_for函数，让当前线程暂停，并休眠50ms，将调度机会留给其他线程，当操作系统空闲时执行后续代码。我认为这种设计是有问题的。如果操作系统不空闲，则后续代码可能永远不会执行。我在Win10系统上写了一个小测试程序，分别使用VC++ 2017和MinGW-w64编译器构建，VC编译器生成的程序每次都会调用后续代码，MinGW编译器生成的程序有时会调用，有时不会调用，完全依据当时操作系统的空闲状态而定。若当前没有从起点到终点的路由，则进入下一次循环。一切就绪后，先调用CreateReferenceLine函数创建参考线列表，再调用UpdateReferenceLine函数更新参考线，最后计算此次参考线生成所消耗的时间。
void ReferenceLineProvider::GenerateThread() {
constexpr int32_t kSleepTime = 50; // milliseconds
while (!is_stop_) {
std::this_thread::yield();
std::this_thread::sleep_for(
std::chrono::duration<double, std::milli>(kSleepTime));
double start_time = Clock::NowInSeconds();
if (!has_routing_) {
AERROR << "Routing is not ready.";
continue;
}
std::list<ReferenceLine> reference_lines;
std::list<hdmap::RouteSegments> segments;
if (!CreateReferenceLine(&reference_lines, &segments)) {
AERROR << "Fail to get reference line";
continue;
}
UpdateReferenceLine(reference_lines, segments);
double end_time = Clock::NowInSeconds();
std::lock_guard<std::mutex> lock(reference_lines_mutex_);
last_calculation_time_ = end_time - start_time;
}
}
- CreateReferenceLine函数
首先使用互斥体锁定，分别获取当前车辆状态和路由，接着调用pnc_map_->IsNewRouting(routing)判断当前路由是否为新路由，若是则调用pnc_map_->UpdateRoutingResponse(routing)更新路由。之后，调用CreateRouteSegments函数来创建路由片段。若不需粘合参考线（!FLAGS_enable_reference_line_stitching），则调用SmoothRouteSegment函数来平滑各路由片段列表segments，并将平滑后的路由片段存储到参考线列表reference_lines中，同时将不能平滑的路由片段从segments中删除；若需粘合参考线（FLAGS_enable_reference_line_stitching），则调用ExtendReferenceLine函数来合并不同参考线片段的重合部分，并将粘合后的路由片段保存到参考线列表reference_lines中，同时将不能粘合的路由片段从列表segments中删除。 - UpdateReferenceLine函数
判断新参考线各线段是否与原参考线对应线段相同，若相同则忽略，否则更新。
七、参考线信息类（ReferenceLineInfo）分析
ReferenceLineInfo类用于存储参考线信息。所谓参考线实际就是一条驾驶路径，因此参考线信息实际上就是一条驾驶路径所需的各种信息，包括车辆状态、当前轨迹点、路径数据、速度数据等。
（一）数据成员
该类的数据成员较多，包括：车辆状态类对象（vehicle_state_）、规划点类对象（adc_planning_point_）、参考线类对象（reference_line_）、代价（cost_）、能否驾驶（is_drivable_）、路径决策类对象（path_decision_）、路径数据类对象（path_data_）、速度数据类对象（speed_data_）、离散化轨迹类对象（discretized_trajectory_）、SL边界类对象（adc_sl_boundary_）、隐藏状态类对象（latency_stats_）、车道路径片段类对象（lanes_）。
- 车辆状态类对象（const common::VehicleState vehicle_state_）
从其他地方获取的车辆状态，不可修改。 - 规划轨迹点类对象（const common::TrajectoryPoint adc_planning_point_）
从其他地方获取的规划点，不可修改。 - 参考线类对象（const ReferenceLine reference_line_）
从其他地方获取的参考线，不可修改。 - 代价（double cost_）
参考线代价，越小越好。 - 能否驾驶（bool is_drivable_）
表示参考线能否驾驶。 - 路径决策类对象（PathDecision path_decision_）
判断当前参考线是否有障碍。 - 路径数据类对象（PathData path_data_）
存储参考线包含的路径数据。 - 速度数据类对象（SpeedData speed_data_）
存储参考线包含的速度信息。 - 离散化轨迹类对象（DiscretizedTrajectory discretized_trajectory_）
存储参考线的离散化轨迹点集。 - SL边界类对象（SLBoundary adc_sl_boundary_）
存储参考线的边界信息。 - 隐藏状态类对象（LatencyStats latency_stats_）
存储参考线包含的隐藏状态，包括总时长（ms）、控制器时长（ms）。 - 车道路径片段类对象（hdmap::RouteSegments lanes_）
存储参考线的车道片段集。
（二）重要成员函数
该类主要包含以下重要成员函数：AddObstacles、IsStartFrom、IsChangeLanePath、CombinePathAndSpeedProfile、ExportTurnSignal、ExportDecision、ReachedDestination，其他函数是辅助函数，如感兴趣可直接查看源代码。
- AddObstacles函数
顾名思义，该函数就是在当前参考线中加入障碍，让参考线更为接近实际的驾驶路径。 - IsStartFrom函数
用于判断当前参考线是否起始于上一条参考线。 - IsChangeLanePath函数
用于判断当前参考线是否是一条改变车道的参考线，也就是说，车道当前位置不在此参考线上。。 - CombinePathAndSpeedProfile函数
将当前参考线的路径数据和速度信息综合考虑，形成实际可供驾驶的路径。 - ExportTurnSignal函数
输出调头信号给其他函数调用。 - ExportDecision函数
输出路径决策给其他函数调用。 - ReachedDestination函数
用于判断当前参考线是否抵达终点。
八、交通决策器类（TrafficDecider）分析
TrafficDecider类继承自Task类，其作用是完成与交通相关的决策。Apollo 2.0版本中的道路交通包括：后方车辆、信号灯、人行横道、安全区、重新路由寻径、参考线终点、目的地、停止信号、改变车道等。Task类提供两个虚函数Init、Execute，其中Init函数用于任务对象的初始化，Task类目前未做任何实际工作；Execute用于执行实际的优化，Task类也未实现任何功能。
（一）数据成员
该类包含两个数据成员：规则工厂类对象（rule_factory_），规则配置类对象（rule_configs_）。
- 规则工厂类对象（
apollo::common::util::Factory）
用于注册和生成相关交通决策规则对象。 - 规则配置类对象（google::protobuf::RepeatedPtrField rule_configs_）
从配置文件中需生成的规则类型，包括：后方车辆（BACKSIDE_VEHICLE）、信号灯（SIGNAL_LIGHT）、人行横道（CROSSWALK）、禁停区（CLEAR_ZONE）、重新路由寻径（REROUTING）、参考线终点（REFERENCE_LINE_END）、目的地（DESTINATION）、停止信号（STOP_SIGN）、改变车道（CHANGE_LANE）等。
（二）重要成员函数
该类重载了Task类的Init、Execute函数。
- Init函数
首先注册多个交通规则，目前已注册BacksideVehicle、SignalLight、Crosswalk、Rerouting、ReferenceLineEnd、Destination、StopSign、ChangeLane等；然后从配置文件获取当前应用的规则，Apollo 2.0版配置的规则包括：BACKSIDE_VEHICLE、SIGNAL_LIGHT、REFERENCE_LINE_END、DESTINATION、CHANGE_LANE。 - Execute函数
首先判断是否启用信号灯决策（FLAGS_enable_traffic_light），若未启动，则跳过该决策。之后调用rule_factory_.CreateObject函数动态创建配置文件中指定的规则对象，Apollo 2.0版创建了如下规则对象：BacksideVehicle、SignalLight、ReferenceLineEnd、Destination、ChangeLane。最后，对上述规则对象依次调用rule->ApplyRule(frame, reference_line_info)来获得交通决策下的参考线信息：reference_line_info。
九、路径决策器类（PathDecider）分析
PathDecider类继承自Task类，其作用是完成与路径相关的决策，Apollo 2.0版本中，该决策器根据当前路线中静态障碍物与车辆的位置，作出忽略(Ignore)、停车(Stop)、左绕行(Left nudge)和右绕行(Right nudge)等决策。Task类提供两个虚函数Init、Execute，其中Init函数用于任务对象的初始化，Task类目前未做任何实际工作；Execute用于执行实际的优化，Task类也未实现任何功能。
（一）数据成员
该类没有数据成员。
（二）重要成员函数
该类重载了Task类的Execute函数，并实现Process、MakeObjectDecision、MakeStaticObstacleDecision几个辅助的功能函数。
- Execute函数
调用Process(reference_line_info->path_data(), reference_line_info->path_decision())来获得路径决策下的参考线信息：reference_line_info。 - Process函数
调用MakeObjectDecision(path_data, path_decision)来获得路径决策：decision。 - MakeObjectDecision函数
调用MakeStaticObstacleDecision(path_data, path_decision)来获得路径决策：decision。 - MakeStaticObstacleDecision函数
根据路径中静态障碍的位置对车辆采取忽略、停车、左绕行和右绕行等决策，并将其保存到：decision。
十、速度决策器类（SpeedDecider）分析
SpeedDecider类继承自Task类，其作用是完成与速度相关的决策。Apollo 2.0版本中，该决策器根据当前路线中障碍物与车辆的位置，作出跟随(Follow)、避让(Yield)、超车(Overtake)、停车(Stop)等决策。Task类提供两个虚函数Init、Execute，其中Init函数用于任务对象的初始化，Task类目前未做任何实际工作；Execute用于执行实际的优化，Task类也未实现任何功能。
（一）数据成员
该类包含以下数据成员：
- 动态规划ST坐标速度配置类对象（DpStSpeedConfig dp_st_speed_config_）
存储从配置文件读取的动态规划ST坐标速度配置信息。 - ST坐标边界配置配置类对象（StBoundaryConfig st_boundary_config_）
存储从配置文件读取的ST坐标边界条件。 - SL坐标边界条件类对象（SLBoundary adc_sl_boundary_）
保存SL坐标下的边界条件。 - 初始轨迹点类对象（apollo::common::TrajectoryPoint init_point_）
记录初始的轨迹点。 - 参考线对象指针（const ReferenceLine* reference_line_）
记录当前的参考线。
（二）重要成员函数
该类重载了Task类的Init、Execute函数，并提供重要的功能函数：MakeObjectDecision。
- Init函数
从配置文件modules\planning\conf\planning_config.pb.txt中读取相关配置信息，并保存到dp_st_speed_config_和st_boundary_config_中。 - Execute函数
调用MakeObjectDecision来获得速度决策下的参考线信息：reference_line_info。 - MakeObjectDecision函数
对当前路径中的障碍逐一判断，分别作出忽略、跟随、避让、超车、停车等决策，并保存到：decision。
十一、动态规划多项式路径优化器类（DpPolyPathOptimizer）分析
DpPolyPathOptimizer类继承自PathOptimizer，后者又继承自Task，其作用是对当前路径进行动态规划多项式优化处理。Task类提供两个虚函数Init、Execute，其中Init函数用于任务对象的初始化，Task类目前未做任何实际工作；Execute用于执行实际的优化，Task类也未实现任何功能。PathOptimizer类重载了Task类的虚函数Execute，同时提供一个保护的纯虚函数Process（由派生类具体实现），在Execute内部调用Process函数完成实际的优化。注意：PathOptimizer是虚基类，不可直接使用它创建类对象，必须从该类派生出具体类后方可实例化。
（一）数据成员
该类包含一个数据成员：动态规划多项式路径配置类对象（config_）。
6. 动态规划多项式路径配置类对象（DpPolyPathConfig config_）
从配置文件中读取相关信息，包括：每步采样点数（sample_points_num_each_level）、步长最大值（step_length_max）、步长最小值（step_length_min）、横向采样偏移（lateral_sample_offset）、横向调整系数（lateral_adjust_coeff）、评估时间间隔（eval_time_interval）、路径分辨率（path_resolution）、障碍忽略距离（obstacle_ignore_distance）、障碍碰撞距离（obstacle_collision_distance）、障碍危险距离（obstacle_risk_distance）、障碍碰撞代价（obstacle_collision_cost）等。
（二）重要成员函数
该类重载了Task类的Init函数，PathOptimizer类的Process函数。
4. Init函数
该函数很简单，就是从配置文件中读取相关信息，获取算法所需的各项参数初始值。
5. Process函数
该函数创建一个DPRoadGraph类对象dp_road_graph，然后调用DPRoadGraph::FindPathTunnel函数寻找最优路径。
十二、动态规划ST坐标速度优化器类（DpStSpeedOptimizer）分析
DpStSpeedOptimizer类继承自SpeedOptimizer，后者又继承自Task，其作用是对当前速度进行动态规划ST坐标优化处理。Task类提供两个虚函数Init、Execute，其中Init函数用于任务对象的初始化，Task类目前未做任何实际工作；Execute用于执行实际的优化，Task类也未实现任何功能。SpeedOptimizer类重载了Task类的虚函数Execute，同时提供一个保护的纯虚函数Process（由派生类具体实现），在Execute内部调用Process函数完成实际的优化，如果优化失败，则调用GenerateStopProfile函数生成停车速度信息。注意：SpeedOptimizer是虚基类，不可直接使用它创建类对象，必须从该类派生出具体类后方可实例化。
（一）数据成员
该类数据成员包括：动态规划ST坐标速度配置类对象（dp_st_speed_config_）、ST坐标边界配置类对象（dp_st_speed_config_）。
- 动态规划ST坐标速度配置类对象（DpStSpeedConfig dp_st_speed_config_）
从配置文件中读取相关信息，包括：路径总长（total_path_length）、总时长（total_time）、距离向矩阵维数（matrix_dimension_s）、时间向矩阵维数（matrix_dimension_t）、速度权重（speed_weight）、加速度权重（accel_weight）、加加速度（即加速度导数）权重（jerk_weight）、障碍权重（obstacle_weight）、参考权重（reference_weight）、向下权重（go_down_buffer）、向上权重（go_up_buffer）、缺省障碍代价（default_obstacle_cost）、障碍代价因子（obstacle_cost_factor）、缺省速度代价（default_speed_cost）、超速惩罚（exceed_speed_penalty）、低速惩罚（low_speed_penalty）、加速度惩罚（accel_penalty）、减速度惩罚（decel_penalty）、正加加速度系数（positive_jerk_coeff）、负加加速度系数（negative_jerk_coeff）、最大加速（max_acceleration）、最大减速（max_deceleration）、ST坐标边界设置（st_boundary_config）、禁停代价因子（keep_clear_cost_factor）等。 - ST坐标边界配置类对象（StBoundaryConfig st_boundary_config_）
从配置文件中读取相关信息，包括：高速向心加速度限制（high_speed_centric_acceleration_limit）、低速向心加速度限制（low_speed_centric_acceleration_limit）、高速阈值（high_speed_threshold）、低速阈值（low_speed_threshold）、最小曲率（minimal_kappa）、点扩展（point_extension）、最低速度（lowest_speed）、平均曲率点数（num_points_to_avg_kappa）等。
（二）重要成员函数
该类重载了Task类的Init函数，PathOptimizer类的Process函数。
- Init函数
该函数很简单，就是从配置文件中读取相关信息，获取算法所需的各项参数初始值。 - Process函数
该函数首先获取路径的边界，然后创建一个DpStGraph类对象st_graph，然后调用DpStGraph::Search函数执行图搜索获取最优速度信息。
十三、二次规划样条路径优化器类（QpSplinePathOptimizer）分析
QpSplinePathOptimizer类继承自PathOptimizer，后者又继承自Task，其作用是对当前路径进行二次规划样条优化处理。Task类提供两个虚函数Init、Execute，其中Init函数用于任务对象的初始化，Task类目前未做任何实际工作；Execute用于执行实际的优化，Task类也未实现任何功能。PathOptimizer类重载了Task类的虚函数Execute，同时提供一个保护的纯虚函数Process（由派生类具体实现），在Execute内部调用Process函数完成实际的优化。注意：PathOptimizer是虚基类，不可直接使用它创建类对象，必须从该类派生出具体类后方可实例化。
（一）数据成员
该类数据成员包括：二次规划样条路径配置类对象（qp_spline_path_config_）、样条生成器类对象指针（spline_generator_）。
- 二次规划样条路径配置类对象（QpSplinePathConfig qp_spline_path_config_）
从配置文件中读取相关信息，包括：样条阶数（spline_order）、最大样条长度（max_spline_length）、时间分辨率（max_spline_length）、正则化权重（regularization_weight）、一次样条权重因子（first_spline_weight_factor）、一阶导数权重（derivative_weight）、二阶导数权重（second_derivative_weight）、三阶导数权重（third_derivative_weight）、参考线权重（reference_line_weight）、输出个数（num_output）、十字路口横向延拓（cross_lane_lateral_extension）、十字路口纵向延拓（cross_lane_longitudinal_extension）、历史路径权重（history_path_weight）、变道中间l值？（lane_change_mid_l）、点限制s位置？（point_constraint_s_position）、变道横向偏移（lane_change_lateral_shift）、调头速度限制（uturn_speed_limit）等。 - 样条生成器类对象指针（
std::unique_ptr）spline_generator_
用于生成二次规划所需的样条曲线。
（二）重要成员函数
该类重载了Task类的Init函数，PathOptimizer类的Process函数。
- Init函数
该函数很简单，就是从配置文件中读取相关信息，获取算法所需的各项参数初始值，同时创建Spline1dGenerator样条生成器对象。 - Process函数
该函数创建一个QpSplinePathGenerator类对象path_generator，然后调用QpSplinePathGenerator::Generate函数寻找最优路径。
十四、二次规划样条ST坐标速度优化器类（QpSplineStSpeedOptimizer）分析
QpSplineStSpeedOptimizer类继承自SpeedOptimizer，后者又继承自Task，其作用是对当前速度进行二次规划样条ST坐标优化处理。Task类提供两个虚函数Init、Execute，其中Init函数用于任务对象的初始化，Task类目前未做任何实际工作；Execute用于执行实际的优化，Task类也未实现任何功能。SpeedOptimizer类重载了Task类的虚函数Execute，同时提供一个保护的纯虚函数Process（由派生类具体实现），在Execute内部调用Process函数完成实际的优化，如果优化失败，则调用GenerateStopProfile函数生成停车速度信息。注意：SpeedOptimizer是虚基类，不可直接使用它创建类对象，必须从该类派生出具体类后方可实例化。
（一）数据成员
该类数据成员包括：二次规划样条ST坐标速度配置类对象（qp_st_speed_config_）、ST坐标边界配置类对象（st_boundary_config_）、样条生成器类对象指针（spline_generator_）。
- 二次规划样条ST坐标速度配置类对象（QpStSpeedConfig qp_st_speed_config_）
从配置文件中读取相关信息，包括：路径总长（total_path_length ）、总时长（total_time）、参考最大加速度（referred_max_acceleration）、参考最小减速度（preferred_min_deceleration）、最大加速度（max_acceleration）、最小减速度（min_deceleration） 、二次规划样条配置（qp_spline_config）、二次规划分段配置（qp_piecewise_config）、ST坐标边界配置（st_boundary_config）等。 - ST坐标边界配置类对象（StBoundaryConfig st_boundary_config_）
从配置文件中读取相关信息，包括：边界缓冲（boundary_buffer）、高速向心加速度限制（high_speed_centric_acceleration_limit）、低速向心加速度限制（low_speed_centric_acceleration_limit）、高速阈值（high_speed_threshold）、低速阈值（low_speed_threshold）、最小曲率（minimal_kappa）、点扩展（point_extension）、最低速度（lowest_speed）、平均曲率点数（num_points_to_avg_kappa）等。 - 样条生成器类对象指针（std::unique_ptr spline_generator_）
用于生成二次规划所需的样条曲线。
（二）重要成员函数
该类重载了Task类的Init函数，PathOptimizer类的Process函数。
- Init函数
该函数很简单，就是从配置文件中读取相关信息，获取算法所需的各项参数初始值，同时创建Spline1dGenerator样条生成器对象。 - Process函数
该函数首先获取路径的边界，然后创建一个QpSplineStGraph类对象st_graph，然后调用DpStGraph::Search函数执行图搜索获取最优速度信息。