Apollo9.0 Planning2.0决策规划算法代码详细解析 (5): OnLanePlanning::Init()

🌟 面向自动驾驶规划算法工程师的专属指南 🌟

欢迎来到《Apollo9.0 Planning2.0决策规划算法代码详细解析》专栏！本专栏专为自动驾驶规划算法工程师量身打造，旨在通过深入剖析Apollo9.0开源自动驾驶软件栈中的Planning2.0模块，帮助读者掌握自动驾驶决策规划算法的核心原理与实现细节。

🔍 VSCode+GDB：精准调试，洞悉代码逻辑 🔍

在自动驾驶算法的开发过程中，调试是至关重要的一环。本专栏将带你深入掌握VSCode+GDB这一强大的调试工具组合，让你能够逐行分析代码，精准定位问题，从而洞悉算法背后的逻辑与原理。通过实战演练，你将学会如何高效地利用调试工具，提升代码质量与开发效率。

💻 C++语法同步讲解：构建算法基石 💻

C++作为自动驾驶领域的主流编程语言，其重要性不言而喻。本专栏在解析算法代码的同时，将同步介绍C++语法，从基础到进阶，涵盖数据类型、控制结构、面向对象编程等核心知识点。通过系统学习，你将能够熟练运用C++语言编写高效、可维护的自动驾驶规划算法代码。

🚀 掌握自动驾驶PNC工程师从业能力 🚀

完成本专栏的学习后，你将具备自动驾驶PNC（规划、导航与控制）工程师的从业能力。你将能够深入理解自动驾驶决策规划算法的设计思路与实现方法，掌握Apollo9.0 Planning2.0模块的核心技术，为自动驾驶汽车的智能决策提供有力支持。

🚀 Apollo9.0 Planning2.0：探索自动驾驶技术前沿 🚀

Apollo9.0作为百度开源的自动驾驶软件栈，其Planning2.0模块在决策规划算法方面取得了显著进展。本专栏将带你深入探索Apollo9.0 Planning2.0的奥秘，揭秘其背后的算法原理与实现细节。通过系统学习，你将能够站在自动驾驶技术的前沿，为自动驾驶汽车的未来发展贡献力量。

🎉 立即加入，开启自动驾驶规划算法之旅 🎉

无论你是自动驾驶领域的初学者，还是有一定经验的工程师，本专栏都将为你提供宝贵的学习资源与实战机会。立即加入《Apollo9.0 Planning2.0决策规划算法代码详细解析》专栏，与我们一起探索自动驾驶技术的无限可能，共同推动自动驾驶技术的未来发展！

一、OnLanePlanning模块介绍

Apollo的OnLanePlanning是自动驾驶框架中的一个核心模块，它主要负责在有车道线的道路上进行路径规划。以下是对OnLanePlanning的详细介绍：

1、模块位置与继承关系

OnLanePlanning的源代码位于modules\planning\on_lane_planning.h。
它继承自PlanningBase类，与NaviPlanning类并列，是Apollo规划模块中的两种主要规划方式之一。

2、初始化与运行

OnLanePlanning的初始化逻辑主要在Init函数中实现。该函数负责分配具体的Planner（规划器），并启动参考线提供器（ReferenceLineProvider）。
在初始化过程中，会根据配置文件选择默认的规划器，通常为PublicRoadPlanner。

3、核心功能

OnLanePlanning的核心功能是在有车道线的道路上进行路径规划。它基于高精地图和车辆当前状态，生成一条安全且舒适的行驶路径。
该模块会考虑实际临时或移动障碍物，以及速度、动力学约束等因素，尽量按照规划路径进行轨迹规划。
OnLanePlanning还包含决策功能，能够根据不同的场景（如红绿灯路口、停车场景等）进行决策，并生成相应的规划轨迹。

4、与其他模块的交互

OnLanePlanning的上游模块包括Localization（定位模块）、Prediction（预测模块）和Routing（路由模块）。这些模块为OnLanePlanning提供必要的输入信息，如车辆当前位置、障碍物预测信息和全局路线指引等。
OnLanePlanning的下游模块是Control（控制模块）。它将生成的规划轨迹传递给Control模块，由Control模块执行具体的车辆控制操作。

5、场景应用

OnLanePlanning主要适用于城区和高速公路等具有车道线的道路场景。在这些场景中，它能够有效地进行路径规划和决策，确保车辆的安全行驶。

6、技术特点

基于车道线的规划：OnLanePlanning的规划算法都是基于车道的，即基于参考线的规划。这使得它能够更好地适应具有车道线的道路场景。
实时性强：采用事件触发方式运行，能够根据车辆状态和道路情况实时生成规划轨迹。
安全性高：在规划过程中会考虑实际临时或移动障碍物以及速度、动力学约束等因素，确保生成的规划轨迹是安全的。

综上所述，Apollo的OnLanePlanning是一个功能强大且高效的路径规划模块。它能够在具有车道线的道路场景中为自动驾驶车辆提供安全且舒适的行驶路径。

二、OnLanePlanning 类定义分析

class OnLanePlanning : public PlanningBase {
 public:
  explicit OnLanePlanning(const std::shared_ptr<DependencyInjector>& injector)
      : PlanningBase(injector) {}

  virtual ~OnLanePlanning();

  std::string Name() const override;

  common::Status Init(const PlanningConfig& config) override;

  void RunOnce(const LocalView& local_view,
               ADCTrajectory* const ptr_trajectory_pb) override;

  common::Status Plan(
      const double current_time_stamp,
      const std::vector<common::TrajectoryPoint>& stitching_trajectory,
      ADCTrajectory* const trajectory) override;

 private:
  common::Status InitFrame(const uint32_t sequence_num,
                           const common::TrajectoryPoint& planning_start_point,
                           const common::VehicleState& vehicle_state);

  common::VehicleState AlignTimeStamp(const common::VehicleState& vehicle_state,
                                      const double curr_timestamp) const;

  void ExportReferenceLineDebug(planning_internal::Debug* debug);
  bool CheckPlanningConfig(const PlanningConfig& config);
  void GenerateStopTrajectory(ADCTrajectory* ptr_trajectory_pb);
  void ExportFailedLaneChangeSTChart(const planning_internal::Debug& debug_info,
                                     planning_internal::Debug* debug_chart);
  void ExportOnLaneChart(const planning_internal::Debug& debug_info,
                         planning_internal::Debug* debug_chart);
  void ExportOpenSpaceChart(const planning_internal::Debug& debug_info,
                            const ADCTrajectory& trajectory_pb,
                            planning_internal::Debug* debug_chart);
  void AddOpenSpaceOptimizerResult(const planning_internal::Debug& debug_info,
                                   planning_internal::Debug* debug_chart);
  void AddPartitionedTrajectory(const planning_internal::Debug& debug_info,
                                planning_internal::Debug* debug_chart);

  void AddStitchSpeedProfile(planning_internal::Debug* debug_chart);

  void AddPublishedSpeed(const ADCTrajectory& trajectory_pb,
                         planning_internal::Debug* debug_chart);

  void AddPublishedAcceleration(const ADCTrajectory& trajectory_pb,
                                planning_internal::Debug* debug);

  void AddFallbackTrajectory(const planning_internal::Debug& debug_info,
                             planning_internal::Debug* debug_chart);

 private:
  PlanningCommand last_command_;
  std::unique_ptr<ReferenceLineProvider> reference_line_provider_;
  Smoother planning_smoother_;
};

构造函数与析构函数

构造函数：explicit OnLanePlanning(const std::shared_ptr<DependencyInjector>& injector)接收一个DependencyInjector的智能指针，用于依赖注入。这是Apollo框架中常用的设计模式，用于解耦模块间的依赖关系。
析构函数：virtual ~OnLanePlanning();是一个虚析构函数，确保当通过基类指针删除派生类对象时，能够正确调用派生类的析构函数。

成员函数

Name：返回规划模块的名称，即"OnLanePlanning"。
Init：初始化函数，接收一个PlanningConfig对象作为配置参数，并返回初始化状态。在初始化过程中，会进行一系列的配置检查和资源分配。
RunOnce：规划模块的主逻辑函数，由定时器周期触发。它接收一个LocalView对象（包含局部环境信息）和一个ADCTrajectory指针（用于存储生成的规划轨迹）。
Plan：根据当前时间戳、拼接轨迹和规划轨迹指针，生成具体的规划轨迹。这个函数可能用于在特定情况下（如重新规划）生成新的轨迹。

私有成员函数

InitFrame：初始化规划帧，包括设置序列号、规划起点和车辆状态。
AlignTimeStamp：对齐车辆状态的时间戳，确保与当前时间一致。
ExportReferenceLineDebug：导出参考线的调试信息。
CheckPlanningConfig：检查规划配置的有效性。
GenerateStopTrajectory：生成停车轨迹。
ExportFailedLaneChangeSTChart、ExportOnLaneChart、ExportOpenSpaceChart：导出不同场景下的调试图表，包括车道变换失败、在车道上行驶和开放空间场景。
AddOpenSpaceOptimizerResult、AddPartitionedTrajectory：向调试图表中添加开放空间优化结果和分区轨迹。
AddStitchSpeedProfile、AddPublishedSpeed、AddPublishedAcceleration：向调试图表中添加拼接速度曲线、发布的速度和加速度信息。
AddFallbackTrajectory：向调试图表中添加备用轨迹。

成员变量

last_command_：记录上一个规划命令。
reference_line_provider_：一个指向ReferenceLineProvider的唯一指针，用于提供参考线信息。
planning_smoother_：一个平滑器对象，用于对规划轨迹进行平滑处理。

总结

OnLanePlanning类是一个功能丰富的路径规划模块，它基于车道线进行规划，并考虑了多种场景和约束条件。通过提供的接口和成员函数，它能够生成安全、舒适且符合交通规则的行驶轨迹。此外，该类还提供了丰富的调试信息导出功能，有助于开发者进行问题诊断和性能优化。

三、OnLanePlanning::Init() 介绍

代码解析：

Status OnLanePlanning::Init(const PlanningConfig& config) {
  if (!CheckPlanningConfig(config)) {
    return Status(ErrorCode::PLANNING_ERROR,
                  "planning config error: " + config.DebugString());
  }

  PlanningBase::Init(config);

  // 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
  const ReferenceLineConfig* reference_line_config = nullptr;
  if (config_.has_reference_line_config()) {
    reference_line_config = &config_.reference_line_config();
  }
  reference_line_provider_ = std::make_unique<ReferenceLineProvider>(
      injector_->vehicle_state(), reference_line_config);
  reference_line_provider_->Start();

  // dispatch planner
  LoadPlanner();
  if (!planner_) {
    return Status(
        ErrorCode::PLANNING_ERROR,
        "planning is not initialized with config : " + config_.DebugString());
  }

  if (config_.learning_mode() != PlanningConfig::NO_LEARNING) {
    PlanningSemanticMapConfig renderer_config;
    ACHECK(apollo::cyber::common::GetProtoFromFile(
        FLAGS_planning_birdview_img_feature_renderer_config_file,
        &renderer_config))
        << "Failed to load renderer config"
        << FLAGS_planning_birdview_img_feature_renderer_config_file;

    BirdviewImgFeatureRenderer::Instance()->Init(renderer_config);
  }

  traffic_decider_.Init(injector_);

  start_time_ = Clock::NowInSeconds();
  return planner_->Init(injector_, FLAGS_planner_config_path);
}

OnLanePlanning::Init 函数是一个初始化函数，用于设置和准备OnLanePlanning类的实例。这个函数执行了以下关键步骤：

配置检查：
- 调用CheckPlanningConfig函数来验证传入的PlanningConfig配置对象是否有效。
- 如果配置无效，则返回一个包含错误代码和错误信息的Status对象。
基类初始化：
- 调用基类PlanningBase的Init函数，传入相同的配置对象。
清除历史记录和状态：
- 通过injector_（一个依赖注入器）清除规划历史和规划状态。
加载地图：
- 使用HDMapUtil::BaseMapPtr()获取高清地图的指针，并存储在hdmap_成员变量中。
- 使用ACHECK宏来确保地图加载成功；如果失败，则打印错误信息并终止程序。
实例化参考线提供者：
- 检查配置中是否包含参考线配置。
- 根据配置（如果有）和车辆状态实例化ReferenceLineProvider。
- 启动参考线提供者。
加载规划器：
- 调用LoadPlanner函数来加载和设置规划器。
- 如果规划器未成功加载，则返回一个包含错误代码和错误信息的Status对象。
学习模式设置（可选）：
- 如果配置中指定了学习模式，则加载渲染器配置并初始化BirdviewImgFeatureRenderer。
交通决策器初始化：
- 调用traffic_decider_的Init函数，传入依赖注入器。
记录开始时间：
- 记录初始化过程的开始时间到start_time_成员变量中。
规划器初始化：
- 最后，调用规划器的Init函数，传入依赖注入器和规划器配置路径。
- 返回规划器初始化的结果。

这个函数是OnLanePlanning类的一个重要组成部分，因为它负责设置类的初始状态，包括加载必要的配置、资源和服务，以及确保所有依赖项都已正确初始化。如果初始化过程中的任何步骤失败，函数将返回一个包含错误信息的Status对象，这有助于调用者了解初始化失败的原因。

c++知识，派生来中调用基类函数：

在init函数中，调用了基类的init函数，

  PlanningBase::Init(config);

在C++中，派生类可以通过多种方式调用基类的函数。以下是几种常见的方法：

1. 直接调用基类成员函数（如果它不是虚函数）

如果基类的成员函数不是虚函数，派生类可以直接通过基类名和作用域解析运算符（::）来调用它，或者使用this->指针（虽然在这种情况下通常不是必需的）。但是，更常见的是直接使用成员函数名，因为编译器会根据上下文自动解析为基类的版本（如果派生类没有覆盖它）。

class Base {  
public:  
    void NonVirtualFunction() {  
        // 基类实现  
    }  
};  
 
class Derived : public Base {  
public:  
    void SomeFunction() {  
        // 直接调用基类的非虚函数  
        Base::NonVirtualFunction();  
        // 或者简单地调用（如果Derived没有同名函数）  
        NonVirtualFunction();  
    }  

};

2. 通过基类指针或引用调用虚函数

如果基类的成员函数是虚函数，并且派生类提供了覆盖（override），那么通过基类指针或引用调用该函数时，将调用派生类的版本。但是，有时你可能希望调用基类的版本。这可以通过使用Base::前缀来实现。

class Base {  
public:  
    virtual void VirtualFunction() {  
        // 基类实现  
    }  
};  
 
class Derived : public Base {  
public:  
    void VirtualFunction() override {  
        // 派生类实现  
        // 调用基类的虚函数  
        Base::VirtualFunction();  
    }  

};

在上面的例子中，当Derived的VirtualFunction被调用时，它首先会调用基类的VirtualFunction，然后执行自己的实现。

3. 使用this->指针调用虚函数（显式地调用基类版本）

虽然不常见，但你也可以使用this->指针和Base::前缀来显式地调用基类的虚函数。这通常用于在派生类的覆盖函数中调用基类的实现。

class Derived : public Base {  
public:  
    void VirtualFunction() override {  
        // 使用this指针和Base::前缀调用基类的虚函数  
        this->Base::VirtualFunction();  
        // 或者简单地  
        Base::VirtualFunction();  
        // 派生类自己的实现  
    }  

};

4. 构造函数和析构函数中的调用

在派生类的构造函数中，如果需要调用基类的构造函数，这必须在初始化列表中完成。对于析构函数，当派生类的析构函数被调用时，它会自动调用基类的析构函数（如果基类有析构函数的话）。

class Base {  
public:  
    Base() { /* 基类构造函数实现 */ }  
    virtual ~Base() { /* 基类析构函数实现 */ }  
};  
 
class Derived : public Base {  
public:  
    Derived() : Base() { /* 派生类构造函数实现 */ }  
    ~Derived() override { /* 派生类析构函数实现；基类析构函数会自动调用 */ }  

};

请注意，在析构函数中，你通常不需要（也不应该）显式地调用基类的析构函数，因为C++会自动处理这一点。然而，在构造函数中，你必须通过初始化列表来调用基类的构造函数。

总的来说，调用基类函数的方法取决于函数是否是虚函数、你是否有一个指向基类或派生类对象的指针或引用，以及你是在构造函数、析构函数还是普通成员函数中调用它。

四、reference_line_provider_参考线生成器线程启动

reference_line_provider_使用异步的方式进行参考线的更新与坐标系的计算，

  reference_line_provider_ = std::make_unique<ReferenceLineProvider>(
      injector_->vehicle_state(), reference_line_config);
  reference_line_provider_->Start();

bool ReferenceLineProvider::Start() {
  if (FLAGS_use_navigation_mode) {
    return true;
  }
  if (!is_initialized_) {
    AERROR << "ReferenceLineProvider has NOT been initiated.";
    return false;
  }

  if (FLAGS_enable_reference_line_provider_thread) {
    task_future_ = cyber::Async(&ReferenceLineProvider::GenerateThread, this);
  }
  return true;
}

ReferenceLineProvider::Start 函数是 ReferenceLineProvider 类的一个成员函数，用于启动参考线生成器。下面是对该函数的详细解释：

检查导航模式：
- 首先，函数检查全局标志 FLAGS_use_navigation_mode 是否被设置。
- 如果设置了导航模式（即 FLAGS_use_navigation_mode 为 true），则函数直接返回 true，表示参考线提供者已启动（在导航模式下可能不需要额外的线程或处理）。
检查初始化状态：
- 接下来，函数检查成员变量 is_initialized_，以确定 ReferenceLineProvider 是否已经被正确初始化。
- 如果 is_initialized_ 为 false，则打印错误日志，并返回 false，表示参考线提供者未能启动。
启动参考线生成线程：
- 如果全局标志 FLAGS_enable_reference_line_provider_thread 被设置（即允许启动参考线提供者线程），则函数会调用 cyber::Async 方法来异步启动一个线程。
- cyber::Async 方法接受一个函数指针和对象指针作为参数，这里传递的是 ReferenceLineProvider::GenerateThread 成员函数和 this 指针（指向当前 ReferenceLineProvider 对象）。
- task_future_ 成员变量被赋值为 cyber::Async 调用返回的 future 对象，这个对象可以用于等待线程完成或获取线程的结果（如果有的话）。不过，在这个特定的代码中，task_future_ 的值似乎没有被进一步使用。
返回成功：
- 最后，如果所有检查都通过，并且线程（如果需要的话）已经启动，函数返回 true，表示参考线提供者已成功启动。

需要注意的是，这个函数中有几个关键点：

全局标志（如 FLAGS_use_navigation_mode 和 FLAGS_enable_reference_line_provider_thread）用于控制函数的行为。这些标志通常在程序启动时设置，并影响整个程序的运行。
成员变量 is_initialized_ 用于跟踪 ReferenceLineProvider 对象的初始化状态。
cyber::Async 是 Apollo 自动驾驶软件框架中用于异步执行任务的函数。它允许程序在不阻塞主线程的情况下执行耗时的操作。

此外，这个函数的实现假设了一些外部条件，比如 is_initialized_ 应该在某处被正确设置，以及 cyber::Async 调用能够成功启动线程。在实际应用中，这些假设需要得到保证，以确保程序的正确性和稳定性。

在planning模块运行时，输入条件如下：

五、LoadPlanner()，初始化规划器

void PlanningBase::LoadPlanner() {
  // Use PublicRoadPlanner as default Planner
  std::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);
}

PlanningBase::LoadPlanner 函数是 PlanningBase 类的一个成员函数，用于加载并初始化规划器（Planner）。这个函数的具体作用如下：

设置默认规划器：
- 函数开始时，将 planner_name 设置为 "apollo::planning::PublicRoadPlanner"，这是默认的规划器名称。
检查配置：
- 接下来，函数检查成员变量 config_ 中的 planner 配置项是否为空。
- 如果 config_.planner() 不为空（即用户指定了其他的规划器），则将 planner_name 更新为用户指定的规划器名称。
- 然后，使用 ConfigUtil::GetFullPlanningClassName 函数来获取规划器的完整类名。这个函数可能会添加一些前缀或后缀，以确保类名与实际的规划器类匹配。
创建规划器实例：
- 最后，函数使用 cyber::plugin_manager::PluginManager::Instance()->CreateInstance<Planner> 方法来创建规划器实例。
- 这里，planner_name 被用作模板参数，告诉插件管理器要创建哪个类的实例。
- planner_ 成员变量被赋值为新创建的规划器实例。

需要注意的是：

config_ 成员变量应该是一个包含规划器配置信息的对象。它可能在 PlanningBase 类的构造函数或其他成员函数中被初始化。
cyber::plugin_manager::PluginManager 是 Apollo 自动驾驶软件框架中用于管理插件（如规划器）的类。CreateInstance 方法根据提供的类名动态地创建类的实例。
Planner 是一个抽象基类或接口，定义了规划器应该实现的方法。具体的规划器类（如 PublicRoadPlanner）将继承自 Planner 并实现其方法。
如果 CreateInstance 方法无法找到或创建指定的规划器类实例，它可能会返回一个空指针或抛出异常。在实际应用中，应该检查 planner_ 是否为 nullptr，并相应地处理错误情况。

此外，这个函数的实现假设了 cyber::plugin_manager::PluginManager 和相关的配置系统已经正确设置，并且能够找到并创建用户指定的规划器类实例。在实际应用中，这些假设需要得到保证，以确保程序的正确性和稳定性。

六、TrafficDecider::Init()

bool TrafficDecider::Init(const std::shared_ptr<DependencyInjector> &injector) {
  if (init_) return true;
  // Load the pipeline config.
  AINFO << "Load config path:" << FLAGS_traffic_rule_config_filename;
  // Load the pipeline of scenario.
  if (!apollo::cyber::common::LoadConfig(FLAGS_traffic_rule_config_filename,
                                         &rule_pipeline_)) {
    AERROR << "Load pipeline of Traffic decider"
           << " failed!";
    return false;
  }
  //-----------------------------------------------
  for (int i = 0; i < rule_pipeline_.rule_size(); i++) {
    auto rule =
        apollo::cyber::plugin_manager::PluginManager::Instance()
            ->CreateInstance<TrafficRule>(ConfigUtil::GetFullPlanningClassName(
                rule_pipeline_.rule(i).type()));
    if (!rule) {
      AERROR << "Init of Traffic rule" << rule_pipeline_.rule(i).name()
             << " failed!";
      return false;
    }
    rule->Init(rule_pipeline_.rule(i).name(), injector);
    rule_list_.push_back(rule);
  }

  init_ = true;
  return true;
}

该函数的主要目的是初始化交通决策器，具体步骤和逻辑如下：

检查是否已经初始化：首先，函数通过检查init_成员变量的值来判断TrafficDecider是否已经初始化。如果init_为true，表示已经初始化过，直接返回true。
加载管道配置：接着，函数通过打印日志信息表明它正在加载配置文件的路径，这个路径是通过宏FLAGS_traffic_rule_config_filename指定的。然后，使用apollo::cyber::common::LoadConfig函数尝试加载该配置文件到rule_pipeline_成员变量中。如果加载失败，则打印错误信息并返回false。
初始化规则列表：配置文件加载成功后，函数遍历rule_pipeline_中定义的每一条规则。对于每一条规则，它使用apollo::cyber::plugin_manager::PluginManager的CreateInstance方法来创建一个TrafficRule的实例。CreateInstance方法需要一个类名字符串作为参数，这个字符串通过ConfigUtil::GetFullPlanningClassName函数从规则配置中获取。
- 如果CreateInstance无法创建TrafficRule实例（返回nullptr），则打印错误信息（包含规则的名字），并返回false。
- 如果实例创建成功，则调用该实例的Init方法，传入规则的名字和injector（一个依赖注入器）。
- 将成功初始化的规则实例添加到rule_list_成员变量中，以便后续使用。
标记为已初始化：所有规则都成功初始化后，将init_成员变量设置为true，表示TrafficDecider已经初始化完成。
返回成功：最后，函数返回true，表示初始化成功。

总结来说，这个Init函数负责加载交通决策器的配置，并根据配置创建和初始化一系列的交通规则（TrafficRule）实例。如果在任何步骤中遇到错误（如配置文件加载失败、规则实例创建失败等），函数将提前返回false。

七、PublicRoadPlanner::Init()

Status PublicRoadPlanner::Init(
    const std::shared_ptr<DependencyInjector>& injector,
    const std::string& config_path) {
  Planner::Init(injector, config_path);
  LoadConfig<PlannerPublicRoadConfig>(config_path, &config_);
  scenario_manager_.Init(injector, config_);
  return Status::OK();
}

PublicRoadPlanner是Planner类的一个子类，专门用于处理公共道路场景的规划任务。下面是对这段代码的详细解读：

函数签名：
- Status PublicRoadPlanner::Init(const std::shared_ptr<DependencyInjector>& injector, const std::string& config_path)
- 这个函数接收两个参数：一个是依赖注入器DependencyInjector的智能指针，另一个是配置文件的路径。
- 函数返回一个Status对象，这通常用于表示操作的成功或失败状态。
调用基类初始化方法：
- Planner::Init(injector, config_path);
- 在PublicRoadPlanner的Init函数中，首先调用了基类Planner的Init方法，传入了相同的参数。
- 这表明PublicRoadPlanner的初始化过程需要首先完成基类Planner的初始化。
加载配置：
- LoadConfig<PlannerPublicRoadConfig>(config_path, &config_);
- 接下来，使用LoadConfig模板函数从指定的配置文件路径加载配置。
- LoadConfig函数接受两个参数：配置文件的路径和一个指向配置对象（在这里是PlannerPublicRoadConfig类型的对象）的指针。
- PlannerPublicRoadConfig是专门为PublicRoadPlanner设计的配置类，用于存储与公共道路规划相关的配置信息。
- 加载的配置信息被存储在config_成员变量中，供后续使用。
初始化场景管理器：
- scenario_manager_.Init(injector, config_);
- 然后，调用scenario_manager_成员变量的Init方法，同样传入了依赖注入器和配置信息。
- scenario_manager_是负责管理和处理不同交通场景的对象。
- 通过初始化scenario_manager_，PublicRoadPlanner能够根据不同的交通场景做出相应的规划决策。
返回成功状态：
- return Status::OK();
- 最后，函数返回一个表示操作成功的状态对象。
- Status::OK()通常是一个静态方法，用于创建一个表示成功状态的Status对象。

总结来说，PublicRoadPlanner的Init函数首先完成了基类的初始化，然后加载了专门的配置信息，并初始化了场景管理器。这个过程确保了PublicRoadPlanner在开始执行规划任务之前已经做好了充分的准备。

八、ScenarioManager::Init()

bool ScenarioManager::Init(const std::shared_ptr<DependencyInjector>& injector,
                           const PlannerPublicRoadConfig& planner_config) {
  if (init_) {
    return true;
  }
  injector_ = injector;
  for (int i = 0; i < planner_config.scenario_size(); i++) {
    auto scenario = PluginManager::Instance()->CreateInstance<Scenario>(
        ConfigUtil::GetFullPlanningClassName(
            planner_config.scenario(i).type()));
    ACHECK(scenario->Init(injector_, planner_config.scenario(i).name()))
        << "Can not init scenario" << planner_config.scenario(i).name();
    scenario_list_.push_back(scenario);
    if (planner_config.scenario(i).name() == "LANE_FOLLOW") {
      default_scenario_type_ = scenario;
    }
  }
  AINFO << "Load scenario list:" << planner_config.DebugString();
  current_scenario_ = default_scenario_type_;
  init_ = true;
  return true;
}

ScenarioManager负责管理不同交通场景规划的对象。下面是对这段代码的详细解读：

函数签名：
- bool ScenarioManager::Init(const std::shared_ptr<DependencyInjector>& injector, const PlannerPublicRoadConfig& planner_config)
- 函数接收两个参数：一个是依赖注入器DependencyInjector的智能指针，另一个是PlannerPublicRoadConfig类型的配置对象（通常这种配置对象很大，使用引用传递以避免不必要的复制）。
- 函数返回一个布尔值，表示初始化是否成功。
检查是否已经初始化：
- if (init_) { return true; }
- 如果ScenarioManager已经初始化过（init_为true），则直接返回true，避免重复初始化。
保存依赖注入器：
- injector_ = injector;
- 将传入的依赖注入器智能指针保存到成员变量injector_中，供后续使用。
遍历配置中的场景：
- 使用for循环遍历planner_config中定义的场景列表。
- 对于每个场景，使用PluginManager::Instance()->CreateInstance<Scenario>方法创建一个Scenario的实例。
  - 这个方法需要一个类名字符串作为参数，这个字符串通过ConfigUtil::GetFullPlanningClassName函数从场景配置中获取。
初始化场景实例：
- 使用ACHECK宏来检查场景实例的Init方法是否成功。
  - ACHECK是一个断言宏，如果条件不满足（即scenario->Init(...)返回false），则打印错误信息并终止程序。
  - 错误信息包括无法初始化的场景名称。
- 如果初始化成功，将场景实例添加到scenario_list_成员变量中。
设置默认场景：
- 如果场景的名称是"LANE_FOLLOW"，则将该场景设置为默认场景（default_scenario_type_）。
打印加载的场景列表：
- 使用AINFO宏打印加载的场景列表的调试信息。
- planner_config.DebugString()方法可能返回一个包含所有场景配置的字符串表示。
设置当前场景：
- 将当前场景（current_scenario_）设置为默认场景。
标记为已初始化：
- 将init_成员变量设置为true，表示ScenarioManager已经初始化完成。
返回成功：
- 函数返回true，表示初始化成功。

总结来说，ScenarioManager的Init函数负责根据配置信息加载和初始化一系列的场景（Scenario）实例，并设置一个默认场景。如果在任何步骤中遇到错误（如场景实例创建失败、场景初始化失败等），函数将使用ACHECK宏终止程序。成功初始化后，函数将标记ScenarioManager为已初始化状态，并返回true。