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规划，实现车辆由开放空间驶入道路的功能。

场景切入条件

1. 存在沿车道行驶命令
2. 有参考线生成且由其他scenario切入
3. 车辆静止且远离终点
4. 车辆附近无道路或道路非城市车道

阶段一：ParkAndGoStageCheck

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

process()方法

阶段二：ParkAndGoStageAdjust

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

阶段三：ParkAndGoStagePreCruise

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

阶段四：ParkAndGoStageCruise

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

未完待续。。。