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

插件总览

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

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

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

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

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

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

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

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

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

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

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

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

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

插件LaneFollowscenario

这个函数用于判断other_scenario是否能够转移到当前的LaneFollowScenario中。

首先，检查frame规划命令中是否存在车道跟随命令。如果不存在，函数返回false，不能转移。
然后，检查frame的参考线信息是否为空。如果为空，返回false，不能转移。
最后，检查other_scenario是否为空。如果为空，返回true，可以转移。

// 是否切换场景
bool LaneFollowScenario::IsTransferable(const Scenario* other_scenario,const Frame& frame) {if (!frame.local_view().planning_command->has_lane_follow_command()) {return false;}// 没有参考线，不切换场景if (frame.reference_line_info().empty()) {return false;}if (other_scenario == nullptr) {return true;}return true;
}

阶段一：LaneFollowStage

process()方法

在reference_line_info中寻找可驾驶的线路:

• 首先检查frame是否包含任何参考线路信息。如果没有，返回finish的StageResult。
• 然后，它遍历所有的参考线路。对于每一条参考线路，调用PlanOnReferenceLine方法来进行规划。
• 如果规划结果没有错误，它会检查这条参考线路是否需要进行车道变更（IsChangeLanePath）。如果不需要，将把这条参考线路标记为可驾驶，并继续处理下一条参考线路。
• 如果需要，检查这条参考线路的代价（Cost）是否小于不进行车道变更的代价（kStraightForwardLineCost）。如果是，那么它将把这条参考线路标记为可驾驶的。
• 否则，把这条参考线路标记为不可驾驶
• 如果规划结果有错误，那么把这条参考线路标记为不可驾驶。
• 在遍历完所有的参考线路后，如果找到了一条可驾驶的线路，返回RUNNING的StageResult。否则，返回ERROR的StageResult。

代码解释

StageResult LaneFollowStage::Process(const TrajectoryPoint& planning_start_point, Frame* frame) {//  检查frame是否包含任何参考线路信息   if (frame->reference_line_info().empty()) {return StageResult(StageStatusType::FINISHED);}bool has_drivable_reference_line = false;ADEBUG << "Number of reference lines:\t"<< frame->mutable_reference_line_info()->size();unsigned int count = 0;StageResult result;// 遍历所有参考线for (auto& reference_line_info : *frame->mutable_reference_line_info()) {// TODO(SHU): need refactorif (count++ == frame->mutable_reference_line_info()->size()) {break;}ADEBUG << "No: [" << count << "] Reference Line.";ADEBUG << "IsChangeLanePath: " << reference_line_info.IsChangeLanePath();if (has_drivable_reference_line) {reference_line_info.SetDrivable(false);break;}// 对于每一条参考线路，调用PlanOnReferenceLine方法来进行规划。result = PlanOnReferenceLine(planning_start_point, frame, &reference_line_info);// 如果规划结果没有错误，它会检查这条参考线路是否需要进行车道变更（IsChangeLanePath）。if (!result.HasError()) {if (!reference_line_info.IsChangeLanePath()) {ADEBUG << "reference line is NOT lane change ref.";// 如果不需要，将把这条参考线路标记为可驾驶，并继续处理下一条参考线路。has_drivable_reference_line = true;continue;}// 如果需要变道，检查这条参考线路的代价（Cost）是否小于不进行车道变更的代价（kStraightForwardLineCost）。if (reference_line_info.Cost() < kStraightForwardLineCost) {// If the path and speed optimization succeed on target lane while// under smart lane-change or IsClearToChangeLane under older version// 如果是，那么它将把这条参考线路标记为可驾驶的。has_drivable_reference_line = true;reference_line_info.SetDrivable(true);} else {// 否则，把这条参考线路标记为不可驾驶reference_line_info.SetDrivable(false);ADEBUG << "\tlane change failed";}} else {// 如果plan中发生错误，那么把这条参考线路标记为不可驾驶。reference_line_info.SetDrivable(false);}}// 在遍历完所有的参考线路后，如果找到了一条可驾驶的线路，返回RUNNING的StageResult。否则，返回ERROR的StageResult。return has_drivable_reference_line? result.SetStageStatus(StageStatusType::RUNNING): result.SetStageStatus(StageStatusType::ERROR);
}

PlanOnReferenceLine()方法

• 首先检查这条参考线路是否需要进行车道变更（IsChangeLanePath）。生成每一个参考线对应的代价，用于后续判断。
• reference_line_info是否变换车道。如果是，增加一个路径成本kStraightForwardLineCost
• 遍历任务列表task_list_中的每一个任务。对于每一个任务，打印执行任务的时间差。
• 如果任务执行出现错误（ret.IsTaskError()），那么会打印出错误信息并停止执行。
• 当前参考线的轨迹类型标记为 正常驾驶轨迹（NORMAL）。
• 然后，路径和速度信息组合成一条轨迹。如果失败，返回错误状态并打印出错误信息。planning_start_point.relative_time()：规划起点的时间偏移量（相对于系统时间）。planning_start_point.path_point().s()：规划起点的纵向位置（沿参考线的弧长）。
• 接下来，它检查是否有目标点（目的地）在参考线上。如果有，它会记录下这个目标点的s值（在参考线上的位置）。
• 然后，对于参考线上的每一个障碍物，如果障碍物是静态的并且有停止决策，它会检查这个障碍物的停止点是否在目标点之前。如果是，那增加一个障碍物成本kReferenceLineStaticObsCost。
• 如果启用了轨迹检查（通过FLAGS变量enable_trajectory_check），那么会检查轨迹是否有效。如果无效，那么会返回一个错误状态并打印出错误信息。
• 最后，它会设置参考线的轨迹和可驾驶状态，然后返回一个运行中的状态。

代码解释

StageResult LaneFollowStage::PlanOnReferenceLine( const TrajectoryPoint& planning_start_point, Frame* frame, ReferenceLineInfo* reference_line_info) {// 首先检查这条参考线路是否需要进行车道变更（IsChangeLanePath）。if (!reference_line_info->IsChangeLanePath()) {reference_line_info->AddCost(kStraightForwardLineCost);}ADEBUG << "planning start point:" << planning_start_point.DebugString();ADEBUG << "Current reference_line_info is IsChangeLanePath: "<< reference_line_info->IsChangeLanePath();StageResult ret;for (auto task : task_list_) {const double start_timestamp = Clock::NowInSeconds();const auto start_planning_perf_timestamp =std::chrono::duration<double>(std::chrono::system_clock::now().time_since_epoch()).count();ret.SetTaskStatus(task->Execute(frame, reference_line_info));const double end_timestamp = Clock::NowInSeconds();const double time_diff_ms = (end_timestamp - start_timestamp) * 1000;ADEBUG << "after task[" << task->Name() << "]:" << reference_line_info->PathSpeedDebugString();ADEBUG << task->Name() << " time spend: " << time_diff_ms << " ms.";RecordDebugInfo(reference_line_info, task->Name(), time_diff_ms);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_planning_perf_timestamp) * 1000;AINFO << "Planning Perf: task name [" << task->Name() << "], "<< plnning_perf_ms << " ms.";// 如果任务执行出现错误（ret.IsTaskError()），那么会打印出错误信息并停止执行。if (ret.IsTaskError()) {AERROR << "Failed to run tasks[" << task->Name()<< "], Error message: " << ret.GetTaskStatus().error_message();break;}// TODO(SHU): disable reference line order changes for now// updated reference_line_info, because it is changed in// lane_change_decider by PrioritizeChangeLane().// reference_line_info = &frame->mutable_reference_line_info()->front();// ADEBUG << "Current reference_line_info is IsChangeLanePath: "//        << reference_line_info->IsChangeLanePath();}RecordObstacleDebugInfo(reference_line_info);// check path and speed results for path or speed fallback// 当前参考线的轨迹类型标记为 正常驾驶轨迹（NORMAL）。reference_line_info->set_trajectory_type(ADCTrajectory::NORMAL);if (ret.IsTaskError()) {fallback_task_->Execute(frame, reference_line_info);}//将路径（PathData）和速度（SpeedData）数据合并为完整的离散化轨迹（DiscretizedTrajectory）// planning_start_point.relative_time()：规划起点的时间偏移量（相对于系统时间）。// planning_start_point.path_point().s()：规划起点的纵向位置（沿参考线的弧长）。DiscretizedTrajectory trajectory;if (!reference_line_info->CombinePathAndSpeedProfile(planning_start_point.relative_time(),planning_start_point.path_point().s(), &trajectory)) {const std::string msg = "Fail to aggregate planning trajectory.";AERROR << msg;return ret.SetStageStatus(StageStatusType::ERROR, msg);}// determine if there is a destination on reference line.double dest_stop_s = -1.0;//当前规划的目标停车点 s 坐标（如最近红绿灯停止线）。// 它检查是否有目标点（目的地）在参考线上for (const auto* obstacle : reference_line_info->path_decision()->obstacles().Items()) {// 如果有，它会记录下这个目标点的s值（在参考线上的位置）。if (obstacle->LongitudinalDecision().has_stop() &&obstacle->LongitudinalDecision().stop().reason_code() == STOP_REASON_DESTINATION) {//获取停止点的s值SLPoint dest_sl = GetStopSL(obstacle->LongitudinalDecision().stop(),reference_line_info->reference_line());dest_stop_s = dest_sl.s();}}for (const auto* obstacle : reference_line_info->path_decision()->obstacles().Items()) {// 交通规则生成的逻辑障碍物（如红绿灯停止线、人行横道边界），无需物理避撞处理// 虚拟障碍物的碰撞风险已通过规则决策处理，此处仅处理真实物理障碍物。if (obstacle->IsVirtual()) {continue;}// 位置固定的物体（如抛锚车辆），需在路径规划阶段直接避让。if (!obstacle->IsStatic()) {continue;}// 检查停车决策 障碍物在目标点之前if (obstacle->LongitudinalDecision().has_stop()) {bool add_stop_obstacle_cost = false;if (dest_stop_s < 0.0) {//无有效停车点（dest_stop_s < 0）：直接将障碍物设为停车目标。add_stop_obstacle_cost = true;// 无有效停车点时强制添加成本} else {//获取障碍物停车的s值SLPoint stop_sl = GetStopSL(obstacle->LongitudinalDecision().stop(),reference_line_info->reference_line());// 当障碍物的停车点（stop_sl.s()）比当前目标更近，且距离自车尾部（AdcSlBoundary.end_s()）在 20 米内时，优先处理该障碍物if (stop_sl.s() < dest_stop_s &&(dest_stop_s - reference_line_info->AdcSlBoundary().end_s()) <20.0) {// 障碍物停车点在当前目标前方且距离自车末端小于20米add_stop_obstacle_cost = true;}}// 1e3 是一个较高的固定成本，显著影响路径代价函数，使规划器优先避让该参考线if (add_stop_obstacle_cost) {static constexpr double kReferenceLineStaticObsCost = 1e3;reference_line_info->AddCost(kReferenceLineStaticObsCost);}}}// 如果启用了轨迹检查（通过FLAGS变量enable_trajectory_check），那么会检查轨迹是否有效。// 如果无效，那么会返回一个错误状态并打印出错误信息。if (FLAGS_enable_trajectory_check) {if (ConstraintChecker::ValidTrajectory(trajectory) != ConstraintChecker::Result::VALID) {const std::string msg = "Current planning trajectory is not valid.";AERROR << msg;return ret.SetStageStatus(StageStatusType::ERROR, msg);}}// 它会设置参考线的轨迹和可驾驶状态，然后返回一个运行中的状态。reference_line_info->SetTrajectory(trajectory);reference_line_info->SetDrivable(true);ret.SetStageStatus(StageStatusType::RUNNING);return ret;
}

---

插件TrafficLightProtectedScenario

IsTransferable()方法用于判断当前是否应从其他场景（如 LANE_FOLLOW）切换到 交通灯保护场景（TRAFFIC_LIGHT_PROTECTED），主要依据参考线上是否存在需处理的交通信号灯，并满足距离与信号状态的触发条件。
场景切入条件

• 检查frame规划命令中是否存在车道跟随命令。如果不存在，函数返回false，不能转移。
• 检查frame的参考线信息是否为空。如果为空，返回false，不能转移。
• 检查other_scenario是否为空。如果为空，返回false，不可以转移。
• 遍历交通灯，如果出现停车、让行等优先级更高的标志，则不切换。
• 无交通灯重叠区域，则不切换。
• 处理多个相邻交通灯（如复合信号灯），确保统一决策，红绿灯的间隔2m，认为是一组。
• 如果所有交通灯均是绿灯或者未知灯，则不切换。
• 否则切换，并记录交通灯ID。

代码解释

bool TrafficLightProtectedScenario::IsTransferable(const Scenario* const other_scenario, const Frame& frame) {if (!frame.local_view().planning_command->has_lane_follow_command()) {return false;}if (other_scenario == nullptr || frame.reference_line_info().empty()) {return false;}const auto& reference_line_info = frame.reference_line_info().front();const auto& first_encountered_overlaps = reference_line_info.FirstEncounteredOverlaps();hdmap::PathOverlap* traffic_sign_overlap = nullptr;// 若存在更高优先级的交通标志（如 STOP_SIGN），优先处理其对应场景for (const auto& overlap : first_encountered_overlaps) {if (overlap.first == ReferenceLineInfo::STOP_SIGN ||overlap.first == ReferenceLineInfo::YIELD_SIGN) {return false;// 优先处理停车标志/让行标志，阻断交通灯场景切换} else if (overlap.first == ReferenceLineInfo::SIGNAL) {traffic_sign_overlap = const_cast<hdmap::PathOverlap*>(&overlap.second);break; // 捕获首个交通灯重叠区域}}if (traffic_sign_overlap == nullptr) {return false; // 无交通灯重叠区域则退出}const std::vector<hdmap::PathOverlap>& traffic_light_overlaps = reference_line_info.reference_line().map_path().signal_overlaps();const double start_check_distance = context_.scenario_config.start_traffic_light_scenario_distance(); // 场景触发距离（如5米）const double adc_front_edge_s = reference_line_info.AdcSlBoundary().end_s();// find all the traffic light belong to// the same group as first encountered traffic lightstd::vector<hdmap::PathOverlap> next_traffic_lights;// 处理多个相邻交通灯（如复合信号灯），确保统一决策。static constexpr double kTrafficLightGroupingMaxDist = 2.0;  // unit: m  // 同一组交通灯最大间距for (const auto& overlap : traffic_light_overlaps) {const double dist = overlap.start_s - traffic_sign_overlap->start_s;if (fabs(dist) <= kTrafficLightGroupingMaxDist) {next_traffic_lights.push_back(overlap); // 同一组交通灯加入处理队列}}bool traffic_light_scenario = false;// note: need iterate all lights to check no RED/YELLOW/UNKNOWNfor (const auto& overlap : next_traffic_lights) {const double adc_distance_to_traffic_light =overlap.start_s - adc_front_edge_s;// 距离检查：仅处理车辆前方且未超过触发距离的交通灯    ADEBUG << "traffic_light[" << overlap.object_id << "] start_s["<< overlap.start_s << "] adc_distance_to_traffic_light["<< adc_distance_to_traffic_light << "]";// enter traffic-light scenarios: based on distance onlyif (adc_distance_to_traffic_light <= 0.0 ||adc_distance_to_traffic_light > start_check_distance) {continue;// 距离不在有效范围则跳过}const auto& signal_color = frame.GetSignal(overlap.object_id).color();ADEBUG << "traffic_light_id[" << overlap.object_id << "] start_s["<< overlap.start_s << "] color[" << signal_color << "]";if (signal_color != perception::TrafficLight::GREEN &&signal_color != perception::TrafficLight::BLACK) {traffic_light_scenario = true;// 红灯或黄灯触发场景切换 绿灯或者未识别状态不触发场景break;}}if (!traffic_light_scenario) {return false; // 所有交通灯均为绿色或未知时退出}// 记录交通灯ID供后续阶段context_.current_traffic_light_overlap_ids.clear();for (const auto& overlap : next_traffic_lights) {context_.current_traffic_light_overlap_ids.push_back(overlap.object_id);}return true;
}

阶段一：TrafficLightProtectedStageApproach

靠近阶段

process()方法

1. 阶段入口与调试信息

StageResult TrafficLightProtectedStageApproach::Process(const TrajectoryPoint& planning_init_point, Frame* frame) {ADEBUG << "stage: Approach";  // 调试日志标记当前阶段为“靠近阶段”CHECK_NOTNULL(frame);  // 检查帧指针非空CHECK_NOTNULL(context_);  // 检查场景上下文非空

• 功能：初始化阶段处理入口，记录日志并校验输入参数有效性。

---

2. 获取上下文与配置

  auto context = GetContextAs<TrafficLightProtectedContext>();  // 获取交通灯保护场景上下文const ScenarioTrafficLightProtectedConfig& scenario_config =context->scenario_config;  // 读取场景配置参数（如最大有效停车距离）

• 设计意图：
  • TrafficLightProtectedContext 存储当前交通灯 ID 列表、配置参数（如 max_valid_stop_distance）。
  • 配置参数从 traffic_light_protected_config.pb.txt 文件中加载，决定阶段的阈值。

---

3. 执行参考线任务链

  StageResult result = ExecuteTaskOnReferenceLine(planning_init_point, frame);  //  stage基类的方法执行路径规划、速度决策等任务 里面调用了task的方法if (result.HasError()) {AERROR << "TrafficLightProtectedStageApproach planning error";  // 任务链异常时记录错误日志}

• 任务链内容：
  • 路径优化（如 PathDecider）生成避障路径。
  • 速度决策（如 SpeedOptimizer）计算安全速度剖面。
  • 若任务失败，触发回退任务（如紧急停车）。

---

4. 交通灯状态检查

  if (context->current_traffic_light_overlap_ids.empty()) {  // 无待处理交通灯时直接完成场景return FinishScenario();}

• 触发条件：当前参考线无关联交通灯或所有交通灯已处理完毕。

---

5. 遍历交通灯并处理

  const auto& reference_line_info = frame->reference_line_info().front();  // 获取主参考线信息bool traffic_light_all_done = true;  // 标记所有交通灯是否处理完成for (const auto& traffic_light_overlap_id :context->current_traffic_light_overlap_ids) {  // 遍历当前交通灯ID列表PathOverlap* current_traffic_light_overlap =reference_line_info.GetOverlapOnReferenceLine(traffic_light_overlap_id, ReferenceLineInfo::SIGNAL);  // 获取交通灯在参考线的重叠区域if (!current_traffic_light_overlap) {continue;  // 跳过无效交通灯}// 设置路权状态为“无优先通行权”（需等待）reference_line_info.SetJunctionRightOfWay(current_traffic_light_overlap->start_s, false);  

• 路权决策：在路口处标记车辆需礼让其他交通参与者，与交规逻辑绑定。

---

6. 距离与信号灯颜色校验

    const double adc_front_edge_s = reference_line_info.AdcSlBoundary().end_s();  // 自车前端s坐标const double distance_adc_to_stop_line = current_traffic_light_overlap->start_s - adc_front_edge_s;  // 计算自车到停止线距离auto signal_color = frame->GetSignal(traffic_light_overlap_id).color();  // 获取信号灯颜色ADEBUG << "traffic_light_overlap_id[" << traffic_light_overlap_id << "] ...";  // 调试日志输出// 校验距离是否超出最大有效停车距离if (distance_adc_to_stop_line > scenario_config.max_valid_stop_distance()) {traffic_light_all_done = false;break;  // 距离过远则标记未完成}// 校验信号灯颜色是否为绿色if (signal_color != TrafficLight::GREEN) {  traffic_light_all_done = false;break;  // 红灯或黄灯则标记未完成}}

• 关键阈值：
  • max_valid_stop_distance 控制触发停车决策的有效范围（如5米），避免过早或过晚响应。
  • 仅绿灯允许通行，红灯/黄灯触发停车墙生成逻辑。

---

7. 阶段状态返回

  if (traffic_light_all_done) {  // 所有交通灯处理完成return FinishStage();  // 结束当前阶段，进入下一阶段（如INTERSECTION_CRUISE）}return result.SetStageStatus(StageStatusType::RUNNING);  // 阶段继续运行

• 状态流转：
  • 完成条件：所有交通灯均为绿灯且自车在有效距离内。
  • 若未完成，保持 RUNNING 状态持续处理。

---

FinishStage()方法

1. 获取场景上下文

auto context = GetContextAs<TrafficLightProtectedContext>();

• 功能：获取当前场景的上下文对象 TrafficLightProtectedContext，其中存储了与交通灯相关的动态状态（如当前处理的交通灯 ID 列表）。
• 设计关联：上下文对象贯穿场景的多个阶段，支持状态跨阶段传递。

---

2. 更新规划状态中的交通灯信息

auto* traffic_light = injector_->planning_context()->mutable_planning_status()->mutable_traffic_light();
traffic_light->clear_done_traffic_light_overlap_id();

• 功能：
  • injector_->planning_context()：通过依赖注入器获取全局规划上下文 PlanningContext。
  • clear_done_traffic_light_overlap_id()：清空已完成处理的交通灯 ID 列表，避免历史数据干扰下一阶段逻辑。
• 设计意图：
  在进入新阶段前重置状态，确保仅保留当前场景相关的交通灯记录。

---

3. 记录已完成的交通灯 ID

for (const auto& traffic_light_overlap_id :context->current_traffic_light_overlap_ids) {traffic_light->add_done_traffic_light_overlap_id(traffic_light_overlap_id);
}

• 功能：将当前场景下处理的交通灯 ID 添加到全局规划上下文的 done_traffic_light_overlap_id 列表。
• 意义：
  • 标记这些交通灯已处理完成，后续阶段不再重复决策（如避免二次停车）。
  • 支持场景跳转时状态持久化（例如换道后仍需处理同一组交通灯）。

---

4. 设置下一阶段并返回状态

next_stage_ = "TRAFFIC_LIGHT_PROTECTED_INTERSECTION_CRUISE";
return StageResult(StageStatusType::FINISHED);

• 功能：
  1. next_stage_：指定下一阶段为 TRAFFIC_LIGHT_PROTECTED_INTERSECTION_CRUISE（交叉路口巡航阶段）。
  2. 返回值：标记当前阶段状态为 FINISHED，通知场景管理器切换阶段。
• 逻辑关联：
  • 根据交通灯场景的预设流程（Approach→Intersection Cruise），阶段顺序由配置文件定义。
  • 交叉路口巡航阶段负责处理车辆通过路口时的动态避障和速度优化。

---

场景阶段流

• Approach阶段：处理靠近交通灯时的停车、等待逻辑（如生成停止墙）。
• Intersection Cruise阶段：处理车辆进入路口后的路径跟踪与动态避障。

阶段二：TrafficLightProtectedStageIntersectionCruise

交叉路口巡航阶段

Process()方法

1. 阶段入口与调试信息

ADEBUG << "stage: IntersectionCruise";  // 标记当前为路口巡航阶段
CHECK_NOTNULL(frame);                   // 验证帧数据指针有效性

• 功能：标识当前处于交叉路口巡航阶段，确保输入的 frame 数据结构非空。这是所有规划阶段的通用前置检查。

---

2. 执行参考线任务链

StageResult result = ExecuteTaskOnReferenceLine(planning_init_point, frame);
if (result.HasError()) {AERROR << "TrafficLightProtectedStageIntersectionCruise plan error";  // 任务链异常时记录错误日志
}

• 任务链内容：
  • 路径优化：可能调用 PathOptimizer（如 PIECEWISE_JERK_PATH_OPTIMIZER）生成避障路径。
  • 速度决策：可能使用 SpeedOptimizer（如非线性速度优化器）生成平滑的速度剖面，满足动力学约束。
  • 动态避障：处理路口内的动态障碍物（如行人、交叉车流）的交互预测与响应策略。
• 错误处理：任务失败时会触发日志记录，但不直接终止场景，依赖回退任务（如紧急停车）保证安全性。

---

3. 阶段完成条件检查

bool stage_done = CheckDone(*frame, injector_->planning_context(), true);

• CheckDone 逻辑推测：
  • 车辆位置：检查自车是否完全通过交叉路口区域（如参考线终点超过路口末端停止线）。
  • 交通灯状态：确认所有关联交通灯已处理（如绿灯状态持续且无新红灯触发）。
  • 障碍物风险：确保路口内无潜在碰撞风险（如横向安全距离达标）。
• 参数 true 的含义：可能表示严格模式，要求所有条件同时满足才标记阶段完成。

---

4. 阶段状态返回

if (stage_done) {return FinishStage();  // 完成阶段，切换至下一阶段（如默认车道跟随）
}
return result.SetStageStatus(StageStatusType::RUNNING);  // 继续执行当前阶段

• FinishStage()：
  • 清理临时状态（如重置路口路径缓存）。
  • 更新规划上下文（PlanningContext），标记路口已通过。
• RUNNING 状态：持续优化轨迹，应对路口内动态变化（如突发障碍物插入）。

插件EmergencyStopScenario

紧急停车场景，车辆在行驶过程中如果收到PadMessage命令“PadMessage::STOP”，主车计算停车距离，直接停车。
** IsTransferable()方法**
判断有没有pad紧急停车指令，有则切换

bool EmergencyStopScenario::IsTransferable(const Scenario* const other_scenario, const Frame& frame) {const auto& pad_msg_driving_action = frame.GetPadMsgDrivingAction();if (pad_msg_driving_action == PadMessage::STOP) {return true;}return false;
}

Process()方法

ScenarioResult EmergencyStopScenario::Process(const common::TrajectoryPoint& planning_init_point, Frame* frame) {ScenarioResult stage_result;if (frame->reference_line_info().empty()) {stage_result.SetStageResult(StageStatusType::ERROR,  "Reference line is empty!");AERROR << "Reference line is empty in EmergencyStopScenario!";return stage_result;}//执行基类的逻辑return Scenario::Process(planning_init_point, frame);
}

阶段一：EmergencyStopStageApproach

该阶段用于主车急停前减速，主车速度达到阈值后退出。标记当前阶段为"紧急停车靠近阶段"

process()方法

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

• 主车灯光设置：打开危险报警闪光灯frame->mutable_reference_line_info()->front().SetEmergencyLight()。
• 构建虚拟障碍物：根据规划计算的停车点，构建id为EMERGENCY_STOP的虚拟障碍物。
• ExecuteTaskOnReferenceLine：输入为规划初始点planning_init_point、Frame信息，按照该stage配置的task列表，依次进行规划。
• 检查主车状态：检查主车是否减速达到目标速度。如果减速达到目标速度，进入FinishStage，结束当前Stage，进入EMERGENCY_STOP_STANDBY阶段。如果未达到目标速度，则仍处于EMERGENCY_STOP_APPROACH阶段，返回状态值StageStatusType::RUNNING。
  以下是对 EmergencyStopStageApproach::Process 函数的逐行解释，结合 Apollo 规划模块的紧急停车场景逻辑及搜索结果内容：

---

1. 阶段标识与输入校验

ADEBUG << "stage: Approach";  // 标记当前阶段为"紧急停车靠近阶段"
CHECK_NOTNULL(frame);          // 验证帧数据指针有效性（防止空指针异常）

• 功能：标识当前处于紧急停车场景的 Approach 阶段，确保输入的 frame 数据结构有效。

---

2. 加载场景配置

scenario_config_.CopyFrom(GetContextAs<EmergencyStopContext>()->scenario_config);

• 功能：从上下文对象 EmergencyStopContext 中拷贝场景配置参数（如最大减速度、停车距离等）到 scenario_config_。
• 关联设计：配置参数来源于 scenario_conf.pb.txt 文件，定义了紧急停车的具体行为规则（如 max_stop_deceleration 最大制动减速度）。

---

3. 设置车辆紧急信号

frame->mutable_reference_line_info()->front().SetEmergencyLight();

• 功能：激活车辆危险报警闪光灯，向其他交通参与者传达紧急状态。
• 实现原理：修改参考线信息中的车辆信号标志位，由控制模块解析执行灯光操作。

---

4. 计算停车点并构建虚拟障碍物
(a) 基础参数获取

const auto& reference_line_info = frame->reference_line_info().front();
const auto& reference_line = reference_line_info.reference_line();
const double adc_speed = injector_->vehicle_state()->linear_velocity();  // 当前车速
const double adc_front_edge_s = reference_line_info.AdcSlBoundary().end_s();  // 自车前端s坐标
const double stop_distance = scenario_config_.stop_distance();  // 配置的停车缓冲距离（如0.5米）

(b) 检查已有停车点

bool stop_fence_exist = false;
double stop_line_s;
const auto& emergency_stop_status =injector_->planning_context()->planning_status().emergency_stop();
if (emergency_stop_status.has_stop_fence_point()) {  // 若规划上下文中已存在停车点common::SLPoint stop_fence_sl;reference_line.XYToSL(emergency_stop_status.stop_fence_point(), &stop_fence_sl);if (stop_fence_sl.s() > adc_front_edge_s) {  // 停车点在前方有效范围内stop_fence_exist = true;stop_line_s = stop_fence_sl.s();}
}

• 设计目的：避免重复计算停车点，支持跨规划周期的状态持久化。
  © 动态计算新停车点

if (!stop_fence_exist) {const double deceleration = scenario_config_.max_stop_deceleration();  // 最大制动减速度（如4m/s²）const double travel_distance = std::ceil(std::pow(adc_speed, 2) / (2 * deceleration));  // 计算理论制动距离static constexpr double kBuffer = 2.0;  // 安全冗余距离stop_line_s = adc_front_edge_s + travel_distance + stop_distance + kBuffer;  // 最终停车点s坐标// 更新规划上下文中的停车点坐标const auto& stop_fence_point = reference_line.GetReferencePoint(stop_line_s);auto* emergency_stop_fence_point = injector_->planning_context()->mutable_planning_status()->mutable_emergency_stop()->mutable_stop_fence_point();emergency_stop_fence_point->set_x(stop_fence_point.x());emergency_stop_fence_point->set_y(stop_fence_point.y());
}

• 物理模型：基于运动学公式 ( s = \frac{v^2}{2a} ) 计算理想制动距离，叠加安全缓冲保证停车可靠性。
• 安全冗余：kBuffer 防止因路面摩擦系数变化或传感器误差导致的停车位置偏差。

---

5. 构建停车决策

const std::string virtual_obstacle_id = "EMERGENCY_STOP";
const std::vector<std::string> wait_for_obstacle_ids;
planning::util::BuildStopDecision(virtual_obstacle_id, stop_line_s, stop_distance,StopReasonCode::STOP_REASON_EMERGENCY, wait_for_obstacle_ids,"EMERGENCY_STOP-scenario", frame,&(frame->mutable_reference_line_info()->front()));

• 功能：在停车点处创建虚拟障碍物 EMERGENCY_STOP，触发路径规划生成减速停车轨迹。
• 参数解析：
  • virtual_obstacle_id：虚拟障碍物唯一标识。
  • stop_line_s：停车线s坐标。
  • StopReasonCode::STOP_REASON_EMERGENCY：停车原因标记为紧急情况，控制模块据此执行急刹逻辑。

---

6. 执行路径规划任务链

StageResult result = ExecuteTaskOnReferenceLine(planning_init_point, frame);
if (result.HasError()) {AERROR << "EmergencyPullOverStageApproach planning error";  // 路径规划失败时记录错误日志
}

• 任务链内容：
  • 路径优化：生成避让虚拟障碍物的路径（如 PathDecider 决策绕行逻辑）。
  • 速度规划：生成符合紧急制动减速度的速度剖面（如 PiecewiseJerkSpeedOptimizer 非线性优化）。
• 错误处理：若任务链失败，依赖回退任务（如 FastStopTrajectoryFallback）生成紧急停车轨迹。

---

7. 阶段完成条件检查

const double max_adc_stop_speed = common::VehicleConfigHelper::Instance()->GetConfig().vehicle_param().max_abs_speed_when_stopped();  // 静止判定阈值（如0.1m/s）
if (adc_speed <= max_adc_stop_speed) {return FinishStage();  // 车速低于阈值，结束当前阶段
}
return result.SetStageStatus(StageStatusType::RUNNING);  // 否则继续执行

• 设计逻辑：
  • 车速降至接近零时，切换至 EMERGENCY_STOP_STANDBY 阶段（保持停车状态）。
  • RUNNING 状态持续优化轨迹，应对突发状况（如障碍物突然切入）。

---

FinishStage()方法

该阶段的退出函数。

• 退出EMERGENCY_STOP_APPROACH阶段，进入EMERGENCY_STOP_STANDBY阶段。

StageResult EmergencyStopStageApproach::FinishStage() {next_stage_ = "EMERGENCY_STOP_STANDBY";return StageResult(StageStatusType::FINISHED);
}

---

阶段二：EmergencyStopStageStandby

标记当前为紧急停车保持阶段

process()方法

以下是对 EmergencyStopStageStandby::Process 函数的逐行解释，结合 Apollo 规划模块的紧急停车场景逻辑及搜索结果内容：

---

1. 阶段标识与输入校验

ADEBUG << "stage: Standby";  // 标记当前为紧急停车保持阶段
CHECK_NOTNULL(frame);         // 验证帧数据指针有效性（防止空指针异常）

• 功能：标识当前处于 EMERGENCY_STOP_STANDBY 阶段，该阶段负责维持紧急停车状态直至收到解除指令。

---

2. 加载场景配置

scenario_config_.CopyFrom(GetContextAs<EmergencyStopContext>()->scenario_config);

• 参数来源：从上下文对象 EmergencyStopContext 拷贝配置参数（如 stop_distance 停车缓冲距离），配置文件路径为 modules/planning/scenarios/emergency_stop/conf/scenario_conf.pb.txt。

---

3. 停车点动态计算
(a) 基础参数获取

const auto& reference_line_info = frame->reference_line_info().front();
const auto& reference_line = reference_line_info.reference_line();
const double adc_front_edge_s = reference_line_info.AdcSlBoundary().end_s();  // 自车前端s坐标
const double stop_distance = scenario_config_.stop_distance();  // 配置的停车缓冲距离（如0.5米）

(b) 检查已有停车点

bool stop_fence_exist = false;
double stop_line_s;
const auto& emergency_stop_status = injector_->planning_context()->planning_status().emergency_stop();
if (emergency_stop_status.has_stop_fence_point()) {  // 若规划上下文中已存在停车点common::SLPoint stop_fence_sl;reference_line.XYToSL(emergency_stop_status.stop_fence_point(), &stop_fence_sl);if (stop_fence_sl.s() > adc_front_edge_s) {  // 停车点在车辆前方有效范围内stop_fence_exist = true;stop_line_s = stop_fence_sl.s();}
}

• 设计意图：支持跨规划周期的停车点持久化，避免因模块重启导致停车位置重置。
  © 生成新停车点

if (!stop_fence_exist) {static constexpr double kBuffer = 2.0;  // 安全冗余距离stop_line_s = adc_front_edge_s + stop_distance + kBuffer;  // 计算最终停车点// 更新规划上下文中的停车点坐标const auto& stop_fence_point = reference_line.GetReferencePoint(stop_line_s);auto* emergency_stop_fence_point = injector_->planning_context()->mutable_planning_status()->mutable_emergency_stop()->mutable_stop_fence_point();emergency_stop_fence_point->set_x(stop_fence_point.x());emergency_stop_fence_point->set_y(stop_fence_point.y());
}

• 安全冗余：kBuffer 防范传感器误差或地面摩擦系数变化引起的停车位置偏差，确保绝对停车安全。

---

4. 构建停车决策

const std::string virtual_obstacle_id = "EMERGENCY_STOP";
const std::vector<std::string> wait_for_obstacle_ids;
planning::util::BuildStopDecision(virtual_obstacle_id, stop_line_s, stop_distance,StopReasonCode::STOP_REASON_EMERGENCY, wait_for_obstacle_ids,"EMERGENCY_STOP-scenario", frame,&(frame->mutable_reference_line_info()->front()));

• 虚拟障碍物功能：生成 ID 为 EMERGENCY_STOP 的虚拟障碍物，强制路径规划保持停车状态。
• 停车原因码：STOP_REASON_EMERGENCY 触发控制模块执行急刹逻辑，而非普通停车。

---

5. 执行路径规划任务链

StageResult result = ExecuteTaskOnReferenceLine(planning_init_point, frame);
if (result.HasError()) {AERROR << "EmergencyStopStageStandby planning error";  // 路径规划失败时记录错误日志
}

• 任务链内容：
  • 路径保持：复用历史路径（如 PathReuseDecider），避免车辆移动。
  • 速度规划：生成零速剖面（SpeedDecider）并校验加速度约束（PiecewiseJerkSpeedOptimizer）。
• 错误处理：若任务链失败，依赖回退任务（如 FastStopTrajectoryFallback）维持停车。

---

6. 退出条件检查

const auto& pad_msg_driving_action = frame->GetPadMsgDrivingAction();
if (pad_msg_driving_action != PadMessage::STOP) {  // 检测到停车指令解除return FinishStage();  // 结束当前阶段，退出紧急停车场景
}
return result.SetStageStatus(StageStatusType::RUNNING);  // 继续维持停车状态

• PadMessage 指令：由外部控制接口（如遥控器）发送，状态切换至非 STOP 时退出。
• 状态流转：退出后可能返回默认场景（如 LANE_FOLLOW）或根据新指令进入其他场景。

插件ParkAndGoScenario

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

场景切入条件

• 检查frame规划命令中是否存在车道跟随命令。如果不存在，函数返回false，不能转移。
• 检查frame的参考线信息是否为空。如果为空，返回false，不能转移。
• 检查other_scenario是否为空。如果为空，返回false，不可以转移。
• 车辆静止且远离终点
• 车辆附近无道路或道路非城市车道

---

1. 基础条件检查

if (!frame.local_view().planning_command->has_lannel_follow_command()) {AINFO << "PARK_AND_GO: Don't has lane follow command!";return false;  // 无车道跟随指令时阻断场景切换
}
if (other_scenario == nullptr || frame.reference_line_info().empty()) {AINFO << "PARK_AND_GO: Don't has reference line info or other scenario!";return false;  // 参考线缺失或场景上下文失效时退出
}

• 设计意图：确保车辆处于车道跟随模式且参考线有效。车道跟随指令是 Park and Go 场景的前提，因其需要在启动后重新规划路径。

---

2. 车辆状态获取

common::VehicleState vehicle_state = injector_->vehicle_state()->vehicle_state();
auto adc_point = common::util::PointFactory::ToPointENU(vehicle_state);
double adc_speed = vehicle_state_provider->linear_velocity();  // 当前车速
const double max_abs_speed_when_stopped = VehicleConfigHelper::GetConfig().max_abs_speed_when_stopped();  // 静止判定阈值（如0.1m/s）

• 关键参数：max_abs_speed_when_stopped 来自车辆参数配置文件，用于判定车辆是否静止。

---

3. 目的地距离计算

const auto routing_end = frame.local_view().end_lane_way_point;  // 路径规划终点
common::SLPoint dest_sl;
reference_line.XYToSL(routing_end->pose(), &dest_sl);  // 终点投影到参考线s坐标
const double adc_front_edge_s = reference_line_info.AdcSlBoundary().end_s();  // 自车前端s坐标
const double adc_distance_to_dest = dest_sl.s() - adc_front_edge_s;  // 自车到终点距离

• 意义：若距离终点过近（低于 min_dist_to_dest() 配置值），无需激活 Park and Go 场景。

---

4. 车道属性判断

hdmap::LaneInfoConstPtr lane;
HDMapUtil::BaseMap().GetNearestLaneWithDistance(adc_point, 5.0, &lane, &s, &l);  // 查找5米内最近车道
bool is_ego_on_lane = (lane != nullptr && lane->IsOnLane({adc_point.x(), adc_point.y()}));  // 车辆是否在车道内
bool is_lane_type_city_driving = (lane->lane().type() == hdmap::Lane::CITY_DRIVING);  // 是否为城市驾驶车道

• 逻辑：
  • 若车辆位于非城市驾驶车道（如停车场内部道路）或完全脱离车道，可能需触发 Park and Go 场景。
  • 城市驾驶车道（CITY_DRIVING）通常要求遵守复杂交规，而非城市车道可能允许更灵活操作。

---

5. 触发条件综合判断

bool is_vehicle_static = (std::fabs(adc_speed) < max_abs_speed_when_stopped);  // 车辆是否静止
bool is_distance_far_enough = (adc_distance_to_dest > scenario_config.min_dist_to_dest());  // 终点距离是否足够远if (is_vehicle_static && is_distance_far_enough && (!is_ego_on_lane || !is_lane_type_city_driving)) {park_and_go = true;  // 触发场景切换
}

• 条件解析：
  1. 静止状态：确保车辆已完成停车动作（如靠边停车场景后需重新启动）。
  2. 远距离终点：配置参数 min_dist_to_dest 避免短距离场景误触发。
  3. 非标准车道位置：车辆可能处于停车场、临时停靠区等非规划道路区域，需特殊处理。

---

阶段一：ParkAndGoStageCheck

检查阶段：检测当前车辆状态是否满足公路行驶要求，通过调用CheckADCReadyToCruise函数判断车辆档位、车速、前方障碍物是否可nudge、与道路参考线朝向差、与参考线纵向距离等条件，决定车辆是否满足公路行驶要求。若满足要求，下一阶段为PARK_AND_GO_CRUISE，若不满足要求，下一阶段为PARK_AND_GO_ADJUST。

process()方法

---

1. 阶段标识与输入校验

ADEBUG << "stage: Check";  // 输出当前阶段为"检查阶段"
CHECK_NOTNULL(frame);       // 确保帧数据指针非空
CHECK_NOTNULL(context_);    // 确保场景上下文对象有效

• 功能：标记当前处于 Park and Go 场景的检查阶段，验证输入有效性。

---

2. 初始化自动驾驶车辆状态

ADCInitStatus();  // 初始化车辆状态信息

• 具体行为：
  • 清除上下文中的历史状态（park_and_go_status->Clear()）。
  • 记录车辆当前位置（x, y, z）和航向角（heading）。
  • 标记当前处于检查阶段（in_check_stage = true）。

---

3. 设置开放空间轨迹模式

frame->mutable_open_space_info()->set_is_on_open_space_trajectory(true);

• 设计意图：
  • 表明当前规划需采用开放空间算法（如混合A*、RS曲线），适用于停车场等非结构化道路。
  • 影响后续任务链的执行逻辑（如路径生成方式）。

---

4. 执行开放空间任务链

StageResult result = ExecuteTaskOnOpenSpace(frame);  // 执行开放空间规划任务链
if (result.HasError()) {AERROR << "ParkAndGoStageAdjust planning error";   // 任务链异常时记录错误日志return result.SetStageStatus(StageStatusType::ERROR);  // 返回错误状态
}

• 任务链内容：
  • 路径搜索：生成避障路径（可能调用 OpenSpaceRoiDecider 与 OpenSpaceTrajectoryProvider）。
  • 速度优化：结合车辆动力学约束生成速度剖面。
  • 碰撞检测：确保生成的轨迹安全可行。
• 错误处理：若任务链失败，返回 ERROR 状态，触发回退机制（如紧急停车）。

---

5. 检查车辆是否准备好巡航

bool ready_to_cruise =CheckADCReadyToCruise(injector_->vehicle_state(), frame,GetContextAs<ParkAndGoContext>()->scenario_config);

• 检查条件：
  • 车速：是否接近零速（abs(velocity) < threshold）。
  • 位置偏离：是否偏离预期启动区域（如停车场车位边界）。
  • 障碍物检测：周围是否存在动态障碍物阻碍启动。
• 参数依赖：scenario_config 提供检查阈值（如速度容差、距离阈值）。

---

6. 阶段完成与状态切换

return FinishStage(ready_to_cruise);  // 根据检查结果决定阶段完成状态

• FinishStage 逻辑：
  • 成功（ready_to_cruise = true）：下一阶段设为 PARK_AND_GO_CRUISE（巡航阶段）。
  • 失败（ready_to_cruise = false）：下一阶段设为 PARK_AND_GO_ADJUST（调整阶段）。
  • 状态更新：标记检查阶段结束（in_check_stage = false）。

---

代码解释

StageResult ParkAndGoStageCheck::Process(const TrajectoryPoint& planning_init_point, Frame* frame) {ADEBUG << "stage: Check";CHECK_NOTNULL(frame);CHECK_NOTNULL(context_);ADCInitStatus();// 初始化车辆状态信息frame->mutable_open_space_info()->set_is_on_open_space_trajectory(true);// 设置开放空间轨迹模式StageResult result = ExecuteTaskOnOpenSpace(frame);  // 执行开放空间规划任务链if (result.HasError()) {AERROR << "ParkAndGoStageAdjust planning error";return result.SetStageStatus(StageStatusType::ERROR);}// 检查车辆是否准备好巡航bool ready_to_cruise =CheckADCReadyToCruise(injector_->vehicle_state(), frame,GetContextAs<ParkAndGoContext>()->scenario_config);return FinishStage(ready_to_cruise); // 根据检查结果决定阶段完成状态
}

FinishStage方法

StageResult ParkAndGoStageCheck::FinishStage(const bool success) {if (success) {next_stage_ = "PARK_AND_GO_CRUISE";//巡航阶段} else {next_stage_ = "PARK_AND_GO_ADJUST";//调整阶段}injector_->planning_context()->mutable_planning_status()->mutable_park_and_go()->set_in_check_stage(false);return StageResult(StageStatusType::FINISHED);
}

---

阶段二：ParkAndGoStageAdjust

调整阶段：车辆驶向道路阶段，为车辆调整自身位姿，驶向道路阶段。该阶段将自车在参考线上投影点沿纵向方向移动一定距离后的位置作为目标点，生成对应轨迹。同时调用CheckADCReadyToCruise函数检测是否满足公路行驶要求。若满足公路行驶要求或轨迹行驶完成，判断轨迹曲率是否小于阈值，若小于，下一阶段为PARK_AND_GO_CRUISE，否则，下一阶段为PARK_AND_GO_PRE_CRUISE。

Process方法

以下是对 ParkAndGoStageAdjust::Process 函数的逐行解释，结合 Apollo 规划模块的 Park and Go 场景逻辑及搜索结果内容：

---

1. 阶段标识与输入校验

ADEBUG << "stage: Adjust";  // 标记当前为调整阶段
CHECK_NOTNULL(frame);        // 验证帧数据指针有效性

• 功能：标识当前处于 Park and Go 场景的调整阶段（Adjust Stage），确保输入数据结构有效。
• 设计关联：调整阶段通常用于修正车辆位置或轨迹偏差，为进入巡航阶段做准备。

---

2. 设置开放空间轨迹模式

frame->mutable_open_space_info()->set_is_on_open_space_trajectory(true);

• 功能：启用开放空间算法，适用于停车场等非结构化环境规划。
• 意义：开放空间算法（如混合A*、RS曲线）支持复杂几何路径生成，解决狭窄空间内的转向问题。

---

3. 执行开放空间任务链

StageResult result = ExecuteTaskOnOpenSpace(frame);  // 执行开放空间任务链
if (result.HasError()) {AERROR << "ParkAndGoStageAdjust planning error";   // 任务链异常时记录错误日志return result.SetStageStatus(StageStatusType::ERROR);  // 返回错误状态
}

• 任务链内容：
  1. 路径搜索：调用 OpenSpaceRoiDecider 确定可行区域，生成避障路径。
  2. 轨迹优化：使用非线性优化器（如 PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER）生成平滑速度剖面。
  3. 碰撞校验：确保轨迹与障碍物无冲突。
• 错误处理：若任务失败，触发回退机制（如紧急停车），保证系统鲁棒性。

---

4. 检查车辆是否准备好巡航

const bool is_ready_to_cruise =CheckADCReadyToCruise(injector_->vehicle_state(), frame,GetContextAs<ParkAndGoContext>()->scenario_config);

• 检查条件：
  1. 车速：是否接近零速（如 abs(velocity) < 0.1m/s）。
  2. 位置偏离：是否偏离预期启动区域（如车位边界超过阈值）。
  3. 障碍物检测：周围是否有动态障碍物阻碍启动。
• 参数依赖：scenario_config 提供阈值（如距离容差、速度阈值）。

---

5. 判断轨迹是否已结束

bool is_end_of_trajectory = false;
const auto& history_frame = injector_->frame_history()->Latest();
if (history_frame) {const auto& trajectory_points =history_frame->current_frame_planned_trajectory().trajectory_point();if (!trajectory_points.empty()) {is_end_of_trajectory =(trajectory_points.rbegin()->relative_time() < 0.0);  // 轨迹点时间是否超期}
}

• 逻辑解析：
  • 检查历史轨迹的最后一点时间戳是否小于当前时间（relative_time < 0），若成立则标记轨迹已结束。
  • 确保规划模块生成的轨迹始终覆盖未来时间段，避免控制模块无轨迹可循。

---

6. 阶段状态切换

if (!is_ready_to_cruise && !is_end_of_trajectory) {return result.SetStageStatus(StageStatusType::RUNNING);  // 继续调整阶段
}
return FinishStage();  // 完成调整，进入下一阶段（如CRUISE）

• FinishStage 逻辑：
  • 成功条件：车辆准备好巡航或轨迹已完整执行，下一阶段设为 PARK_AND_GO_CRUISE。
  • 上下文更新：清理临时状态（如重置开放空间路径缓存）。
• 状态延续：若检查未通过且轨迹未结束，保持 RUNNING 状态持续优化。

---

多阶段协同

• Check阶段：验证初始状态并触发调整需求。
• Adjust阶段：动态修正轨迹偏差，适应环境变化（如障碍物移动）。
• Cruise阶段：进入结构化道路的标准车道跟随模式。

---

代码解释：

StageResult ParkAndGoStageAdjust::Process(const TrajectoryPoint& planning_init_point, Frame* frame) {ADEBUG << "stage: Adjust";CHECK_NOTNULL(frame);
// 设置开放空间轨迹模式frame->mutable_open_space_info()->set_is_on_open_space_trajectory(true);StageResult result = ExecuteTaskOnOpenSpace(frame);// 执行开放空间任务链if (result.HasError()) {AERROR << "ParkAndGoStageAdjust planning error";return result.SetStageStatus(StageStatusType::ERROR);}// 检查车辆是否准备好巡航 检查条件： 车速接近0 位置偏离 周围是否有动态障碍物阻碍启动const bool is_ready_to_cruise =CheckADCReadyToCruise(injector_->vehicle_state(), frame,GetContextAs<ParkAndGoContext>()->scenario_config);bool is_end_of_trajectory = false;const auto& history_frame = injector_->frame_history()->Latest();if (history_frame) {const auto& trajectory_points =history_frame->current_frame_planned_trajectory().trajectory_point();if (!trajectory_points.empty()) {is_end_of_trajectory =(trajectory_points.rbegin()->relative_time() < 0.0);// 轨迹点时间是否超期}}if (!is_ready_to_cruise && !is_end_of_trajectory) {return result.SetStageStatus(StageStatusType::RUNNING);// 继续调整阶段}return FinishStage(); // 完成调整，进入下一阶段
}

---

FinishStage方法

StageResult ParkAndGoStageAdjust::FinishStage() {const auto vehicle_status = injector_->vehicle_state();ADEBUG << vehicle_status->steering_percentage();if (std::fabs(vehicle_status->steering_percentage()) <GetContextAs<ParkAndGoContext>()->scenario_config.max_steering_percentage_when_cruise()) {next_stage_ = "PARK_AND_GO_CRUISE";} else {ResetInitPostion();next_stage_ = "PARK_AND_GO_PRE_CRUISE";}return StageResult(StageStatusType::FINISHED);
}

阶段三：ParkAndGoStagePreCruise

调整轨迹曲率阶段，
PARK_AND_GO_PRE_CRUISE同PARK_AND_GO_ADJUST，用于调整自车位姿，使轨迹曲率小于阈值，若轨迹曲率小于阈值，转到下一阶段PARK_AND_GO_CRUISE。

Process方法

以下是对 ParkAndGoStagePreCruise::Process 函数的逐行解释，结合 Apollo 规划模块的逻辑设计及搜索结果内容：

---

1. 阶段标识与输入校验

ADEBUG << "stage: Pre Cruise";  // 标记当前为预巡航阶段
CHECK_NOTNULL(frame);           // 验证帧数据有效性
CHECK_NOTNULL(context_);        // 验证场景上下文有效性

• 功能：标识当前处于 Park and Go 场景的预巡航阶段（Pre-Cruise Stage），确保输入参数合法。此阶段是车辆从调整阶段（Adjust）到巡航阶段（Cruise）的过渡。

---

2. 加载场景配置

const ScenarioParkAndGoConfig& scenario_config =GetContextAs<ParkAndGoContext>()->scenario_config;

• 功能：从上下文对象 ParkAndGoContext 中获取场景配置参数，例如 max_steering_percentage_when_cruise（允许进入巡航的最大方向盘转角百分比），参数来源于 scenario_conf.pb.txt 配置文件。

---

3. 设置开放空间轨迹模式

frame->mutable_open_space_info()->set_is_on_open_space_trajectory(true);

• 设计意图：启用开放空间规划算法（如 Hybrid A* 和 RS 曲线），适用于非结构化道路场景下的轨迹生成。此模式允许车辆在停车场等复杂环境中进行路径搜索和动态避障。

---

4. 执行开放空间任务链

StageResult result = ExecuteTaskOnOpenSpace(frame);  // 执行开放空间规划任务链
if (result.HasError()) {AERROR << "ParkAndGoStagePreCruise planning error";  // 任务链异常时记录错误日志return result.SetStageStatus(StageStatusType::ERROR);
}

• 任务链内容：
  • 路径搜索：调用 HybridAStar::Plan 生成粗解路径，结合 Reeds-Shepp 曲线优化转弯逻辑。
  • 轨迹优化：使用 OpenSpaceTrajectoryOptimizer 进行路径与速度解耦优化，确保轨迹平滑且符合车辆动力学约束。
  • 碰撞检测：验证轨迹安全性，避免与障碍物冲突。

---

5. 车辆状态检查与退出条件

auto vehicle_status = injector_->vehicle_state();
AINFO << "Current steering percentage: " << vehicle_status->steering_percentage();// 条件1：方向盘转角是否回正至阈值内
bool steering_condition = (std::fabs(vehicle_status->steering_percentage()) < scenario_config.max_steering_percentage_when_cruise());// 条件2：综合检查车辆是否准备好进入巡航（如车速、位置偏差等）
bool ready_to_cruise = CheckADCReadyToCruise(injector_->vehicle_state(), frame, scenario_config);if (steering_condition && ready_to_cruise) {return FinishStage();  // 进入下一阶段（CRUISE）
}

• 方向盘转角检查：
  max_steering_percentage_when_cruise 用于限制车辆进入巡航前的转向角度（如 <5%），确保车辆姿态接近直线行驶。
• CheckADCReadyToCruise 逻辑推测：
  1. 车速：是否接近零速（避免急加速进入巡航）。
  2. 位置偏差：车辆是否沿参考线行驶（横向偏差小于阈值）。
  3. 障碍物检测：路径前方是否存在动态障碍物阻碍。
  4. 轨迹有效性：历史轨迹是否覆盖未来时间段（避免控制模块无轨迹可用）。

---

6. 阶段状态返回

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

• RUNNING 状态处理：持续优化轨迹直至满足退出条件。例如：
  • 方向盘未回正时，调整路径逐步修正转向角度。
  • 存在临时障碍物时，生成绕行轨迹。
• FinishStage() 后续逻辑：
  • 清理开放空间轨迹缓存。
  • 将下一阶段设为 PARK_AND_GO_CRUISE，进入结构化道路的标准车道跟随模式。

---

关联设计与应用场景

1. 开放空间与结构化道路的衔接

   • 预巡航阶段：确保车辆从非结构化环境（如停车场）平滑过渡到结构化道路（如城市车道）的控制模式。
   • 方向盘回正要求：防止转向残留导致巡航阶段路径跟踪抖动。

2. 轨迹优化与安全冗余

   • Hybrid A 粗解*：快速生成初始路径，但可能存在锯齿状转向（需后续优化）。
   • DL-IAPS/PJSO 算法：路径-速度解耦优化技术，提升轨迹平滑性与乘坐舒适性。

---

典型应用场景

• 停车场出口过渡：车辆从停车位调整至车道中线后，预巡航阶段验证转向回正并生成进入主路的轨迹。
• 临时避障恢复：绕行障碍物后重新对齐参考线，确保安全进入巡航。

---

代码解释:

StageResult ParkAndGoStagePreCruise::Process(const TrajectoryPoint& planning_init_point, Frame* frame) {ADEBUG << "stage: Pre Cruise";CHECK_NOTNULL(frame);CHECK_NOTNULL(context_);//加载场景配置const ScenarioParkAndGoConfig& scenario_config =GetContextAs<ParkAndGoContext>()->scenario_config;
// 设置开放空间轨迹模式frame->mutable_open_space_info()->set_is_on_open_space_trajectory(true);StageResult result = ExecuteTaskOnOpenSpace(frame);// 执行开放空间任务链if (result.HasError()) {AERROR << "ParkAndGoStagePreCruise planning error";return result.SetStageStatus(StageStatusType::ERROR);}// const bool ready_to_cruise =//     CheckADCReadyToCruise(frame, scenario_config_);//车辆状态检查与退出条件 auto vehicle_status = injector_->vehicle_state();AINFO << "Current steering percentage: "<< vehicle_status->steering_percentage();// 条件1：方向盘转角是否回正至阈值内// 条件2：综合检查车辆是否准备好进入巡航（如车速、位置偏差等）if ((std::fabs(vehicle_status->steering_percentage()) <scenario_config.max_steering_percentage_when_cruise()) &&CheckADCReadyToCruise(injector_->vehicle_state(), frame,scenario_config)) {return FinishStage();// 进入下一阶段（CRUISE）}return result.SetStageStatus(StageStatusType::RUNNING);
}

FinishStage方法

StageResult ParkAndGoStagePreCruise::FinishStage() {next_stage_ = "PARK_AND_GO_CRUISE";return StageResult(StageStatusType::FINISHED);
}

---

阶段四：ParkAndGoStageCruise

接近参考线阶段，
PARK_AND_GO_CRUISE公共道路行驶阶段，当自车与参考线横向距离小于阈值，该阶段结束。

Process方法

以下是对 ParkAndGoStageCruise::Process 函数的逐行解释，结合 Apollo 规划模块的逻辑设计及搜索结果内容：

---

1. 阶段标识与输入校验

ADEBUG << "stage: Cruise";  // 标记当前为巡航阶段
CHECK_NOTNULL(frame);        // 验证帧数据指针有效性

• 功能：标识当前处于 Park and Go 场景的巡航阶段（Cruise Stage），确保输入的 frame 数据结构有效。
• 设计关联：此阶段是车辆从非结构化环境（如停车场）过渡到结构化道路（如城市车道）的核心阶段。

---

2. 执行参考线任务链

StageResult result = ExecuteTaskOnReferenceLine(planning_init_point, frame);
if (result.HasError()) {AERROR << "ParkAndGoStageCruise planning error";  // 任务链异常时记录错误日志
}

• 任务链内容：
  1. 路径跟踪：使用 PathDecider 决策保持当前参考线路径。
  2. 速度优化：调用 SpeedOptimizer（如非线性速度优化器）生成平滑的速度剖面。
  3. 动态避障：检测静态/动态障碍物并生成绕行策略。
• 错误处理：若任务链失败，依赖回退任务（如 FastStopTrajectoryFallback）保证安全性，但当前代码仅记录错误不终止阶段。

---

3. 获取参考线信息

const ReferenceLineInfo& reference_line_info = frame->reference_line_info().front();

• 功能：获取当前参考线信息，包含车辆位置、路径规划结果及环境感知数据。
• 设计意义：参考线信息是结构化道路导航的基础，用于判断车辆是否按预期行驶。

---

4. 车辆状态检查

ParkAndGoStatus status = CheckADCParkAndGoCruiseCompleted(reference_line_info);

• CheckADCParkAndGoCruiseCompleted 逻辑推测：
  1. 路径起点检查：验证车辆是否到达全局路径规划的起点（如 routing_begin_s），标记巡航完成。
  2. 横向偏差：检查车辆横向偏移是否小于阈值（如 <0.3米），确保沿参考线行驶。
  3. 速度匹配：判断车速是否匹配结构化道路要求（如接近目标巡航速度）。
  4. 障碍物风险：确保路径前方无阻碍车辆正常行驶的动态障碍物。
• 状态返回值：
  • CRUISE_COMPLETE：满足所有退出条件。
  • CRUISING：需继续执行当前阶段。

---

5. 阶段状态切换

if (status == CRUISE_COMPLETE) {return FinishStage();  // 完成阶段，切换至下一场景（如LANE_FOLLOW）
}
return result.SetStageStatus(StageStatusType::RUNNING);  // 继续巡航

• FinishStage() 逻辑：
  1. 清理临时状态（如重置开放空间路径缓存）。
  2. 将下一阶段设为默认车道跟随（LANE_FOLLOW_DEFAULT_STAGE）。
  3. 更新规划上下文，标记 Park and Go 场景完成。
• RUNNING 状态处理：持续优化路径与速度，直至满足退出条件。

---

关联设计与功能亮点

1. 结构化道路导航衔接

   • 通过参考线任务链实现从非结构化到结构化环境的平滑过渡。
   • 速度优化器考虑曲率约束（如 use_soft_bound 参数），提升乘坐舒适性。

2. 安全冗余机制

   • 横向偏差校验防止车辆偏离参考线导致碰撞风险。
   • 动态障碍物检测支持紧急制动或绕行动作。

3. 场景化状态机

   • Check阶段 → Adjust阶段 → Pre-Cruise阶段 → Cruise阶段 的流转逻辑，覆盖完整泊出流程。

---

FinishStage方法

StageResult ParkAndGoStageCruise::FinishStage() { return FinishScenario(); 
}