diff --git a/modules/planning/conf/scenario/emergency_pull_over_config.pb.txt b/modules/planning/conf/scenario/emergency_pull_over_config.pb.txt index 65bbc84749a..465e8656691 100644 --- a/modules/planning/conf/scenario/emergency_pull_over_config.pb.txt +++ b/modules/planning/conf/scenario/emergency_pull_over_config.pb.txt @@ -3,6 +3,7 @@ emergency_pull_over_config: { max_stop_deceleration: 3.0 slow_down_deceleration_time: 3.0 target_slow_down_speed: 1.0 + stop_distance: 1.5 } stage_type: EMERGENCY_PULL_OVER_SLOW_DOWN diff --git a/modules/planning/proto/planning_config.proto b/modules/planning/proto/planning_config.proto index b67e4a438a3..e988caabf26 100644 --- a/modules/planning/proto/planning_config.proto +++ b/modules/planning/proto/planning_config.proto @@ -163,6 +163,7 @@ message ScenarioEmergencyPullOverConfig { optional double max_stop_deceleration = 1 [default = 3.0]; optional double slow_down_deceleration_time = 2 [default = 3.0]; // second optional double target_slow_down_speed = 3 [default = 2.5]; // m/s + optional double stop_distance = 4 [default = 1.5]; // meter } message ScenarioValetParkingConfig { diff --git a/modules/planning/scenarios/emergency/emergency_pull_over/stage_approach.cc b/modules/planning/scenarios/emergency/emergency_pull_over/stage_approach.cc index f84ded18335..b01efa3bfb8 100644 --- a/modules/planning/scenarios/emergency/emergency_pull_over/stage_approach.cc +++ b/modules/planning/scenarios/emergency/emergency_pull_over/stage_approach.cc @@ -68,7 +68,7 @@ Stage::StageStatus EmergencyPullOverStageApproach::Process( reference_line.XYToSL({emergency_pull_over_status.position().x(), emergency_pull_over_status.position().y()}, &pull_over_sl); - const double stop_distance = 1.0; + const double stop_distance = scenario_config_.stop_distance(); const double stop_line_s = pull_over_sl.s() + stop_distance + VehicleConfigHelper::GetConfig().vehicle_param().front_edge_to_center(); @@ -76,7 +76,7 @@ Stage::StageStatus EmergencyPullOverStageApproach::Process( const std::vector wait_for_obstacle_ids; planning::util::BuildStopDecision( virtual_obstacle_id, stop_line_s, stop_distance, - StopReasonCode::STOP_REASON_PREPARKING, wait_for_obstacle_ids, + StopReasonCode::STOP_REASON_PULL_OVER, wait_for_obstacle_ids, "EMERGENCY_PULL_OVER-scenario", frame, &(frame->mutable_reference_line_info()->front())); @@ -92,10 +92,10 @@ Stage::StageStatus EmergencyPullOverStageApproach::Process( .vehicle_param() .max_abs_speed_when_stopped(); ADEBUG << "adc_speed[" << adc_speed << "] distance[" << distance << "]"; - constexpr double kStopSpeedTolerance = 0.15; + constexpr double kStopSpeedTolerance = 0.4; constexpr double kStopDistanceTolerance = 0.5; if (adc_speed <= max_adc_stop_speed + kStopSpeedTolerance && - distance <= stop_distance + kStopDistanceTolerance) { + distance <= kStopDistanceTolerance) { return FinishStage(); } } @@ -104,10 +104,12 @@ Stage::StageStatus EmergencyPullOverStageApproach::Process( } Stage::StageStatus EmergencyPullOverStageApproach::FinishStage() { + /* auto* emergency_pull_over = PlanningContext::Instance() ->mutable_planning_status() ->mutable_emergency_pull_over(); emergency_pull_over->set_is_in_emergency_pull_over_scenario(false); + */ next_stage_ = ScenarioConfig::EMERGENCY_PULL_OVER_STANDBY; return Stage::FINISHED; diff --git a/modules/planning/scenarios/emergency/emergency_pull_over/stage_standby.cc b/modules/planning/scenarios/emergency/emergency_pull_over/stage_standby.cc index def16953cc7..b6cee413cd7 100644 --- a/modules/planning/scenarios/emergency/emergency_pull_over/stage_standby.cc +++ b/modules/planning/scenarios/emergency/emergency_pull_over/stage_standby.cc @@ -71,15 +71,22 @@ Stage::StageStatus EmergencyPullOverStageStandby::Process( reference_line.XYToSL({emergency_pull_over_status.position().x(), emergency_pull_over_status.position().y()}, &pull_over_sl); - const double stop_distance = 1.0; - const double stop_line_s = + const double stop_distance = scenario_config_.stop_distance(); + double stop_line_s = pull_over_sl.s() + stop_distance + VehicleConfigHelper::GetConfig().vehicle_param().front_edge_to_center(); + const double adc_front_edge_s = reference_line_info.AdcSlBoundary().end_s(); + double distance = stop_line_s - adc_front_edge_s; + if (distance <= 0.0) { + // push stop fence further + stop_line_s = adc_front_edge_s + stop_distance; + } + const std::string virtual_obstacle_id = "EMERGENCY_PULL_OVER"; const std::vector wait_for_obstacle_ids; planning::util::BuildStopDecision( virtual_obstacle_id, stop_line_s, stop_distance, - StopReasonCode::STOP_REASON_PREPARKING, wait_for_obstacle_ids, + StopReasonCode::STOP_REASON_PULL_OVER, wait_for_obstacle_ids, "EMERGENCY_PULL_OVER-scenario", frame, &(frame->mutable_reference_line_info()->front()));