planning: improvements on emergency_pull_over scenario

seekme-ai · Nov 6, 2019 · 94759d1 · 94759d1
1 parent b0f0638
commit 94759d1
Show file tree

Hide file tree

Showing 4 changed files with 18 additions and 7 deletions.
diff --git 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
@@ -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
@@ -68,15 +68,15 @@ 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();
     const std::string virtual_obstacle_id = "EMERGENCY_PULL_OVER";
     const std::vector<std::string> 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
@@ -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<std::string> 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()));