Apollo学习——planning模块（3）之planning_base

planning_component 、planning_base 、on_lane_planning 和 navi_planning 的关系

---

1. 模块关系总览

继承层次

• PlanningComponent ：Cyber RT 框架中的 入口组件，负责调度规划模块的输入输出和管理生命周期。
• PlanningBase ：规划算法的 抽象基类 ，定义通用接口（如 Init(), RunOnce()）。
• OnLanePlanning 与 NaviPlanning ：继承自 PlanningBase 的 具体规划器实现，分别针对不同场景设计的动态多态子类。

调用关系

PlanningComponent → (通过指针调用) PlanningBase → OnLanaPlanning/NaviPlanning → 具体规划算法

---

2. 各模块作用详解

(1) PlanningComponent

• 功能定位：

  • 数据集成 ：接收上游模块的输入（如 PredictionObstacles 障碍物预测、LocalizationEstimate 定位数据、Chassis 底盘信息）。
  • 生命周期管理 ：通过 Init() 初始化规划器（选择 OnLanePlanning 或 NaviPlanning），在 Proc() 中触发规划主流程。
  • 消息发布 ：输出规划轨迹 ADCTrajectory、路由请求 RoutingRequest 和学习数据 PlanningLearningData。

• 核心代码逻辑：

  // 初始化时根据配置选择规划模式
  if (FLAGS_use_navigation_mode) {
    planning_base_ = std::make_unique<NaviPlanning>(injector_);
  } else {
    planning_base_ = std::make_unique<OnLanePlanning>(injector_);
  }

(2) PlanningBase

• 功能定位：

  • 抽象接口定义 ：提供规划算法的统一接口（如 RunOnce() 和 Plan()），确保子类实现一致性。
  • 数据封装 ：管理 Frame 对象（包含车辆状态、环境信息）和 DependencyInjector（依赖注入容器，集成场景、车辆模型等）。
  • 多态支持 ：通过虚函数机制实现动态绑定，允许运行时切换 OnLanePlanning 或 NaviPlanning。

• 关键成员：

  • Frame：整合当前车辆状态、障碍物信息、参考线等上下文数据。
  • ReferenceLineInfo：存储参考线及其动态决策信息，供轨迹规划使用。

(3) OnLanePlanning

• 功能定位：

  • 高精地图规划 ：基于高精地图生成参考线，适用于 城市道路 、复杂交通场景（如红绿灯路口、人行道）。
  • 场景机制 ：内置双层状态机（Scenario + Stage），支持 LaneFollowScenario（车道保持）、TrafficLightProtectedScenario（交通灯路口）等场景。
  • 任务调度 ：调用 PublicRoadPlanner 等规划器，执行具体的路径优化算法（如 EM Planner、Lattice Planner）。

• 典型流程：

  1. 生成参考线（ReferenceLineProvider）。
  2. 场景切换判定（ScenarioManager::Update()）。
  3. 执行场景内的多阶段任务（Stage + Task）。

(4) NaviPlanning

• 功能定位：

  • 相对地图规划 ：基于实时生成的相对地图（relative_map），适用于 高速公路 或其他 规则简单场景。
  • 轻量化设计：简化参考线生成逻辑，依赖车道级导航信息而非全局高精地图。
  • 高效处理：优化算法复杂度，适合长距离、低动态障碍物的场景。

• 与 OnLanePlanning 的差异：

  特性	OnLanePlanning	NaviPlanning
  地图依赖	高精地图	相对地图（动态生成）
  适用场景	城市道路、复杂交通	高速公路、规则简单道路
  场景切换机制	支持多场景状态机	场景逻辑简化
  计算资源消耗	较高	较低

---

3. 数据流与功能协作

协作流程

1. 输入触发 ：
   • PlanningComponent 的 Proc() 由上游消息（如障碍物预测）触发，整合 LocalView 数据包。
2. 规划执行 ：
   • 调用 planning_base_->RunOnce()，由具体子类（OnLanePlanning 或 NaviPlanning）实现规划逻辑。
3. 场景与任务 ：
   • OnLanePlanning 中通过 ScenarioManager 切换场景，执行阶段任务（如绕障、停车）。
   • NaviPlanning 直接生成参考线并规划轨迹，跳过复杂场景判定。
4. 结果输出 ：
   • 发布 ADCTrajectory 轨迹至控制模块，完成闭环。

关键交互点

• DependencyInjector：注入全局依赖（如车辆状态、历史轨迹），供所有子类共享。
• PlanningLearningData ：在 RL_TEST 模式下收集强化学习训练数据。

---

总结

• PlanningComponent 是入口，负责数据集成与调度。
• PlanningBase 定义框架，实现多态支持。
• OnLanePlanning 面向复杂场景，NaviPlanning 面向高速场景，二者通过继承实现功能差异化。

基类：planning_base

规划器（OnLanePlanning、NaviPlanning）需要重写的函数有

  //初始化
  virtual apollo::common::Status Init(const PlanningConfig& config);//可以不重写

  virtual std::string Name() const = 0;

  virtual void RunOnce(const LocalView& local_view, ADCTrajectory* const adc_trajectory) = 0;
  
  virtual apollo::common::Status Plan( const double current_time_stamp, const std::vector<common::TrajectoryPoint>& stitching_trajectory, ADCTrajectory* const trajectory) = 0;

不需要重写的有

  //判断规划是否完成
  bool IsPlanningFinished( const ADCTrajectory::TrajectoryType& current_trajectory_type) const;
  // 为轨迹添加设置路的左右边界
  bool GenerateWidthOfLane(const Vec2d& current_location, Vec2d& left_point, Vec2d& right_point);

 protected:
 //规划模块生成轨迹数据时填充协议头信息（Header）的核心方法
  virtual void FillPlanningPb(const double timestamp, ADCTrajectory* const trajectory_pb);
  //加载规划器 默认使用apollo::planning::PublicRoadPlanner
  void LoadPlanner();

代码解释

namespace apollo {
namespace planning {

using apollo::common::Status;

PlanningBase::PlanningBase(const std::shared_ptr<DependencyInjector>& injector): injector_(injector) {}

PlanningBase::~PlanningBase() {}
//如果不重写，则使用基类Init，加载参数
Status PlanningBase::Init(const PlanningConfig& config) {
  injector_->planning_context()->Init();
  config_ = config;
  return Status::OK();
}
//判断规划状态是否完成
bool PlanningBase::IsPlanningFinished(
    const ADCTrajectory::TrajectoryType& current_trajectory_type) const {
  const auto frame = injector_->frame_history()->Latest();
  //判断当前轨迹类型是否为OPEN SPACE 泊车
  if (current_trajectory_type == apollo::planning::ADCTrajectory::OPEN_SPACE) {
    AINFO << "Current trajectory type is: OPEN SPACE";
    if (frame->open_space_info().openspace_planning_finish()) {
      AINFO << "OPEN SPACE: planning finished";
      return true;
    } else {
      AINFO << "OPEN SPACE: planning not finished";
      return false;
    }
  } else {
    // const auto frame = injector_->frame_history()->Latest();
    if (nullptr == frame || frame->reference_line_info().empty() ||
        nullptr == local_view_.planning_command) {
      AINFO << "Current reference point is empty;";
      return true;
    }
    const auto& reference_line_info = frame->reference_line_info().front();
    // Check if the ReferenceLineInfo is the last passage.
    const auto& reference_points =
        reference_line_info.reference_line().reference_points();
    if (reference_points.empty()) {
      AINFO << "Current reference points is empty;";
      return true;
    }
    const auto& last_reference_point = reference_points.back();
    const std::vector<hdmap::LaneWaypoint>& lane_way_points =
        last_reference_point.lane_waypoints();
    if (lane_way_points.empty()) {
      AINFO << "Last reference point is empty;";
      return true;
    }
    // Get the end lane way point.
    if (nullptr == frame->local_view().end_lane_way_point) {
      AINFO << "Current end lane way is empty;";
      return true;
    }
    bool is_has_passed_destination = injector_->planning_context()
                                         ->planning_status()
                                         .destination()
                                         .has_passed_destination();
    AINFO << "Current passed destination:" << is_has_passed_destination;
    return is_has_passed_destination;
  }
}
// 规划模块生成轨迹数据时填充协议头信息（Header）的核心方法
// 时间戳同步：设置轨迹消息的全局时间戳
// 多传感器时间对齐：记录激光雷达、摄像头、雷达等传感器的原始数据时间戳
// 路由信息传递：复制当前路由请求的元数据
void PlanningBase::FillPlanningPb(const double timestamp,
                                  ADCTrajectory* const trajectory_pb) {
  trajectory_pb->mutable_header()->set_timestamp_sec(timestamp); // timestamp通常通过Clock::NowInSeconds()获取系统当前时间
  // 多传感器时间对齐
  if (local_view_.prediction_obstacles->has_header()) {
    trajectory_pb->mutable_header()->set_lidar_timestamp(
        local_view_.prediction_obstacles->header().lidar_timestamp());
    trajectory_pb->mutable_header()->set_camera_timestamp(
        local_view_.prediction_obstacles->header().camera_timestamp());
    trajectory_pb->mutable_header()->set_radar_timestamp(
        local_view_.prediction_obstacles->header().radar_timestamp());
  }
  // local_view_.planning_command包含来自Routing模块的全局路径请求
  trajectory_pb->mutable_routing_header()->CopyFrom(
      local_view_.planning_command->header());
}
//加载规划器默认apollo::planning::PublicRoadPlanner
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);
}
//设置路的左右边界
bool PlanningBase::GenerateWidthOfLane(const Vec2d& current_location,
                                       Vec2d& left_point, Vec2d& right_point) {
  double left_width = 0, right_width = 0;
  const auto frame = injector_->frame_history()->Latest();
  if (nullptr == frame || frame->reference_line_info().empty()) {
    AINFO << "Reference lane is empty!";
    return false;
  }
  const auto& reference_line_info = frame->reference_line_info().front();
  // get current SL
  common::SLPoint current_sl;
  reference_line_info.reference_line().XYToSL(current_location, &current_sl);
  // Get the lane width of vehicle location
  bool get_width_of_lane = reference_line_info.reference_line().GetLaneWidth(
      current_sl.s(), &left_width, &right_width);
  AINFO << "get_width_of_lane: " << get_width_of_lane
        << ", left_width: " << left_width << ", right_width: " << right_width;
  if (get_width_of_lane && left_width != 0 && right_width != 0) {
    AINFO << "Get the width of lane successfully!";
    SLPoint sl_left_point, sl_right_point;
    sl_left_point.set_s(current_sl.s());
    sl_left_point.set_l(left_width);
    sl_right_point.set_s(current_sl.s());
    sl_right_point.set_l(-right_width);
    reference_line_info.reference_line().SLToXY(sl_left_point, &left_point);
    reference_line_info.reference_line().SLToXY(sl_right_point, &right_point);
    return true;
  } else {
    AINFO << "Failed to get the width of lane!";
    return false;
  }
}

}  // namespace planning
}  // namespace apollo