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

介绍

• 功能定位：

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

• 典型流程：

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

---

OnLanePlanning继承自PlanningBase类，在planning_component初始化的时候使用参数进行选择加载。

初始化(Init)流程：

1、检查配置参数是否有问题；
2、初始化基类Init；
3、清楚历史数据缓存；
4、重置PlanningStatus 状态机；
5、加载高精地图hdmap_；
6、初始化参考线线程reference_line_provider_；
7、开启参考线线程；
8、加载规划器；
9、判断planner_是否加载成功；
10、初始化交通决策器
11、初始化规划器planner_->Init()

• 通过初始化，将PlanningStatus 状态机重置、加载高精地图hdmap_、开启参考线线程reference_line_provider_、加载规划器planner_、初始化交通决策器traffic_decider_、初始化规划器planner_。

---

代码注释

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 清除 History 类中缓存的障碍物状态和决策信息，避免历史数据干扰新规划周期。
  injector_->history()->Clear();

  // clear planning status 重置 PlanningStatus 状态机（如变道、停车等场景状态），确保规划从初始条件开始
  injector_->planning_context()->mutable_planning_status()->Clear();

  // load map 通过 HDMapUtil 获取高精地图（HDMap）指针，用于 生成参考线、车道匹配、场景判定（如红绿灯路口）。
  hdmap_ = HDMapUtil::BaseMapPtr();
  ACHECK(hdmap_) << "Failed to load map";

  // instantiate reference line provider 成车辆周围的参考线（Reference Line），提供平滑的中心车道线轨迹，用于路径规划和横向控制。
  const ReferenceLineConfig* reference_line_config = nullptr;
  if (config_.has_reference_line_config()) {
    reference_line_config = &config_.reference_line_config();
  }
  // 配置参数 reference_line_config 控制参考线生成规则（如采样间隔、车道扩展范围）。
  reference_line_provider_ = std::make_unique<ReferenceLineProvider>(
      injector_->vehicle_state(), reference_line_config);
  // 启动独立线程持续更新参考线，确保实时性。
  reference_line_provider_->Start();

  // dispatch planner
  LoadPlanner();//在基类中加载规划器，默认使用 PublicRoadPlanner

  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);
}

重点：

高精地图获取

高精地图获取是通过HDMapUtil::BaseMapPtr();获取，以下是源码

const HDMap* HDMapUtil::BaseMapPtr() {
  if (base_map_ == nullptr) {
    std::lock_guard<std::mutex> lock(base_map_mutex_);
    if (base_map_ == nullptr) {  // Double check.
      base_map_ = CreateMap(BaseMapFile());
    }
  }
  return base_map_.get();
}

参考线线程

参考线线程的配置参数是apollo::planning::LaneFollowMap，在文件planning_config.txt中配置
代码为

  const ReferenceLineConfig* reference_line_config = nullptr;
  if (config_.has_reference_line_config()) {
    reference_line_config = &config_.reference_line_config();
  }
  // 配置参数 reference_line_config 控制参考线生成规则（如采样间隔、车道扩展范围）。
  reference_line_provider_ = std::make_unique<ReferenceLineProvider>(
      injector_->vehicle_state(), reference_line_config);
  reference_line_provider_->Start();

LaneFollowMap: PncMapBase的一个子类，在lane_follow_command指令下，从routing结果转换到ReferenceLine的过程中做前期准备，主要功能是处理routing结果，将routing数据处理并存储在map相关的数据结构中；根据routing结果及当前车辆位置，计算可行驶的passage信息，并转换成RouteSegments结构，为ReferenceLine提供数据传入参数。封装在reference_line_provider中。

规划器的加载

默认加载PublicRoadPlanner规划器

  LoadPlanner();

交通决策器的加载

定义在planning_interface_base/traffic_rules_base/traffic_decider.h中，是planning在运行场景之前，根据不同的交通规则，决策车辆是否需要停车，减速或其他操作。因为它是在运行场景之前进行的，所以对所有的场景都会起作用。
traffic_decider_实在planning_base基类中申明。
相关链接：Apollo学习——planning模块（4）之traffic_decider交通规则决策器

traffic_decider_.Init(injector_);

planner_初始化

默认使用PublicRoadPlanner规划器

---

规划(RunOnce)流程：

OnLanePlanning::RunOnce是在主程序中循环调用，为核心程序，是规划器规划的入口程序。

1、将获取到的信息赋值给类内参数local_view_ = local_view;
2、记录当前时间；
3、获取车辆当前数据状态status，数据异常会返回状态为error；
4、如果状态status有问题，或者车速位置等不合理，则生成停车轨迹；
5、时间对齐处理；
6、通知参考线提供者当前车辆位置
7、获取终点车道信息；
8、轨迹缝合计算 避免规划周期切换时的轨迹跳变，保证控制连续性；
9、更新自车信息；
10、裁剪参考线，并更新状态status；
11、如果status有问题，则生成停车轨迹；
12、交通规则决策 遍历交通规则 整合规则处理结果，构建最终的车辆行为目标；
13、执行规划核心逻辑plan，并返回状态status；
14、打印log（轨迹点、自车的外轮廓）；
15、判断status是否问题，则生成停车轨迹；
16、重规划标记与原因记录
17、通过OpenSpaceInfo类判断当前是否处于开放空间轨迹模式；
18、如果是泊车，直接填充规划消息（FillPlanningPb），无需参考线延迟统计；
19、否则常规道路逻辑，记录参考线的耗时；
20、记录耗时等信息。

重点

获取从外界获取的各种信息local_view_

通过形参将local_view传到函数内

 local_view_ = local_view;

校验数据返回状态status

// 将定位和底盘数据融合为统一的车辆状态（如坐标、速度、航向角等） 若定位数据缺失或底盘信号异常，Update()会返回错误状态。
  Status status = injector_->vehicle_state()->Update(*local_view_.localization_estimate, *local_view_.chassis);

获取车辆状态vehicle_state

// 时间戳一致性校验 确保规划周期开始时间不早于车辆状态更新时间，避免使用过期数据。
  VehicleState vehicle_state = injector_->vehicle_state()->vehicle_state();

时间校验

  // 检测规划系统时间（start_timestamp）是否落后于车辆状态时间（vehicle_state_timestamp）。
  if (start_timestamp + 1e-6 < vehicle_state_timestamp) {
    common::monitor::MonitorLogBuffer monitor_logger_buffer(
        common::monitor::MonitorMessageItem::PLANNING);
    monitor_logger_buffer.ERROR("ego system time is behind GPS time");
  }

通知参考线提供者当前车辆位置

  // 通知参考线提供者当前车辆位置
  reference_line_provider_->UpdateVehicleState(vehicle_state);

获取终点

  // 从参考线提供者获取当前路由的终点车道点（用于判断是否接近目的地）。
  reference_line_provider_->GetEndLaneWayPoint(local_view_.end_lane_way_point);//结果存入local_view_.end_lane_way_point供后续逻辑使用。

缝合轨迹

  //轨迹缝合计算 避免规划周期切换时的轨迹跳变，保证控制连续性
  std::vector<TrajectoryPoint> stitching_trajectory = //缝合后的轨迹
      TrajectoryStitcher::ComputeStitchingTrajectory(
          *(local_view_.chassis), vehicle_state, start_timestamp,
          planning_cycle_time, FLAGS_trajectory_stitching_preserved_length,//保留的历史轨迹长度（默认约3秒）FLAGS_trajectory_stitching_preserved_length = 3s
          true, last_publishable_trajectory_.get(), &replan_reason, // 重规划原因（replan_reason）
          *local_view_.control_interactive_msg);

裁剪参考线

  //裁剪参考线  障碍物投影到参考线坐标系  生成ReferenceLineInfo对象（含路径、速度决策结果） 绑定车辆状态和初始轨迹点。
  status = InitFrame(frame_num, stitching_trajectory.back(), vehicle_state);

交通规则决策 遍历交通规则 整合规则处理结果

  // 交通规则决策 遍历交通规则 整合规则处理结果，构建最终的车辆行为目标
  for (auto& ref_line_info : *frame_->mutable_reference_line_info()) { // 遍历所有参考线（reference_line_info）。
    auto traffic_status = traffic_decider_.Execute(frame_.get(), &ref_line_info); // 执行交通规则决策（如红绿灯、停止线判断）。
    if (!traffic_status.ok() || !ref_line_info.IsDrivable()) { // 若决策失败或参考线不可行驶，标记该参考线为不可用状态。
      ref_line_info.SetDrivable(false);
      AWARN << "Reference line " << ref_line_info.Lanes().Id()<< " traffic decider failed";
    }
  }

执行规划核心逻辑Plan方法

  // 执行规划核心逻辑 1、路径优化（QP/SQP算法）。2、速度规划（ST图优化）。3、障碍物交互决策（如让行或超车）。 
  // 调用场景管理器（Scenario Manager）和任务执行器（Task Executor）
  status = Plan(start_timestamp, stitching_trajectory, ptr_trajectory_pb); // 规划周期开始时间 缝合路径时间 

代码解释

// 先执行交通规则 再规划轨迹 规划轨迹的函数是Plan
void OnLanePlanning::RunOnce(const LocalView& local_view,
                             ADCTrajectory* const ptr_trajectory_pb) {
  // when rerouting, reference line might not be updated. In this case, planning
  // module maintains not-ready until be restarted.
  local_view_ = local_view;
  // 以下时间将用于后续校验数据时效性和轨迹时间补偿
  const double start_timestamp = Clock::NowInSeconds();// 记录当前规划周期开始的Apollo内部时钟时间（秒）。
  const double start_system_timestamp = std::chrono::duration<double>(
          std::chrono::system_clock::now().time_since_epoch()).count();//记录系统时钟的绝对时间（Unix时间戳）。

  // localization
  ADEBUG << "Get localization:"<< local_view_.localization_estimate->DebugString();

  // chassis
  ADEBUG << "Get chassis:" << local_view_.chassis->DebugString();
  // 将定位和底盘数据融合为统一的车辆状态（如坐标、速度、航向角等） 若定位数据缺失或底盘信号异常，Update()会返回错误状态。
  Status status = injector_->vehicle_state()->Update(*local_view_.localization_estimate, *local_view_.chassis);

  VehicleState vehicle_state = injector_->vehicle_state()->vehicle_state();// 时间戳一致性校验 确保规划周期开始时间不早于车辆状态更新时间，避免使用过期数据。
  const double vehicle_state_timestamp = vehicle_state.timestamp();
  DCHECK_GE(start_timestamp, vehicle_state_timestamp)
      << "start_timestamp is behind vehicle_state_timestamp by "
      << start_timestamp - vehicle_state_timestamp << " secs";
  //车辆异常状态处理
  if (!status.ok() || !util::IsVehicleStateValid(vehicle_state)) {  //IsVehicleStateValid()会校验车速、位置等是否在合理范围内
    const std::string msg =
        "Update VehicleStateProvider failed "
        "or the vehicle state is out dated.";
    AERROR << msg;
    ptr_trajectory_pb->mutable_decision()
        ->mutable_main_decision()
        ->mutable_not_ready()
        ->set_reason(msg);// 在轨迹消息（ptr_trajectory_pb）中设置not_ready状态及原因。
    status.Save(ptr_trajectory_pb->mutable_header()->mutable_status());//将错误码存入消息头部的status字段。
    // TODO(all): integrate reverse gear 安全措施
    ptr_trajectory_pb->set_gear(canbus::Chassis::GEAR_DRIVE);//强制设置档位为前进档
    FillPlanningPb(start_timestamp, ptr_trajectory_pb); // 填充规划消息头
    GenerateStopTrajectory(ptr_trajectory_pb); // 生成停车轨迹（GenerateStopTrajectory），确保车辆安全停止
    return;
  }
  // 检测规划系统时间（start_timestamp）是否落后于车辆状态时间（vehicle_state_timestamp）。
  if (start_timestamp + 1e-6 < vehicle_state_timestamp) {
    common::monitor::MonitorLogBuffer monitor_logger_buffer(
        common::monitor::MonitorMessageItem::PLANNING);
    monitor_logger_buffer.ERROR("ego system time is behind GPS time");
  }
  //时间对齐处理 若时间差小于阈值（message_latency_threshold，默认约0.5秒）
  if (start_timestamp - vehicle_state_timestamp <
      FLAGS_message_latency_threshold) {
      //调用AlignTimeStamp()对齐车辆状态时间戳到规划周期开始时间。 确保规划使用的车辆状态与当前周期时间严格同步
    vehicle_state = AlignTimeStamp(vehicle_state, start_timestamp);
  }

  // 参考线更新与场景重置 Update reference line provider and reset scenario if new routing
  // 通知参考线提供者当前车辆位置
  reference_line_provider_->UpdateVehicleState(vehicle_state);
  if (local_view_.planning_command->is_motion_command() &&
      util::IsDifferentRouting(last_command_, *local_view_.planning_command)) {//比较新旧规划命令
    last_command_ = *local_view_.planning_command;
    // AINFO << "new_command:" << last_command_.DebugString();
    reference_line_provider_->Reset();//清空参考线缓存
    injector_->history()->Clear();//清除历史轨迹
    injector_->planning_context()->mutable_planning_status()->Clear();//重置规划状态机
    reference_line_provider_->UpdatePlanningCommand(//更新新路由命令
        *(local_view_.planning_command));
    planner_->Reset(frame_.get());//重置规划器内部状态
  }
  // 获取终点车道信息 Get end lane way point.
  // 从参考线提供者获取当前路由的终点车道点（用于判断是否接近目的地）。
  reference_line_provider_->GetEndLaneWayPoint(local_view_.end_lane_way_point);//结果存入local_view_.end_lane_way_point供后续逻辑使用。

  // planning is triggered by prediction data, but we can still use an estimated
  // cycle time for stitching
  const double planning_cycle_time = 1.0 / static_cast<double>(FLAGS_planning_loop_rate);//规划周期 FLAGS_planning_loop_rate = 10Hz

  std::string replan_reason;
  //轨迹缝合计算 避免规划周期切换时的轨迹跳变，保证控制连续性
  std::vector<TrajectoryPoint> stitching_trajectory = //缝合后的轨迹
      TrajectoryStitcher::ComputeStitchingTrajectory(
          *(local_view_.chassis), vehicle_state, start_timestamp,
          planning_cycle_time, FLAGS_trajectory_stitching_preserved_length,//保留的历史轨迹长度（默认约3秒）FLAGS_trajectory_stitching_preserved_length = 3s
          true, last_publishable_trajectory_.get(), &replan_reason, // 重规划原因（replan_reason）
          *local_view_.control_interactive_msg);
  // 自车信息更新 用缝合轨迹的终点和当前车辆状态更新自车信息（ego_info）。
  injector_->ego_info()->Update(stitching_trajectory.back(), vehicle_state);// back()缝合轨迹的最后一个点（最近未来状态）。 vehicle_state当前车辆状态（位置、速度等）。
  
  // 步骤：
  // 分配帧序号（frame_num），用于日志追踪。调用InitFrame()初始化规划帧：
  const uint32_t frame_num = static_cast<uint32_t>(seq_num_++);
  AINFO << "Planning start frame sequence id = [" << frame_num << "]";

  //裁剪参考线  障碍物投影到参考线坐标系  生成ReferenceLineInfo对象（含路径、速度决策结果） 绑定车辆状态和初始轨迹点。
  status = InitFrame(frame_num, stitching_trajectory.back(), vehicle_state);

  if (status.ok()) {
    injector_->ego_info()->CalculateFrontObstacleClearDistance(
        frame_->obstacles());//计算自车到最近障碍物的纵向距离（用于跟车或避障）。
    injector_->ego_info()->CalculateCurrentRouteInfo(
        reference_line_provider_.get());// 基于参考线计算当前路径的曲率、长度等属性。
  }
  // 功能：当启用调试标志时，将规划输入数据（如障碍物、参考线）记录到轨迹消息的debug字段。
  // 用途：用于离线回放或可视化工具（如Dreamview）分析规划决策依据。
  if (FLAGS_enable_record_debug) {
    frame_->RecordInputDebug(ptr_trajectory_pb->mutable_debug());
  }

  // 计算帧初始化耗时（从start_timestamp到当前时间），存入轨迹的延迟统计字段。帮助识别初始化阶段的性能瓶颈。
  ptr_trajectory_pb->mutable_latency_stats()->set_init_frame_time_ms(Clock::NowInSeconds() - start_timestamp);
  
  
  // 规划失败处理
  if (!status.ok()) {
    AERROR << status.ToString();
    if (FLAGS_publish_estop) { // 紧急停止模式（publish_estop）
      // "estop" signal check in function "Control::ProduceControlCommand()"
      // estop_ = estop_ || local_view_.trajectory.estop().is_estop();
      // we should add more information to ensure the estop being triggered.
      ADCTrajectory estop_trajectory;
      EStop* estop = estop_trajectory.mutable_estop(); //创建包含EStop标志的轨迹，标记紧急停止原因
      estop->set_is_estop(true);
      estop->set_reason(status.error_message());
      status.Save(estop_trajectory.mutable_header()->mutable_status());
      ptr_trajectory_pb->CopyFrom(estop_trajectory); //将错误状态保存到消息头。
    } else {// 普通错误模式
      ptr_trajectory_pb->mutable_decision()
          ->mutable_main_decision()
          ->mutable_not_ready()
          ->set_reason(status.ToString()); // 设置决策结果为not_ready并注明原因。
      status.Save(ptr_trajectory_pb->mutable_header()->mutable_status());
      GenerateStopTrajectory(ptr_trajectory_pb); //生成停车轨迹（GenerateStopTrajectory）。
    }
    // TODO(all): integrate reverse gear
    ptr_trajectory_pb->set_gear(canbus::Chassis::GEAR_DRIVE);
    FillPlanningPb(start_timestamp, ptr_trajectory_pb);
    frame_->set_current_frame_planned_trajectory(*ptr_trajectory_pb);
    const uint32_t n = frame_->SequenceNum();
    injector_->frame_history()->Add(n, std::move(frame_));
    return;
  }

  // 交通规则决策 遍历交通规则 整合规则处理结果，构建最终的车辆行为目标
  for (auto& ref_line_info : *frame_->mutable_reference_line_info()) { // 遍历所有参考线（reference_line_info）。
    auto traffic_status = traffic_decider_.Execute(frame_.get(), &ref_line_info); // 执行交通规则决策（如红绿灯、停止线判断）。
    if (!traffic_status.ok() || !ref_line_info.IsDrivable()) { // 若决策失败或参考线不可行驶，标记该参考线为不可用状态。
      ref_line_info.SetDrivable(false);
      AWARN << "Reference line " << ref_line_info.Lanes().Id()<< " traffic decider failed";
    }
  }

  // 执行规划核心逻辑 1、路径优化（QP/SQP算法）。2、速度规划（ST图优化）。3、障碍物交互决策（如让行或超车）。 
  // 调用场景管理器（Scenario Manager）和任务执行器（Task Executor）
  status = Plan(start_timestamp, stitching_trajectory, ptr_trajectory_pb); // 规划周期开始时间 缝合路径时间 



  // print trajxy 轨迹与障碍物可视化调试
  PrintCurves trajectory_print_curve; // 轨迹点打印
  for (const auto& p : ptr_trajectory_pb->trajectory_point()) {
    trajectory_print_curve.AddPoint("trajxy", p.path_point().x(),
                                    p.path_point().y());
  }
  trajectory_print_curve.PrintToLog();

  // print obstacle polygon // 障碍物多边形打印
  for (const auto& obstacle : frame_->obstacles()) { 
    obstacle->PrintPolygonCurve();
  }
  // print ego box // 自车包围盒打印
  PrintBox print_box("ego_box");
  print_box.AddAdcBox(vehicle_state.x(), vehicle_state.y(),
                      vehicle_state.heading(), true);
  print_box.PrintToLog();


  // 性能统计与监控
  const auto end_system_timestamp =
      std::chrono::duration<double>(std::chrono::system_clock::now().time_since_epoch()).count(); 


  // 从规划开始到结束的系统时钟差值（毫秒）
  const auto time_diff_ms = (end_system_timestamp - start_system_timestamp) * 1000;
  ADEBUG << "total planning time spend: " << time_diff_ms << " ms.";

  ptr_trajectory_pb->mutable_latency_stats()->set_total_time_ms(time_diff_ms);
  ADEBUG << "Planning latency: "<< ptr_trajectory_pb->latency_stats().DebugString();


// 当规划状态status异常时（如路径搜索失败或约束冲突）：
  if (!status.ok()) {
    status.Save(ptr_trajectory_pb->mutable_header()->mutable_status()); // 保存错误状态：将错误码和描述存入轨迹消息头部的status字段，供控制模块解析
    AERROR << "Planning failed:" << status.ToString();
    if (FLAGS_publish_estop) { // 紧急停止（EStop）处理
      AERROR << "Planning failed and set estop";
      // "estop" signal check in function "Control::ProduceControlCommand()"
      // estop_ = estop_ || local_view_.trajectory.estop().is_estop();
      // we should add more information to ensure the estop being triggered.
      EStop* estop = ptr_trajectory_pb->mutable_estop();
      estop->set_is_estop(true); // 若启用FLAGS_publish_estop，设置紧急停止标志is_estop为true，并记录失败原因（如"障碍物避让失败"）。
      estop->set_reason(status.error_message()); // 控制模块收到含estop的轨迹后，会立即触发刹车或停车
    }
  }
  // 重规划标记与原因记录
  ptr_trajectory_pb->set_is_replan(stitching_trajectory.size() == 1);
  if (ptr_trajectory_pb->is_replan()) { //is_replan标记：若缝合轨迹（stitching_trajectory）仅含1个点，说明需完全重新规划（如因路由变更或突发障碍）
    ptr_trajectory_pb->set_replan_reason(replan_reason); // 记录具体原因（如"参考线不连续"或"动态障碍物阻挡"），用于调试和策略优化
  }



  // 通过OpenSpaceInfo类判断当前是否处于开放空间轨迹模式
  // 开放空间（没有参考线，停车场之类的地方）轨迹不依赖传统车道参考线，使用自由空间规划算法（如混合A*）
  if (frame_->open_space_info().is_on_open_space_trajectory()) {
     // 开放空间逻辑 直接填充规划消息（FillPlanningPb），无需参考线延迟统计
    FillPlanningPb(start_timestamp, ptr_trajectory_pb);
    ADEBUG << "Planning pb:" << ptr_trajectory_pb->header().DebugString();
    frame_->set_current_frame_planned_trajectory(*ptr_trajectory_pb);
  } else { // 常规道路逻辑
    auto* ref_line_task = ptr_trajectory_pb->mutable_latency_stats()->add_task_stats();
    ref_line_task->set_time_ms(reference_line_provider_->LastTimeDelay() * 1000.0);//记录ReferenceLineProvider生成参考线的耗时（毫秒），用于性能分析
    ref_line_task->set_name("ReferenceLineProvider");

    FillPlanningPb(start_timestamp, ptr_trajectory_pb);
    ADEBUG << "Planning pb:" << ptr_trajectory_pb->header().DebugString();

    frame_->set_current_frame_planned_trajectory(*ptr_trajectory_pb);
    if (FLAGS_enable_planning_smoother) { // 若启用FLAGS_enable_planning_smoother，调用planning_smoother_基于历史帧优化轨迹连续性（如减少急转弯）
      planning_smoother_.Smooth(injector_->frame_history(), frame_.get(),
                                ptr_trajectory_pb);
    }
  }
  // 性能指标：
  //   计算整个规划周期耗时（plnning_perf_ms），从start_system_timestamp到当前时间 
  //   规划器名称（如OnLanePlanning）和耗时，用于监控实时性 

  const auto end_planning_perf_timestamp =  /* 当前系统时间 */
      std::chrono::duration<double>(
          std::chrono::system_clock::now().time_since_epoch())
          .count();
  const auto plnning_perf_ms =
      (end_planning_perf_timestamp - start_system_timestamp) * 1000;
  AINFO << "Planning Perf: planning name [" << Name() << "], "
        << plnning_perf_ms << " ms.";
  AINFO << "Planning end frame sequence id = [" << frame_num << "]";
  injector_->frame_history()->Add(frame_num, std::move(frame_)); // 记录帧序号（frame_num）和轨迹数据到frame_history，支持回溯分析和调试
}

---

规划核心逻辑(Plan)流程：

执行规划核心逻辑 1、路径优化（QP/SQP算法）。2、速度规划（ST图优化）。3、障碍物交互决策（如让行或超车）。 调用场景管理器（Scenario Manager）和任务执行器（Task Executor）。
通过planner_->Plan生成轨迹，支持混合A*算法PublicRoadPlannerstitching_trajectory.back()：历史轨迹末端点，确保新轨迹与历史轨迹平滑衔接 frame_.get()：包含本周期所有上下文数据（障碍物、定位、参考线等）front_clear_distance：写入车辆前方安全距离，用于控制模块的碰撞检测
// 开放空间 ： 调用HybridAStar生成粗轨迹，通过OBCA优化
// 常规道路：使用PublicRoadPlanner进行车道级路径-速度解耦规划

1、执行planner_->plan进行规划路线
2、判断是泊车还是常规行驶
泊车:
（1）同步调试信息：将开放空间规划过程中的调试数据（如轨迹优化中间状态、碰撞检测结果）同步到 debug_instance_，用于可视化或日志分析。；
（2）获取可发布的轨迹数据，前面生成的轨迹点和当前的档位（倒车过程总需要频繁切换档位）；
（3）设置轨迹类型与碰撞状态
（4）将以上状态类型等内容填充到ptr_trajectory_pb中
（5）生成人工介入建议
（6）设置停车决策状态：IN_PARKING，目的是告诉控制模块，目前处于停车中。
常规行驶：
（1）从Frame对象中获取当前最优驾驶参考线信息，选最小cost的轨迹
（2）在变道场景下的目标车道参考线，用于长周期决策规划
（3）当无有效参考线时，清空历史轨迹并返回PLANNING_ERROR,触发停车
（4）plan规划的轨迹拼接最优参考线，确保轨迹连续性
（5）将规划的参考线与当前使用的参考线进行合并
（6）将车道线信息以及路 权，车道线信息、轨迹类型等传入ptr_trajectory_pb
（7）将轨迹数据转换为 Protobuf 格式，轨迹发布

代码解释

Status OnLanePlanning::Plan(const double current_time_stamp, const std::vector<TrajectoryPoint>& stitching_trajectory, ADCTrajectory* const ptr_trajectory_pb) {

  auto* ptr_debug = ptr_trajectory_pb->mutable_debug();
  if (FLAGS_enable_record_debug) {
    ptr_debug->mutable_planning_data()->mutable_init_point()->CopyFrom(
        stitching_trajectory.back());
    frame_->mutable_open_space_info()->set_debug(ptr_debug);
    frame_->mutable_open_space_info()->sync_debug_instance();
  }
  // 通过planner_->Plan生成轨迹，支持混合A*算法与PublicRoadPlanner
  // stitching_trajectory.back()：历史轨迹末端点，确保新轨迹与历史轨迹平滑衔接 
  // frame_.get()：包含本周期所有上下文数据（障碍物、定位、参考线等）
  // front_clear_distance：写入车辆前方安全距离，用于控制模块的碰撞检测
  // 开放空间 ： 调用HybridAStar生成粗轨迹，通过OBCA优化
  // 常规道路：使用PublicRoadPlanner进行车道级路径-速度解耦规划
  // planner_的定义在modules/planning/planning_interface_base/planner_base planner_-> PlannerWithReferenceLine -> PublicRoadPlanner
  auto status = planner_->Plan(stitching_trajectory.back(), frame_.get(), ptr_trajectory_pb);

  ptr_debug->mutable_planning_data()->set_front_clear_distance(injector_->ego_info()->front_clear_distance());
  // 获取可发布的轨迹数据
  if (frame_->open_space_info().is_on_open_space_trajectory()) {//判断是否是开放道路，如果是开放空间则需要使用混合A*算法生成初始轨迹
    frame_->mutable_open_space_info()->sync_debug_instance(); // 将开放空间规划器的内部状态（如QP求解器迭代次数、碰撞检测结果）同步到debug_instance_调试信息中，用于可视化工具分析
    
    const auto& publishable_trajectory = frame_->open_space_info().publishable_trajectory_data().first;// 存储经过平滑处理后的轨迹数据，包含位置、速度、曲率等参数，由DL-IAPS算法生成 的最终轨迹点序列。
    const auto& publishable_trajectory_gear = frame_->open_space_info().publishable_trajectory_data().second; //车辆档位状态（前进/倒车），这在泊车场景中频繁切换

    const auto& trajectory_type = frame_->open_space_info().trajectory_type(); //设置轨迹类型 OPEN_SPACE
    const auto& is_collision = frame_->open_space_info().is_collision(); //碰撞检测标记，若为 true 将触发紧急制动或路径重规划
    // 将轨迹数据结构序列化为 Protobuf 格式，用于跨模块通信
    publishable_trajectory.PopulateTrajectoryProtobuf(ptr_trajectory_pb);
    ptr_trajectory_pb->set_gear(publishable_trajectory_gear); // set_gear()设置R档/D档，适应倒车入库等需求
    ptr_trajectory_pb->set_trajectory_type(trajectory_type);//设置trajectory_type区分泊入、泊出等子场景 倒车入库：设置为GEAR_REVERSE，激活后轮转向控制逻辑 前进泊出：保持GEAR_DRIVE，匹配常规控制策略
    ptr_trajectory_pb->set_is_collision(is_collision); // is_collision标志触发紧急制动或重新规划

    // TODO(QiL): refine engage advice in open space trajectory optimizer. 在开放空间轨迹优化器中优化参与建议
    auto* engage_advice = ptr_trajectory_pb->mutable_engage_advice();

    // enable start auto from open_space planner.人机交互建议
    if (injector_->vehicle_state()->vehicle_state().driving_mode() !=
        Chassis::DrivingMode::Chassis_DrivingMode_COMPLETE_AUTO_DRIVE) {
      engage_advice->set_advice(EngageAdvice::READY_TO_ENGAGE); // 提示READY_TO_ENGAGE，要求用户准备接管（如泊车开始前的确认）
      engage_advice->set_reason("Ready to engage when staring with OPEN_SPACE_PLANNER");// 状态说明
    } else {
      engage_advice->set_advice(EngageAdvice::KEEP_ENGAGED);// 维持KEEP_ENGAGED，确保系统持续控制
      engage_advice->set_reason("Keep engage while in parking");
    }
    // TODO(QiL): refine the export decision in open space info
    // 标记当前主决策为“泊车中”，通知控制模块启用低速控制策略（如限制最大速度、增加制动灵敏度）
    ptr_trajectory_pb->mutable_decision()
        ->mutable_main_decision()
        ->mutable_parking()
        ->set_status(MainParking::IN_PARKING);

    if (FLAGS_enable_record_debug) {
      // ptr_debug->MergeFrom(frame_->open_space_info().debug_instance());
      frame_->mutable_open_space_info()->RecordDebug(ptr_debug);
      ADEBUG << "Open space debug information added!";
      // call open space info load debug
      // TODO(Runxin): create a new flag to enable openspace chart
      ExportOpenSpaceChart(ptr_trajectory_pb->debug(), *ptr_trajectory_pb,
                           ptr_debug);
    }
  } else { //常规马路
    // FindDriveReferenceLineInfo()：从Frame对象中获取当前最优驾驶参考线信息，cost已经计算碗了，只需要选择最小即可
    const auto* best_ref_info = frame_->FindDriveReferenceLineInfo();
    // FindTargetReferenceLineInfo()：在变道场景下获取目标车道的参考线，用于长周期决策 
    const auto* target_ref_info = frame_->FindTargetReferenceLineInfo();
    if (!best_ref_info) { //当无有效参考线时，清空历史轨迹并返回PLANNING_ERROR,触发停车
      const std::string msg = "planner failed to make a driving plan";
      AERROR << msg;
      if (last_publishable_trajectory_) {
        last_publishable_trajectory_->Clear();
      }
      return Status(ErrorCode::PLANNING_ERROR, msg);
    }
    // Store current frame stitched path for possible speed fallback in next
    // frames 将历史轨迹与当前参考线路径（best_ref_path）拼接，确保控制模块的轨迹连续性
    DiscretizedPath current_frame_planned_path;
    // stitching_trajectory：历史轨迹的末端点，用于与当前规划路径平滑衔接，避免急转向
    for (const auto& trajectory_point : stitching_trajectory) {
      current_frame_planned_path.push_back(trajectory_point.path_point());
    }
    //best_ref_path：最优参考线，通过std::copy合并至规划路径   规划的轨迹拼接最有参考线，确保轨迹连续性
    const auto& best_ref_path = best_ref_info->path_data().discretized_path();
    std::copy(best_ref_path.begin() + 1, best_ref_path.end(),
              std::back_inserter(current_frame_planned_path));
    frame_->set_current_frame_planned_path(current_frame_planned_path);
    // 调试信息整合
    ptr_debug->MergeFrom(best_ref_info->debug());
    // 调试图表导出控制
    if (FLAGS_export_chart) {
      ExportOnLaneChart(best_ref_info->debug(), ptr_debug);
    } else {
      ExportReferenceLineDebug(ptr_debug);
      // Export additional ST-chart for failed lane-change speed planning
      const auto* failed_ref_info = frame_->FindFailedReferenceLineInfo();
      if (failed_ref_info) {
        ExportFailedLaneChangeSTChart(failed_ref_info->debug(), ptr_debug);
      }
    }
    // latency_stats：记录各子模块耗时（如参考线生成、路径优化、速度决策）。
    ptr_trajectory_pb->mutable_latency_stats()->MergeFrom(best_ref_info->latency_stats());
    // set right of way status
    ptr_trajectory_pb->set_right_of_way_status(best_ref_info->GetRightOfWayStatus());
    //  车道级导航信息传递
    for (const auto& id : best_ref_info->TargetLaneId()) { // lane_id：当前参考线关联的车道ID序列，用于HMI显示车道保持状态。
      ptr_trajectory_pb->add_lane_id()->CopyFrom(id);
    }

    for (const auto& id : target_ref_info->TargetLaneId()) {// target_lane_id：目标参考线的车道ID（变道场景下显示目标车道）。
      ptr_trajectory_pb->add_target_lane_id()->CopyFrom(id);
    }
    // 轨迹类型标识 NORMAL：正常车道跟随。 LANE_CHANGE：执行变道动作。 PARKING：泊车场景轨迹。
    ptr_trajectory_pb->set_trajectory_type(best_ref_info->trajectory_type());

    if (FLAGS_enable_rss_info) { // RSS安全信息注入
      *ptr_trajectory_pb->mutable_rss_info() = best_ref_info->rss_info();
    }
    // 决策数据导出
    best_ref_info->ExportDecision(ptr_trajectory_pb->mutable_decision(), injector_->planning_context());

    // Add debug information.
    if (FLAGS_enable_record_debug) {
      auto* reference_line = ptr_debug->mutable_planning_data()->add_path();
      reference_line->set_name("planning_reference_line");
      const auto& reference_points =
          best_ref_info->reference_line().reference_points();
      double s = 0.0;
      double prev_x = 0.0;
      double prev_y = 0.0;
      bool empty_path = true;
      for (const auto& reference_point : reference_points) {
        auto* path_point = reference_line->add_path_point();
        path_point->set_x(reference_point.x());
        path_point->set_y(reference_point.y());
        path_point->set_theta(reference_point.heading());
        path_point->set_kappa(reference_point.kappa());
        path_point->set_dkappa(reference_point.dkappa());
        if (empty_path) {
          path_point->set_s(0.0);
          empty_path = false;
        } else {
          double dx = reference_point.x() - prev_x;
          double dy = reference_point.y() - prev_y;
          s += std::hypot(dx, dy);
          path_point->set_s(s);
        }
        prev_x = reference_point.x();
        prev_y = reference_point.y();
      }
    }

    last_publishable_trajectory_.reset(new PublishableTrajectory(
        current_time_stamp, best_ref_info->trajectory()));
    PrintCurves debug_traj;
    for (size_t i = 0; i < last_publishable_trajectory_->size(); i++) {
      auto& traj_pt = last_publishable_trajectory_->at(i);
      debug_traj.AddPoint("traj_sv", traj_pt.path_point().s(), traj_pt.v());
      debug_traj.AddPoint("traj_sa", traj_pt.path_point().s(), traj_pt.a());
      debug_traj.AddPoint("traj_sk", traj_pt.path_point().s(),
                          traj_pt.path_point().kappa());
    }
    // debug_traj.PrintToLog();
    ADEBUG << "current_time_stamp: " << current_time_stamp;

    last_publishable_trajectory_->PrependTrajectoryPoints(
        std::vector<TrajectoryPoint>(stitching_trajectory.begin(),
                                     stitching_trajectory.end() - 1));
    // 将轨迹数据转换为 Protobuf 格式
    last_publishable_trajectory_->PopulateTrajectoryProtobuf(ptr_trajectory_pb);

    best_ref_info->ExportEngageAdvice(
        ptr_trajectory_pb->mutable_engage_advice(),
        injector_->planning_context());
  }

  return status;
}