From ee9ab05df629ce6bb4dca85ddda262df5c943cc7 Mon Sep 17 00:00:00 2001 From: jmtao Date: Wed, 6 Nov 2019 18:41:00 -0800 Subject: [PATCH] planning: combine pull_over status in PlanningContext for both PullOver and EmergencyPullOver scenarios to share --- .../planning/proto/planning_internal.proto | 2 -- modules/planning/proto/planning_status.proto | 35 ++++++++---------- .../emergency_pull_over/stage_approach.cc | 25 +++++-------- .../emergency_pull_over/stage_slow_down.cc | 8 ++--- .../emergency_pull_over/stage_standby.cc | 14 ++++---- .../planning/scenarios/scenario_manager.cc | 36 +++++++------------ modules/planning/scenarios/scenario_manager.h | 3 -- 7 files changed, 47 insertions(+), 76 deletions(-) diff --git a/modules/planning/proto/planning_internal.proto b/modules/planning/proto/planning_internal.proto index 77e9ee96eeb..0337c3a05ed 100644 --- a/modules/planning/proto/planning_internal.proto +++ b/modules/planning/proto/planning_internal.proto @@ -200,9 +200,7 @@ message PlanningData { optional ScenarioDebug scenario = 26; optional OpenSpaceDebug open_space = 27; optional SmootherDebug smoother = 28; - optional apollo.planning.PullOverStatus pull_over_status = 29; - optional apollo.planning.EmergencyPullOverStatus emergency_pull_over_status = 30; } message LatticeStPixel { diff --git a/modules/planning/proto/planning_status.proto b/modules/planning/proto/planning_status.proto index b14744279df..0d3cb3b5f06 100644 --- a/modules/planning/proto/planning_status.proto +++ b/modules/planning/proto/planning_status.proto @@ -57,12 +57,6 @@ message DestinationStatus { optional bool has_passed_destination = 1 [default = false]; } -message EmergencyPullOverStatus { - optional apollo.common.PointENU position = 1; - optional double theta = 2; - optional bool is_in_emergency_pull_over_scenario = 3 [default = false]; -} - message OpenSpaceStatus { repeated string partitioned_trajectories_index_history = 1; optional bool position_init = 2 [default = false]; @@ -155,19 +149,18 @@ message PlanningStatus { optional ChangeLaneStatus change_lane = 2; optional CrosswalkStatus crosswalk = 3; optional DestinationStatus destination = 4; - optional EmergencyPullOverStatus emergency_pull_over = 5; - optional apollo.common.EngageAdvice engage_advice = 6; - optional OpenSpaceStatus open_space = 7; - optional PedestrianStatus pedestrian = 8; - optional ParkAndGoStatus park_and_go = 9; - optional PathDeciderStatus path_decider = 10; - optional PathReuseDeciderStatus path_reuse_decider = 11; - optional PullOverStatus pull_over = 12; - optional ReroutingStatus rerouting = 13; - optional RightOfWayStatus right_of_way = 14; - optional ScenarioStatus scenario = 15; - optional SidePassStatus side_pass = 16; - optional StopSignStatus stop_sign = 17; - optional TrafficLightStatus traffic_light = 18; - optional YieldSignStatus yield_sign = 19; + optional apollo.common.EngageAdvice engage_advice = 5; + optional OpenSpaceStatus open_space = 6; + optional PedestrianStatus pedestrian = 7; + optional ParkAndGoStatus park_and_go = 8; + optional PathDeciderStatus path_decider = 9; + optional PathReuseDeciderStatus path_reuse_decider = 10; + optional PullOverStatus pull_over = 11; + optional ReroutingStatus rerouting = 12; + optional RightOfWayStatus right_of_way = 13; + optional ScenarioStatus scenario = 14; + optional SidePassStatus side_pass = 15; + optional StopSignStatus stop_sign = 16; + optional TrafficLightStatus traffic_light = 17; + optional YieldSignStatus yield_sign = 18; } 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 b01efa3bfb8..6bb4f964f33 100644 --- a/modules/planning/scenarios/emergency/emergency_pull_over/stage_approach.cc +++ b/modules/planning/scenarios/emergency/emergency_pull_over/stage_approach.cc @@ -57,16 +57,16 @@ Stage::StageStatus EmergencyPullOverStageApproach::Process( } // add a stop fence - const auto& emergency_pull_over_status = - PlanningContext::Instance()->planning_status().emergency_pull_over(); - if (emergency_pull_over_status.has_position() && - emergency_pull_over_status.position().has_x() && - emergency_pull_over_status.position().has_y()) { + const auto& pull_over_status = + PlanningContext::Instance()->planning_status().pull_over(); + if (pull_over_status.has_position() && + pull_over_status.position().has_x() && + pull_over_status.position().has_y()) { const auto& reference_line_info = frame->reference_line_info().front(); const auto& reference_line = reference_line_info.reference_line(); common::SLPoint pull_over_sl; - reference_line.XYToSL({emergency_pull_over_status.position().x(), - emergency_pull_over_status.position().y()}, + reference_line.XYToSL({pull_over_status.position().x(), + pull_over_status.position().y()}, &pull_over_sl); const double stop_distance = scenario_config_.stop_distance(); const double stop_line_s = @@ -93,9 +93,9 @@ Stage::StageStatus EmergencyPullOverStageApproach::Process( .max_abs_speed_when_stopped(); ADEBUG << "adc_speed[" << adc_speed << "] distance[" << distance << "]"; constexpr double kStopSpeedTolerance = 0.4; - constexpr double kStopDistanceTolerance = 0.5; + constexpr double kStopDistanceTolerance = 3.0; if (adc_speed <= max_adc_stop_speed + kStopSpeedTolerance && - distance <= kStopDistanceTolerance) { + std::fabs(distance) <= kStopDistanceTolerance) { return FinishStage(); } } @@ -104,13 +104,6 @@ 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_slow_down.cc b/modules/planning/scenarios/emergency/emergency_pull_over/stage_slow_down.cc index dafe3401450..2a9ef71935e 100644 --- a/modules/planning/scenarios/emergency/emergency_pull_over/stage_slow_down.cc +++ b/modules/planning/scenarios/emergency/emergency_pull_over/stage_slow_down.cc @@ -79,10 +79,10 @@ Stage::StageStatus EmergencyPullOverStageSlowDown::Process( } Stage::StageStatus EmergencyPullOverStageSlowDown::FinishStage() { - auto* emergency_pull_over = PlanningContext::Instance() - ->mutable_planning_status() - ->mutable_emergency_pull_over(); - emergency_pull_over->set_is_in_emergency_pull_over_scenario(true); + auto* pull_over_status = PlanningContext::Instance() + ->mutable_planning_status() + ->mutable_pull_over(); + pull_over_status->set_plan_pull_over_path(true); next_stage_ = ScenarioConfig::EMERGENCY_PULL_OVER_APPROACH; 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 b6cee413cd7..00515660d1d 100644 --- a/modules/planning/scenarios/emergency/emergency_pull_over/stage_standby.cc +++ b/modules/planning/scenarios/emergency/emergency_pull_over/stage_standby.cc @@ -60,16 +60,16 @@ Stage::StageStatus EmergencyPullOverStageStandby::Process( } // add a stop fence - const auto& emergency_pull_over_status = - PlanningContext::Instance()->planning_status().emergency_pull_over(); - if (emergency_pull_over_status.has_position() && - emergency_pull_over_status.position().has_x() && - emergency_pull_over_status.position().has_y()) { + const auto& pull_over_status = + PlanningContext::Instance()->planning_status().pull_over(); + if (pull_over_status.has_position() && + pull_over_status.position().has_x() && + pull_over_status.position().has_y()) { const auto& reference_line_info = frame->reference_line_info().front(); const auto& reference_line = reference_line_info.reference_line(); common::SLPoint pull_over_sl; - reference_line.XYToSL({emergency_pull_over_status.position().x(), - emergency_pull_over_status.position().y()}, + reference_line.XYToSL({pull_over_status.position().x(), + pull_over_status.position().y()}, &pull_over_sl); const double stop_distance = scenario_config_.stop_distance(); double stop_line_s = diff --git a/modules/planning/scenarios/scenario_manager.cc b/modules/planning/scenarios/scenario_manager.cc index 61ee95ff215..a8ecd4f29b4 100644 --- a/modules/planning/scenarios/scenario_manager.cc +++ b/modules/planning/scenarios/scenario_manager.cc @@ -909,10 +909,7 @@ void ScenarioManager::UpdatePlanningContext( // BareIntersection scenario UpdatePlanningContextBareIntersectionScenario(frame, scenario_type); - // EmergencyPullOver scenario - UpdatePlanningContextEmergencyPullOverScenario(frame, scenario_type); - - // PullOver scenario + // PullOver & EmergencyPullOver scenarios UpdatePlanningContextPullOverScenario(frame, scenario_type); // StopSign scenario @@ -1118,18 +1115,22 @@ void ScenarioManager::UpdatePlanningContextYieldSignScenario( // update: pull_over status in PlanningContext void ScenarioManager::UpdatePlanningContextPullOverScenario( const Frame& frame, const ScenarioConfig::ScenarioType& scenario_type) { + auto* pull_over = PlanningContext::Instance() + ->mutable_planning_status() + ->mutable_pull_over(); if (scenario_type == ScenarioConfig::PULL_OVER) { - PlanningContext::Instance() - ->mutable_planning_status() - ->mutable_pull_over() - ->set_is_in_pull_over_scenario(true); + pull_over->set_pull_over_type(PullOverStatus::PULL_OVER); + pull_over->set_plan_pull_over_path(true); + return; + } else if (scenario_type == ScenarioConfig::EMERGENCY_PULL_OVER) { + pull_over->set_pull_over_type(PullOverStatus::EMERGENCY_PULL_OVER); return; } - PlanningContext::Instance() - ->mutable_planning_status() - ->mutable_pull_over() - ->set_is_in_pull_over_scenario(false); + pull_over->set_plan_pull_over_path(false); + + // check pull_over_status left behind + // keep it if close to destination, to keep stop fence const auto& pull_over_status = PlanningContext::Instance()->planning_status().pull_over(); if (pull_over_status.has_position() && pull_over_status.position().has_x() && @@ -1161,17 +1162,6 @@ void ScenarioManager::UpdatePlanningContextPullOverScenario( } } -// update: emergency_pull_over status in PlanningContext -void ScenarioManager::UpdatePlanningContextEmergencyPullOverScenario( - const Frame& frame, const ScenarioConfig::ScenarioType& scenario_type) { - if (scenario_type != ScenarioConfig::EMERGENCY_PULL_OVER) { - auto* emergency_pull_over = PlanningContext::Instance() - ->mutable_planning_status() - ->mutable_emergency_pull_over(); - emergency_pull_over->Clear(); - } -} - } // namespace scenario } // namespace planning } // namespace apollo diff --git a/modules/planning/scenarios/scenario_manager.h b/modules/planning/scenarios/scenario_manager.h index 84613719da9..e03aa9621aa 100644 --- a/modules/planning/scenarios/scenario_manager.h +++ b/modules/planning/scenarios/scenario_manager.h @@ -87,9 +87,6 @@ class ScenarioManager final { void UpdatePlanningContextBareIntersectionScenario( const Frame& frame, const ScenarioConfig::ScenarioType& scenario_type); - void UpdatePlanningContextEmergencyPullOverScenario( - const Frame& frame, const ScenarioConfig::ScenarioType& scenario_type); - void UpdatePlanningContextPullOverScenario( const Frame& frame, const ScenarioConfig::ScenarioType& scenario_type);