planning: stop-sign/traffic-light scenario needs clear stop_done_overlap_id...

planning: stop-sign/traffic-light scenario needs clear stop_done_overlap_id after passing the overlap (3189)

modules/planning/scenarios/scenario_manager.cc

@@ -153,10 +153,15 @@ void ScenarioManager::Observe(const Frame& frame) {
       min_start_s = stop_sign_overlap.start_s;
       PlanningContext::GetScenarioInfo()->next_stop_sign_overlap =
           stop_sign_overlap;
+    } else {
+      // clear stop_done_overlap_id if already passed
+      if (PlanningContext::GetScenarioInfo()->stop_done_overlap_id ==
+          stop_sign_overlap.object_id) {
+        PlanningContext::GetScenarioInfo()->stop_done_overlap_id = "";
+      }
     }
   }
-  ADEBUG
-      << "Stop Sign: "
+  ADEBUG << "Stop Sign: "
       << PlanningContext::GetScenarioInfo()->next_stop_sign_overlap.object_id;
 
   // find next traffic_light_overlap
@@ -170,6 +175,12 @@ void ScenarioManager::Observe(const Frame& frame) {
       min_start_s = traffic_light_overlap.start_s;
       PlanningContext::GetScenarioInfo()->next_traffic_light_overlap =
           traffic_light_overlap;
+    } else {
+      // clear stop_done_overlap_id if already passed
+      if (PlanningContext::GetScenarioInfo()->stop_done_overlap_id ==
+          traffic_light_overlap.object_id) {
+        PlanningContext::GetScenarioInfo()->stop_done_overlap_id = "";
+      }
     }
   }
   ADEBUG << "Traffic Light: "
@@ -189,8 +200,7 @@ void ScenarioManager::Observe(const Frame& frame) {
           crosswalk_overlap;
     }
   }
-  ADEBUG
-      << "Crosswalk: "
+  ADEBUG << "Crosswalk: "
       << PlanningContext::GetScenarioInfo()->next_crosswalk_overlap.object_id;
 }
 

modules/planning/scenarios/stage.cc

@@ -106,6 +106,11 @@ bool Stage::ExecuteTaskOnReferenceLine(
   return true;
 }
 
+Stage::StageStatus Stage::FinishScenario() {
+  next_stage_ = ScenarioConfig::NO_STAGE;
+  return Stage::FINISHED;
+}
+
 }  // namespace scenario
 }  // namespace planning
 }  // namespace apollo

modules/planning/scenarios/stage.h

@@ -82,6 +82,8 @@ class Stage {
   bool ExecuteTaskOnReferenceLine(
       const common::TrajectoryPoint& planning_start_point, Frame* frame);
 
+  virtual Stage::StageStatus FinishScenario();
+
   std::map<TaskConfig::TaskType, std::unique_ptr<Task>> tasks_;
   std::vector<Task*> task_list_;
   ScenarioConfig::StageConfig config_;

modules/planning/scenarios/stop_sign/unprotected/stage_creep.cc

@@ -69,8 +69,7 @@ Stage::StageStatus StageCreep::Process(
                      return overlap.object_id == stop_sign_overlap_id;
                    });
   if (stop_sign_overlap_it == stop_sign_overlaps.end()) {
-    next_stage_ = ScenarioConfig::NO_STAGE;
-    return Stage::FINISHED;
+    return FinishScenario();
   }
 
   const double wait_time =

modules/planning/scenarios/stop_sign/unprotected/stage_intersection_cruise.cc

@@ -62,7 +62,7 @@ Stage::StageStatus StageIntersectionCruise::Process(
                      return overlap.object_id == stop_sign_overlap_id;
                    });
   if (stop_sign_overlap_it == stop_sign_overlaps.end()) {
-    return FinishStage();
+    return FinishScenario();
   }
 
   // check pass intersection
@@ -77,8 +77,7 @@ Stage::StageStatus StageIntersectionCruise::Process(
 }
 
 Stage::StageStatus StageIntersectionCruise::FinishStage() {
-  next_stage_ = ScenarioConfig::NO_STAGE;
-  return Stage::FINISHED;
+  return FinishScenario();
 }
 
 }  // namespace stop_sign

modules/planning/scenarios/stop_sign/unprotected/stage_pre_stop.cc

@@ -73,8 +73,7 @@ Stage::StageStatus StagePreStop::Process(
                      return overlap.object_id == stop_sign_overlap_id;
                    });
   if (stop_sign_overlap_it == stop_sign_overlaps.end()) {
-    next_stage_ = ScenarioConfig::NO_STAGE;
-    return Stage::FINISHED;
+    return FinishScenario();
   }
 
   const double adc_front_edge_s = reference_line_info.AdcSlBoundary().end_s();

modules/planning/scenarios/stop_sign/unprotected/stage_stop.cc

@@ -73,8 +73,7 @@ Stage::StageStatus StageStop::Process(
                      return overlap.object_id == stop_sign_overlap_id;
                    });
   if (stop_sign_overlap_it == stop_sign_overlaps.end()) {
-    next_stage_ = ScenarioConfig::NO_STAGE;
-    return Stage::FINISHED;
+    return FinishScenario();
   }
 
   const double adc_front_edge_s = reference_line_info.AdcSlBoundary().end_s();
@@ -221,6 +220,14 @@ int StageStop::RemoveWatchVehicle(
   return 0;
 }
 
+Stage::StageStatus StageStop::FinishScenario() {
+  PlanningContext::GetScenarioInfo()->stop_done_overlap_id = "";
+  PlanningContext::GetScenarioInfo()->stop_sign_wait_for_obstacles.clear();
+
+  next_stage_ = ScenarioConfig::NO_STAGE;
+  return Stage::FINISHED;
+}
+
 Stage::StageStatus StageStop::FinishStage() {
   PlanningContext::GetScenarioInfo()->stop_done_overlap_id =
       GetContext()->stop_sign_id;

modules/planning/scenarios/stop_sign/unprotected/stage_stop.h

@@ -53,6 +53,7 @@ class StageStop : public Stage {
           watch_vehicles);
 
  private:
+  Stage::StageStatus FinishScenario() override;
   Stage::StageStatus FinishStage();
 
  private:

modules/planning/scenarios/traffic_light/protected/stage_intersection_cruise.cc

@@ -62,7 +62,7 @@ Stage::StageStatus StageIntersectionCruise::Process(
                      return overlap.object_id == traffic_light_overlap_id;
                    });
   if (traffic_light_overlap_it == traffic_light_overlaps.end()) {
-    return FinishStage();
+    return FinishScenario();
   }
 
   // check pass intersection
@@ -77,8 +77,7 @@ Stage::StageStatus StageIntersectionCruise::Process(
 }
 
 Stage::StageStatus StageIntersectionCruise::FinishStage() {
-  next_stage_ = ScenarioConfig::NO_STAGE;
-  return Stage::FINISHED;
+  return FinishScenario();
 }
 
 }  // namespace traffic_light

modules/planning/scenarios/traffic_light/protected/stage_stop.cc

@@ -66,8 +66,7 @@ Stage::StageStatus StageStop::Process(
                      return overlap.object_id == traffic_light_id;
                    });
   if (traffic_light_overlap_it == traffic_light_overlaps.end()) {
-    next_stage_ = ScenarioConfig::NO_STAGE;
-    return Stage::FINISHED;
+    return FinishScenario();
   }
 
   const double adc_front_edge_s = reference_line_info.AdcSlBoundary().end_s();
@@ -89,6 +88,13 @@ Stage::StageStatus StageStop::Process(
   return Stage::RUNNING;
 }
 
+Stage::StageStatus StageStop::FinishScenario() {
+  PlanningContext::GetScenarioInfo()->stop_done_overlap_id = "";
+
+  next_stage_ = ScenarioConfig::NO_STAGE;
+  return Stage::FINISHED;
+}
+
 Stage::StageStatus StageStop::FinishStage() {
   PlanningContext::GetScenarioInfo()->stop_done_overlap_id =
       GetContext()->traffic_light_id;

modules/planning/scenarios/traffic_light/protected/stage_stop.h

@@ -48,6 +48,7 @@ class StageStop : public Stage {
   }
 
  private:
+  Stage::StageStatus FinishScenario() override;
   Stage::StageStatus FinishStage();
 
  private:

modules/planning/scenarios/traffic_light/unprotected_right_turn/stage_creep.cc

@@ -71,8 +71,7 @@ Stage::StageStatus StageCreep::Process(
                      return overlap.object_id == traffic_light_overlap_id;
                    });
   if (traffic_light_overlap_it == traffic_light_overlaps.end()) {
-    next_stage_ = ScenarioConfig::NO_STAGE;
-    return Stage::FINISHED;
+    return FinishScenario();
   }
 
   // check on traffic light color

modules/planning/scenarios/traffic_light/unprotected_right_turn/stage_intersection_cruise.cc

@@ -62,7 +62,7 @@ Stage::StageStatus StageIntersectionCruise::Process(
                      return overlap.object_id == traffic_light_overlap_id;
                    });
   if (traffic_light_overlap_it == traffic_light_overlaps.end()) {
-    return FinishStage();
+    return FinishScenario();
   }
 
   // check pass intersection
@@ -77,8 +77,7 @@ Stage::StageStatus StageIntersectionCruise::Process(
 }
 
 Stage::StageStatus StageIntersectionCruise::FinishStage() {
-  next_stage_ = ScenarioConfig::NO_STAGE;
-  return Stage::FINISHED;
+  return FinishScenario();
 }
 
 }  // namespace traffic_light

modules/planning/scenarios/traffic_light/unprotected_right_turn/stage_stop.cc

@@ -66,8 +66,7 @@ Stage::StageStatus StageStop::Process(
                      return overlap.object_id == traffic_light_id;
                    });
   if (traffic_light_overlap_it == traffic_light_overlaps.end()) {
-    next_stage_ = ScenarioConfig::NO_STAGE;
-    return Stage::FINISHED;
+    return FinishScenario();
   }
 
   const double adc_front_edge_s = reference_line_info.AdcSlBoundary().end_s();
@@ -100,6 +99,13 @@ Stage::StageStatus StageStop::Process(
   return Stage::RUNNING;
 }
 
+Stage::StageStatus StageStop::FinishScenario() {
+  PlanningContext::GetScenarioInfo()->stop_done_overlap_id = "";
+
+  next_stage_ = ScenarioConfig::NO_STAGE;
+  return Stage::FINISHED;
+}
+
 Stage::StageStatus StageStop::FinishStage() {
   PlanningContext::GetScenarioInfo()->stop_done_overlap_id =
       GetContext()->traffic_light_id;

modules/planning/scenarios/traffic_light/unprotected_right_turn/stage_stop.h

@@ -48,6 +48,7 @@ class StageStop : public Stage {
   }
 
  private:
+  Stage::StageStatus FinishScenario() override;
   Stage::StageStatus FinishStage();
 
  private: