Each stage does its business logic inside Process function.
If the stage want to transit to a different stage after finish, it should set the type of 'next_stage_'.
41 {
42 ADEBUG <<
"stage: Approach";
43 CHECK_NOTNULL(frame);
44 auto scenario_context = GetContextAs<EmergencyPullOverContext>();
45 const auto& scenario_config = scenario_context->scenario_config;
46
47 auto& reference_line_info = frame->mutable_reference_line_info()->front();
48 reference_line_info.LimitCruiseSpeed(
49 scenario_context->target_slow_down_speed);
50
52
53 double stop_line_s = 0.0;
54
55
56 const auto& pull_over_status =
57 injector_->planning_context()->planning_status().pull_over();
58 if (pull_over_status.has_position() && pull_over_status.position().has_x() &&
59 pull_over_status.position().has_y()) {
60 const auto& reference_line = reference_line_info.reference_line();
61 common::SLPoint pull_over_sl;
62 reference_line.XYToSL(pull_over_status.position(), &pull_over_sl);
63 const double stop_distance = scenario_config.stop_distance();
64 stop_line_s =
65 pull_over_sl.s() + stop_distance +
67 const std::string virtual_obstacle_id = "EMERGENCY_PULL_OVER";
68 const std::vector<std::string> wait_for_obstacle_ids;
70 virtual_obstacle_id, stop_line_s, stop_distance,
71 StopReasonCode::STOP_REASON_PULL_OVER, wait_for_obstacle_ids,
72 "EMERGENCY_PULL_OVER-scenario", frame,
73 &(frame->mutable_reference_line_info()->front()));
74
75 ADEBUG <<
"Build a stop fence for emergency_pull_over: id["
76 << virtual_obstacle_id << "] s[" << stop_line_s << "]";
77 }
78
80 if (result.HasError()) {
81 AERROR <<
"EmergencyPullOverStageApproach planning error";
82 }
83
84 if (stop_line_s > 0.0) {
85 const double adc_front_edge_s = reference_line_info.AdcSlBoundary().end_s();
86 double distance = stop_line_s - adc_front_edge_s;
87 const double adc_speed =
injector_->vehicle_state()->linear_velocity();
88 const double max_adc_stop_speed = common::VehicleConfigHelper::Instance()
89 ->GetConfig()
90 .vehicle_param()
91 .max_abs_speed_when_stopped();
92 ADEBUG <<
"adc_speed[" << adc_speed <<
"] distance[" << distance <<
"]";
93 static constexpr double kStopSpeedTolerance = 0.4;
94 static constexpr double kStopDistanceTolerance = 3.0;
95 if (adc_speed <= max_adc_stop_speed + kStopSpeedTolerance &&
96 std::fabs(distance) <= kStopDistanceTolerance) {
97 return FinishStage();
98 }
99 }
100
102}
static const VehicleConfig & GetConfig()
Get the current vehicle configuration.
std::shared_ptr< DependencyInjector > injector_
StageResult ExecuteTaskOnReferenceLine(const common::TrajectoryPoint &planning_start_point, Frame *frame)
int BuildStopDecision(const std::string &stop_wall_id, const double stop_line_s, const double stop_distance, const StopReasonCode &stop_reason_code, const std::vector< std::string > &wait_for_obstacles, const std::string &decision_tag, Frame *const frame, ReferenceLineInfo *const reference_line_info, double stop_wall_width)
optional VehicleParam vehicle_param
optional double front_edge_to_center