Apollo10.0学习——planning模块（8）之scenario、Stage插件详解二

插件总览

planning模块对于scenario的切换的代码是在scenario_manager中实现的，目前apollo一共支持了11多种场景和场景的定义。

• LaneFollowscenario:默认驾驶场景，包括本车道保持、变道、基本转弯

• TrafficLightProtectedScenario 有保护交通灯，即有明确的交通指示灯（左转、右转），是有路权保护的红绿灯场景，在该场景下可以实现在红绿灯路口前红灯停车，绿灯通过路口。

• EmergencyStopScenario: 紧急停车场景，车辆在行驶过程中如果收到PadMessage命令“PadMessage::STOP”，主车计算停车距离，直接停车。

• ParkAndGoScenario :从车位出库到路线上，用于车辆在远离终点且静止条件下，在非城市车道或匹配不到道路点的位置，通过freespace规划，实现车辆由开放空间驶入道路的功能。

• ValetParkingScenario可以在停车区域泊入指定的车位。

• PullOverScenario: 靠边停车场景， enable_pull_over_at_destination 设置为 true时, 当车辆到达终点附近时，将自动切入 PullOverScenario 并完成靠边停车。

• BareIntersectionUnprotectedScenario: 无保护裸露交叉路口场景，在交通路口既没有停止标志，也没有交通灯，车辆在路口前一段距离范围内切换到此场景。

• EmergencyPullOverScenario: 紧急靠边停车场景，车辆在行驶过程中如果收到PadMessage命令“PULL_OVER”，车辆就近找到合适的位置在当前车道内停车，相比于直接停车，这样保证了行驶过程中的停车安全性和舒适性。

• StopSignUnprotectedScenario无保护停止标志，场景可以在高精地图中有停止标记的路口时停车，观望周边车辆，等待周围车辆驶离后跛行，再快速通过路口。

• TrafficLightUnprotectedLeftTurnScenario 是没有路权保护的红绿灯左转场景。在该场景下，主车在左转车道线上

• TrafficLightUnprotectedRightTurnScenario 是有路权保护的红绿灯右转场景，在该场景下可以实现在红绿灯路口前红灯停车，绿灯通过路口。

• YieldSignScenario场景可以在有让行标记的场景减速观望，然后慢速通过。

插件ValetParkingScenario

ValetParkingScenario可以在停车区域泊入指定的车位。

场景切入条件

1. planning command里存在泊车命令
2. 距离泊车点距离parking_spot_range_to_start以内

1. 检查泊车指令有效性

if (!frame.local_view().planning_command->has_parking_command()) {
  return false;  // 无泊车指令时阻断场景切换
}

• 功能：验证规划指令中是否包含泊车命令。泊车指令通常由路由请求（Routing Request）触发，是进入代客泊车场景的必要条件。
• 设计意图：确保车辆仅在用户明确请求泊车操作时激活该场景。

---

2. 输入参数校验

if (other_scenario == nullptr || frame.reference_line_info().empty()) {
  return false;  // 上下文或参考线缺失时退出
}

• 意义：
  • other_scenario 为空表示无前序场景上下文，可能导致状态错误。
  • 参考线缺失意味着无法进行路径规划，需终止场景切换。

---

3. 解析目标停车位 ID

std::string target_parking_spot_id;
if (frame.local_view().planning_command->has_parking_command() &&
    frame.local_view().planning_command->parking_command().has_parking_spot_id()) {
  // 从指令中提取停车位 ID
  target_parking_spot_id = ...;
} else {
  ADEBUG << "No parking space id from routing";
  return false;
}

• 功能：从泊车指令中获取目标停车位的唯一标识符（如 parking_spot_id）。
• 异常处理：若 ID 缺失或为空，终止切换并记录调试日志。

---

4. 搜索目标停车位

const auto& nearby_path = frame.reference_line_info().front().reference_line().map_path();
PathOverlap parking_space_overlap;
if (!SearchTargetParkingSpotOnPath(nearby_path, target_parking_spot_id, &parking_space_overlap)) {
  ADEBUG << "No such parking spot found...";
  return false;  // 停车位不在当前路径上则退出
}

• SearchTargetParkingSpotOnPath 逻辑：
  1. 沿参考线遍历地图路径（map_path），匹配指定 ID 的停车位。
  2. 若找到则记录其覆盖区域（parking_space_overlap）。
• 设计关联：依赖高精地图数据（HDMap）实现停车位定位，确保泊车路径可达性。

---

5. 检查距离条件

double parking_spot_range_to_start = context_.scenario_config.parking_spot_range_to_start();
if (!CheckDistanceToParkingSpot(frame, vehicle_state, nearby_path,
                                parking_spot_range_to_start,
                                parking_space_overlap)) {
  ADEBUG << "target parking spot too far...";
  return false;
}

• CheckDistanceToParkingSpot 逻辑推测：
  1. 计算自车当前位置到目标停车位起点（start_s）的纵向距离。
  2. 若距离超过配置参数 parking_spot_range_to_start（如 50米），判定为过远。
• 参数来源：parking_spot_range_to_start 来自 scenario_conf.pb.txt 配置文件。

---

6. 更新场景上下文

context_.target_parking_spot_id = target_parking_spot_id;
return true;  // 满足所有条件，允许切换至代客泊车场景

• 上下文作用：
  • 存储目标停车位 ID，供后续阶段（如 StageApproachingParkingSpot）使用。
  • 支持跨阶段状态传递，例如生成泊车路径时需基于此 ID 查询地图数据。

---

关联设计总结

1. 场景切换条件

   • 泊车指令存在性：通过路由服务明确触发场景切换。
   • 停车位可达性：需在参考线路径范围内且距离合理（避免远距离误触发）。
   • 地图数据依赖：依赖高精地图的停车位标注信息实现精准定位。

2. 安全冗余机制

   • 距离阈值校验：防止车辆过早进入泊车场景导致路径规划复杂化。
   • 空值防御：对 other_scenario 和 reference_line_info 的空指针检查避免运行时崩溃。

3. 调试与日志

   • 使用 ADEBUG 记录关键判定结果（如停车位未找到或距离过远），便于问题排查。
   • 上下文更新后返回 true，触发后续阶段的初始化。

---

典型应用场景

• 停车场入口触发：用户选择目标停车位后，车辆行驶至预设距离范围时激活代客泊车场景。
• 动态路径调整：若车辆偏离参考线导致停车位丢失，自动退出场景并等待重新触发。

---

阶段一：StageApproachingParkingSpot

阶段：接近停车位阶段

process()方法

1. 阶段标识与输入校验

ADEBUG << "stage: StageApproachingParkingSpot";  // 标记当前为接近停车位阶段
CHECK_NOTNULL(frame);  // 确保帧数据指针有效性 
StageResult result;    // 初始化阶段结果对象

• 功能：标识当前处于 Valet Parking 场景的 Approaching Parking Spot 阶段，验证输入数据结构合法性。

---

2. 上下文有效性校验

auto scenario_context = GetContextAs<ValetParkingContext>();
if (scenario_context->target_parking_spot_id.empty()) {
  return result.SetStageStatus(StageStatusType::ERROR);  // 无目标停车位ID时返回错误
}

• 设计意图：确保上下文中的停车位 ID 有效。ValetParkingContext 包含 target_parking_spot_id 字段用于标识目标停车位 。

---

3. 设置开放空间信息

// 将上下文中的停车位ID传递至当前帧
*(frame->mutable_open_space_info()->mutable_target_parking_spot_id()) =
    scenario_context->target_parking_spot_id;

// 同步预停止标志及坐标点
frame->mutable_open_space_info()->set_pre_stop_rightaway_flag(
    scenario_context->pre_stop_rightaway_flag);
*(frame->mutable_open_space_info()->mutable_pre_stop_rightaway_point()) =
    scenario_context->pre_stop_rightaway_point;

• 字段意义：
  • pre_stop_rightaway_flag：标记是否需要立即预停车（如检测到障碍物）。
  • pre_stop_rightaway_point：预停车目标点坐标 。
• 作用：传递跨阶段状态，支持路径规划模块动态调整行为 。

---

4. 参考线遍历与障碍物处理

auto* reference_lines = frame->mutable_reference_line_info();
for (auto& reference_line : *reference_lines) {
  auto* path_decision = reference_line.path_decision();
  if (nullptr == path_decision) continue;

  // 查找并处理目的地虚拟障碍物（ID为FLAGS_destination_obstacle_id）
  auto* dest_obstacle = path_decision->Find(FLAGS_destination_obstacle_id);
  if (nullptr == dest_obstacle) continue;

  ObjectDecisionType decision;
  decision.mutable_ignore();  // 设置忽略决策
  dest_obstacle->EraseDecision();
  dest_obstacle->AddLongitudinalDecision("ignore-dest-in-valet-parking", decision);
}

• 关键逻辑：
  • 虚拟障碍物忽略：代客泊车场景需忽略全局路径终点标识的虚拟障碍物（如 DESTINATION 标签），避免误触发停车决策 。
  • 决策标签："ignore-dest-in-valet-parking" 标识此操作场景，便于调试追踪 。

---

5. 执行参考线任务链

result = ExecuteTaskOnReferenceLine(planning_init_point, frame);

• 任务链内容：
  1. 路径决策：生成避障路径（如 PathDecider）。
  2. 速度优化：调用 SpeedOptimizer 生成平滑速度曲线。
  3. 轨迹校验：验证轨迹是否符合车辆动力学约束 。

---

6. 同步预停止状态

scenario_context->pre_stop_rightaway_flag =
    frame->open_space_info().pre_stop_rightaway_flag();
scenario_context->pre_stop_rightaway_point =
    frame->open_space_info().pre_stop_rightaway_point();

• 双向同步：确保上下文与帧数据中的预停止状态一致，支持跨阶段连续性 。

---

7. 阶段退出条件判断

if (CheckADCStop(*frame)) {  // 检查车辆是否完全停止
  next_stage_ = "VALET_PARKING_PARKING";  // 进入停车阶段
  return StageResult(StageStatusType::FINISHED);  // 当前阶段完成
}

if (result.HasError()) {  // 任务链执行异常
  AERROR << "StopSignUnprotectedStagePreStop planning error";
  return result.SetStageStatus(StageStatusType::ERROR);
}

• CheckADCStop：
  • 可能校验车速为零且横向偏移在阈值内。
  • 满足条件时切换至 VALET_PARKING_PARKING 阶段（实际泊入操作） 。
• 错误处理：路径规划失败时记录日志并返回错误状态 。

---

8. 默认状态返回

return result.SetStageStatus(StageStatusType::RUNNING);  // 继续执行当前阶段

• RUNNING 状态处理：持续优化轨迹直至满足停车条件。

---

设计关联与关键机制

1. 虚拟障碍物管理

   • 通过忽略全局路径终点障碍物，避免代客泊车场景与常规路径规划的冲突 。
   • 标签化决策（ignore-dest-in-valet-parking）增强调试可追溯性 。

2. 跨阶段状态同步

   • 上下文（ValetParkingContext）与帧数据（OpenSpaceInfo）双向同步，确保路径规划的连贯性 。

3. 安全冗余设计

   • 双重校验（停车位 ID 非空、车辆停止状态）防止场景误切换。
   • 动态更新预停止点支持紧急避障场景 。

---

代码解释：

StageResult StageApproachingParkingSpot::Process(
    const common::TrajectoryPoint& planning_init_point, Frame* frame) {
  ADEBUG << "stage: StageApproachingParkingSpot";// 标记当前为接近停车位阶段
  CHECK_NOTNULL(frame);
  StageResult result;
  auto scenario_context = GetContextAs<ValetParkingContext>();
  // 无目标停车位ID时返回错误
  if (scenario_context->target_parking_spot_id.empty()) {
    return result.SetStageStatus(StageStatusType::ERROR);
  }
  // 将上下文中的停车位ID传递至当前帧
  *(frame->mutable_open_space_info()->mutable_target_parking_spot_id()) =
      scenario_context->target_parking_spot_id;
  // 同步预停止标志及坐标点
  frame->mutable_open_space_info()->set_pre_stop_rightaway_flag(
      scenario_context->pre_stop_rightaway_flag);// 标记是否需要立即预停车（如检测到障碍物）。
  *(frame->mutable_open_space_info()->mutable_pre_stop_rightaway_point()) =
      scenario_context->pre_stop_rightaway_point;//预停车目标点坐标 。
  auto* reference_lines = frame->mutable_reference_line_info();
  // 参考线遍历与障碍物处理
  for (auto& reference_line : *reference_lines) {
    auto* path_decision = reference_line.path_decision();
    if (nullptr == path_decision) {
      continue;
    }
    // 查找并处理目的地虚拟障碍物（ID为FLAGS_destination_obstacle_id）
    auto* dest_obstacle = path_decision->Find(FLAGS_destination_obstacle_id);
    if (nullptr == dest_obstacle) {
      continue;
    }
    ObjectDecisionType decision;
    decision.mutable_ignore(); // 设置忽略决策
    dest_obstacle->EraseDecision();
    dest_obstacle->AddLongitudinalDecision("ignore-dest-in-valet-parking",
                                           decision);
  }
  // 执行参考线任务链
  result = ExecuteTaskOnReferenceLine(planning_init_point, frame);
// 同步预停止状态
  scenario_context->pre_stop_rightaway_flag =
      frame->open_space_info().pre_stop_rightaway_flag();
  scenario_context->pre_stop_rightaway_point =
      frame->open_space_info().pre_stop_rightaway_point();

  if (CheckADCStop(*frame)) { // 检查车辆是否完全停止
    next_stage_ = "VALET_PARKING_PARKING";// 进入停车阶段
    return StageResult(StageStatusType::FINISHED);//当前阶段完成
  }
  if (result.HasError()) {
    AERROR << "StopSignUnprotectedStagePreStop planning error";
    return result.SetStageStatus(StageStatusType::ERROR);
  }

  return result.SetStageStatus(StageStatusType::RUNNING);
}

阶段二：StageParking

process()方法

以下是对 StageParking::Process 函数的逐行解释，结合 Apollo 代客泊车场景逻辑及搜索结果内容：

---

1. 上下文注释说明

// Open space planning doesn't use planning_init_point from upstream because
// of different stitching strategy

• 设计意图：开放空间规划（如泊车场景）采用独立的轨迹拼接策略，与常规结构化道路的轨迹生成逻辑不同。例如，开放空间需处理车辆静止启动、倒车轨迹生成等特殊工况。

---

2. 获取场景上下文

auto scenario_context = GetContextAs<ValetParkingContext>();

• 功能：获取代客泊车场景的上下文对象 ValetParkingContext，包含目标停车位 ID、预停止状态等跨阶段共享数据。
• 数据关联：上下文中的 target_parking_spot_id 在 ValetParkingScenario::IsTransferable 阶段已通过停车位搜索和距离校验。

---

3. 设置开放空间信息

frame->mutable_open_space_info()->set_is_on_open_space_trajectory(true);  // 标记为开放空间模式
*(frame->mutable_open_space_info()->mutable_target_parking_spot_id()) =
    scenario_context->target_parking_spot_id;  // 传递目标停车位ID

• 作用：
  • 模式切换：启用开放空间规划算法（如 Hybrid A* 和 RS 曲线），支持非结构化道路的轨迹生成。
  • 停车位标识：确保路径规划模块能根据 ID 查询高精地图中的停车位几何信息。

---

4. 执行开放空间任务链

StageResult result = ExecuteTaskOnOpenSpace(frame);

• 任务链内容：
  1. 可行驶区域构建：基于停车位多边形生成可行区域（OpenSpaceRoiDecider）。
  2. 路径搜索：使用 Hybrid A* 算法生成初始避障轨迹。
  3. 轨迹优化：调用 OpenSpaceTrajectoryOptimizer 结合车辆动力学参数优化轨迹平滑性。
  4. 档位决策：根据轨迹方向（前进/倒车）设置档位切换点。
  5. 碰撞检测：验证轨迹与障碍物无冲突。

---

5. 错误处理与状态返回

if (result.HasError()) {
  AERROR << "StageParking planning error";  // 记录错误日志
  return result.SetStageStatus(StageStatusType::ERROR);  // 终止阶段
}
return result.SetStageStatus(StageStatusType::RUNNING);  // 继续执行

• 错误类型：
  • 路径搜索失败：可能因停车位尺寸不足或障碍物封锁。
  • 优化约束冲突：如曲率超过车辆最大转向能力。
• 状态流转：持续返回 RUNNING 直至车辆完全停入车位（需外部条件触发）。

---

关联设计参数与配置

1. 轨迹优化配置

   • open_space_trajectory_optimizer_config：控制优化器参数（如权重系数、迭代次数）。
   • open_space_standstill_acceleration：设置车辆静止时的加速度阈值（防止误判为移动）。

2. 轨迹拼接策略

   • open_space_trajectory_stitching_preserved_length：保留历史轨迹的拼接长度，确保控制模块平滑过渡。

---

典型应用场景

• 垂直泊车：车辆根据目标停车位 ID 生成倒车轨迹，分阶段调整方向直至对齐车位中线。
• 动态避障：泊车过程中检测到临时障碍物时，重新规划轨迹绕过障碍物。

---

与其他阶段的协同

• 前置阶段：StageApproachingParkingSpot 负责将车辆引导至停车位附近并触发预停止。
• 后置逻辑：当检测到车辆完全停入车位（如横向偏移 <0.3米），切换至结束状态并退出泊车场景。

---

代码解释：

StageResult StageParking::Process(
    const common::TrajectoryPoint& planning_init_point, Frame* frame) {
  // Open space planning doesn't use planning_init_point from upstream because
  // of different stitching strategy
  // 获取场景上下文
  auto scenario_context = GetContextAs<ValetParkingContext>();
  frame->mutable_open_space_info()->set_is_on_open_space_trajectory(true);// 标记为开放空间模式
  *(frame->mutable_open_space_info()->mutable_target_parking_spot_id()) =
      scenario_context->target_parking_spot_id;// 传递目标停车位ID
  // 执行开放空间任务链
  StageResult result = ExecuteTaskOnOpenSpace(frame);
  if (result.HasError()) {
    AERROR << "StageParking planning error";
    return result.SetStageStatus(StageStatusType::ERROR);
  }
  return result.SetStageStatus(StageStatusType::RUNNING);
}

FinishStage方法

StageResult StageParking::FinishStage() {
  return StageResult(StageStatusType::FINISHED);
}

插件PullOverScenario

PullOverScenario: 靠边停车场景，如果参数配置 enable_pull_over_at_destination 设置为 true, 当车辆到达终点附近时，将自动切入 PullOverScenario 并完成靠边停车。

IsTransferable: 场景切入条件

1. 当前command为lane_follow_command
2. 参考线信息不为空
3. FLAGS_enable_pull_over_at_destination 参数配置允许靠边停车场景
4. 主车不处于变道状态
5. 主车距离目标点满足靠边停车距离阈值
6. 不处于overlap
7. 最右侧车道允许靠边停车

代码逻辑

1. 基础条件校验

if (!frame.local_view().planning_command->has_late_follow_command()) {
  return false;  // 当前规划指令非车道跟随模式时阻断场景切换
}
if (other_scenario == nullptr || frame.reference_line_info().empty()) {
  return false;  // 上下文或参考线缺失时退出
}
if (!FLAGS_enable_pull_over_at_destination) {
  return false;  // 全局参数禁用靠边停车功能则退出
}

• 关键逻辑：
  • 指令校验：仅当车辆处于车道跟随模式（lane_follow_command）时才允许切换场景。
  • 配置参数：FLAGS_enable_pull_over_at_destination 控制是否启用终点靠边停车功能[[1][63]]。

---

2. 终点位置有效性检查

const auto routing_end = frame.local_view().end_lane_way_point;
if (nullptr == routing_end) {
  return false;  // 路由终点无效时退出
}
common::SLPoint dest_sl;
reference_line.XYToSL(routing_end->pose(), &dest_sl);
if (!reference_line.IsOnLane(dest_sl)) {
  return false;  // 终点不在当前车道内则阻断
}

• 设计意图：
  • 确保导航终点位于当前参考线上，避免车辆在无车道区域内尝试停车[[48][63]]。
  • 坐标系转换：将全局坐标（XY）转换为参考线坐标系（SL）以进行纵向距离计算。

---

3. 纵向距离校验

const double adc_front_edge_s = reference_line_info.AdcSlBoundary().end_s();
const double adc_distance_to_dest = dest_sl.s() - adc_front_edge_s;

bool pull_over_scenario = (
  frame.reference_line_info().size() == 1 &&  // 禁止变道中切入
  adc_distance_to_dest >= context_.scenario_config.pull_over_min_distance_buffer() &&
  adc_distance_to_dest <= context_.scenario_config.start_pull_over_scenario_distance()
);

• 参数意义[[48][63]]：
  • pull_over_min_distance_buffer：最小触发距离（如 10 米），防止车辆过早触发。
  • start_pull_over_scenario_distance：最大触发距离（如 50 米），超出范围不激活场景。
• 限制条件：参考线数量为 1 保证车辆不处于变道状态。

---

4. 终点过近处理

if (adc_distance_to_dest < context_.scenario_config.max_distance_stop_search()) {
  pull_over_scenario = false;  // 终点过近时停止搜索停车位
}

• 逻辑说明：当车辆距离终点过近（如 <5 米）时，无法找到有效的靠边停车区域，强制退出场景。

---

5. 避开交通设施区域

constexpr double kDistanceToAvoidJunction = 8.0;  // 禁止在交叉口附近停车
for (const auto& overlap : first_encountered_overlaps) {
  if (overlap.first 为 PNC_JUNCTION/SIGNAL/STOP_SIGN/YIELD_SIGN) {
    // 计算与交通设施的相对距离
    if (距离在 kDistanceToAvoidJunction 内) {
      pull_over_scenario = false;
      break;
    }
  }
}

• 安全设计：禁止在交叉口、信号灯、停止标志等区域附近停车，避免阻碍交通。

---

6. 右侧车道合法性校验

while (check_s < dest_sl.s()) {
  reference_line.GetLaneFromS(check_s, &lanes);
  // 检查右侧相邻车道类型
  for (const auto& neighbor_lane_id : lane->right_neighbor_forward_lane_id()) {
    if (neighbor_lane->type() == CITY_DRIVING) {
      rightmost_driving_lane = false;  // 右侧存在可行驶车道则禁止停车
      break;
    }
  }
}

• 规则依据：仅允许在最右侧城市驾驶车道（CITY_DRIVING）进行靠边停车，右侧若存在同类型车道说明当前车道非最右侧[[48][63]]。
• 动态检测：沿路径分段校验，确保全程右侧无合法行驶车道。

---

Stages

阶段名	类型	描述
PULL_OVER_APPROACH	apollo::planning::PullOverStageApproach	主车靠近靠边停车点
PULL_OVER_RETRY_APPROACH_PARKING	apollo::planning::PullOverStageRetryApproachParking	接近Parking位置点，主车速度、距离误差达到阈值后，进入下一个stage
PULL_OVER_RETRY_PARKING	apollo::planning::PullOverStageRetryParking	执行openspace的轨迹规划，主车位置、航向达到阈值后，退出该stage

---

阶段一：PullOverStageApproach

PullOverStageApproach : 该阶段用于主车规划接近靠边停车点，获取靠边停车轨迹，并检查是否完成靠边停车。

Process方法

Process: 该阶段处理的主函数，输入为规划初始点 planning_init_point、Frame；输出为当前阶段处理状态StageResult

• ExecuteTaskOnReferenceLine：输入为规划初始点planning_init_point、Frame信息，按照该stage配置的task列表，依次进行规划。
• CheckADCPullOver：检查主车的PullOver状态。输入主车状态、参考线信息、场景信息和规划上下文信息，根据主车当前位置和速度，判断与停靠点关系，确定主车PullOverState。状态返回值分为: UNKNOWN, PASS_DESTINATION, APPROACHING, PARK_COMPLETE 和 PARK_FAIL。如果完成靠边停车，即状态为 PASS_DESTINATION 或 PARK_COMPLETE ，则进入FinishStage，结束当前Stage，并且退出当前PullOverScenario；如果靠边停车失败，即状态为PARK_FAIL，则进入FinishStage，结束当前Stage，进入PULL_OVER_RETRY_APPROACH_PARKING阶段。
• CheckADCPullOverPathPoint：如果当前仍处于靠边停车阶段，检查关键path_point，根据path_point与停靠点的位置和heading偏差，判断是否path_fail。如果path_fail==true, 在未到达停靠点前设置STOP的虚拟障碍物。主车到达虚拟障碍物后，进入FinishStage，结束当前Stage，进入PULL_OVER_RETRY_APPROACH_PARKING阶段。如果path_fail==false，则仍处于PULL_OVER_APPROACH阶段。

FinishStage: 该阶段的退出函数，输入为bool success，即该阶段是否靠边停车成功。

• 如果success==true，退出PULL_OVER_APPROACH阶段，并退出PullOverScenario。
• 如果success==false，退出PULL_OVER_APPROACH阶段，进入PULL_OVER_RETRY_APPROACH_PARKING阶段。

阶段二：PullOverStageRetryApproachParking

PullOverStageRetryApproachParking：上一阶段直接靠边停车失败，进入该阶段重试接近靠边停车点。

Process方法

Process: 该阶段处理的主函数，输入为规划初始点 planning_init_point、Frame；输出为当前阶段处理状态StageResult

• ExecuteTaskOnReferenceLine：输入为规划初始点planning_init_point、Frame信息，按照该stage配置的task列表，依次进行规划。
• CheckADCStop：检查主车是否靠近停车点，输入为Frame信息。主车满足速度小于阈值，位置距离规划设置的open_space_pre_stop_fence_s小于阈值，进入FinishStage，结束当前Stage。

FinishStage: 该阶段的退出函数。

• 退出PULL_OVER_RETRY_APPROACH_PARKING阶段，进入PULL_OVER_RETRY_PARKING阶段。

阶段三：PullOverStageRetryParking

PullOverStageRetryParking：上一阶段靠近停车点后，进入该阶段实线停车。

Process方法

Process: 该阶段处理的主函数，输入为规划初始点 planning_init_point、Frame；输出为当前阶段处理状态StageResult

• ExecuteTaskOnReferenceLine：输入为规划初始点planning_init_point、Frame信息，按照该stage配置的task列表，依次进行规划。该阶段主要时调用Openspace的轨迹规划方法进行靠边停车规划。
• CheckADCPullOverOpenSpace：检查主车是否停车，输入为Frame信息。主车与目标点的位置、航向偏差小于阈值，进入FinishStage，结束当前Stage。

FinishStage: 该阶段的退出函数。

• 退出PULL_OVER_RETRY_PARKING阶段，退出当前PullOverScenario。