Apollo9.0 Planning2.0决策规划算法代码详细解析 (4): PlanningComponent::Proc()

news2024/10/5 15:02:38

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

欢迎来到《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决策规划算法代码详细解析》专栏，与我们一起探索自动驾驶技术的无限可能，共同推动自动驾驶技术的未来发展！

一、PlanningComponent::Proc() 简介

PlanningComponent::Proc() 作为整个planning 模块的入口，每个planning周期都会执行一次，主要提供以下一些功能：

CheckRerouting() 重新路由判断
更新local_view_，local_view_中包含一次planning需要的所有外部信息
CheckInput() 检查输入
规划器进行规划，并给输出轨迹赋值
更新轨迹的时间戳，并且发布轨迹

二、PlanningComponent::CheckRerouting() 重新路由判断

PlanningComponent::CheckRerouting()函数读取 planning的结果，判断是否需要重新规划routing，如果需要重新routing，调用rerouting_client_发送rerouting请求；

void PlanningComponent::CheckRerouting() {
  auto* rerouting = injector_->planning_context()
                        ->mutable_planning_status()
                        ->mutable_rerouting();
  if (!rerouting->need_rerouting()) {
    return;
  }
  common::util::FillHeader(node_->Name(),
                           rerouting->mutable_lane_follow_command());
  auto lane_follow_command_ptr =
      std::make_shared<apollo::external_command::LaneFollowCommand>(
          rerouting->lane_follow_command());
  rerouting_client_->SendRequest(lane_follow_command_ptr);
  rerouting->set_need_rerouting(false);
}

三、更新local_view_

local_view_中包含一次planning需要的所有外部信息：

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<relative_map::MapMsg> relative_map;
  std::shared_ptr<PadMessage> pad_msg;
  std::shared_ptr<storytelling::Stories> stories;
  std::shared_ptr<PlanningCommand> planning_command;
  std::shared_ptr<routing::LaneWaypoint> end_lane_way_point;
};

更新前：

更新后：

以local_view_.planning_command为例，语法细节如下：

local_view_.planning_command =
          std::make_shared<PlanningCommand>(planning_command_);

在这段代码中，local_view_.planning_command 被赋予了一个新的std::shared_ptr<PlanningCommand>，该指针指向一个通过 std::make_shared<PlanningCommand> 新创建的 PlanningCommand 对象。这个新对象是通过拷贝构造函数从 planning_command_ 初始化而来的。

具体步骤如下：

内存分配和对象构造：std::make_shared<PlanningCommand>(planning_command_) 调用会首先分配一块足够大的内存区域来同时存储 PlanningCommand 对象和与之关联的引用计数（以及可能的控制块，用于存储删除器等）。然后，在这块内存上构造一个 PlanningCommand 对象，作为参数传递的 planning_command_ 被用作这个新对象的拷贝源。
智能指针初始化：构造好的 PlanningCommand 对象地址被传递给 std::shared_ptr<PlanningCommand> 的构造函数，从而创建一个管理这个新对象的智能指针。此时，引用计数被初始化为1，表示有一个 std::shared_ptr 实例（即我们刚刚创建的）正在指向这个对象。
赋值操作：最后，这个新创建的 std::shared_ptr<PlanningCommand> 被赋值给 local_view_.planning_command。如果 local_view_.planning_command 之前已经指向了一个 PlanningCommand 对象，那么那个对象的引用计数会递减（如果递减到0，则对象会被自动删除）。然后，local_view_.planning_command 开始指向新创建的对象。

需要注意的是，由于 std::make_shared 和 std::shared_ptr 的使用，这段代码是线程安全的（在赋值操作本身这一层面，但前提是 local_view_ 和 planning_command_ 的访问已经被适当地同步了，比如通过互斥锁）。此外，它也符合RAII（资源获取即初始化）原则，因为智能指针的构造函数会自动管理资源的获取（在这里是对象的创建和内存的分配），而析构函数则会在适当的时候（比如智能指针离开其作用域时）自动释放资源（在这里是减少引用计数并在必要时删除对象）。

还有一点很重要，就是 PlanningCommand 类必须有一个可访问的拷贝构造函数，因为 std::make_shared 在这里使用了它来从 planning_command_ 创建新对象。如果 PlanningCommand 是不可拷贝的（即它的拷贝构造函数被删除或声明为 delete），那么这段代码将无法编译。

四、PlanningComponent::CheckInput() 检查输入

bool PlanningComponent::CheckInput() {
  ADCTrajectory trajectory_pb;
  auto* not_ready = trajectory_pb.mutable_decision()
                        ->mutable_main_decision()
                        ->mutable_not_ready();

  if (local_view_.localization_estimate == nullptr) {
    not_ready->set_reason("localization not ready");
  } else if (local_view_.chassis == nullptr) {
    not_ready->set_reason("chassis not ready");
  } else if (HDMapUtil::BaseMapPtr() == nullptr) {
    not_ready->set_reason("map not ready");
  } else {
    // nothing
  }

  if (FLAGS_use_navigation_mode) {
    if (!local_view_.relative_map->has_header()) {
      not_ready->set_reason("relative map not ready");
    }
  } else {
    if (!local_view_.planning_command ||
        !local_view_.planning_command->has_header()) {
      not_ready->set_reason("planning_command not ready");
    }
  }

  if (not_ready->has_reason()) {
    AINFO << not_ready->reason() << "; skip the planning cycle.";
    common::util::FillHeader(node_->Name(), &trajectory_pb);
    planning_writer_->Write(trajectory_pb);
    return false;
  }
  return true;
}

五、调用规划器进行规划，并给输出轨迹赋值

  ADCTrajectory adc_trajectory_pb;
  planning_base_->RunOnce(local_view_, &adc_trajectory_pb);
  auto start_time = adc_trajectory_pb.header().timestamp_sec();
  common::util::FillHeader(node_->Name(), &adc_trajectory_pb);

默认的planning_base_是OnLanePlanning，数据结构如下：

六、更新轨迹的时间戳，并且发布轨迹

  const double dt = start_time - adc_trajectory_pb.header().timestamp_sec();
  for (auto& p : *adc_trajectory_pb.mutable_trajectory_point()) {
    p.set_relative_time(p.relative_time() + dt);
  }
  planning_writer_->Write(adc_trajectory_pb);