planning: combine pull_over status in PlanningContext for both PullOv…

…er and EmergencyPullOver scenarios to share
seekme-ai · Nov 7, 2019 · ee9ab05 · ee9ab05
1 parent dd04fc8
commit ee9ab05
Show file tree

Hide file tree

Showing 7 changed files with 47 additions and 76 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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);