planning: stop_sign scenario fix end condition and transferrable condition

modules/planning/scenarios/stop_sign/stop_sign_unprotected/stop_sign_unprotected_intersection_cruise.cc

@@ -38,6 +38,7 @@ namespace planning {
 namespace scenario {
 namespace stop_sign {
 
+using apollo::hdmap::PathOverlap;
 using common::TrajectoryPoint;
 
 Stage::StageStatus StopSignUnprotectedIntersectionCruise::Process(
@@ -45,8 +46,7 @@ Stage::StageStatus StopSignUnprotectedIntersectionCruise::Process(
   ADEBUG << "stage: IntersectionCruise";
   CHECK_NOTNULL(frame);
 
-  if (GetContext()->stop_sign_id !=
-      PlanningContext::GetScenarioInfo()->next_stop_sign_overlap.object_id) {
+  if (CheckPassIntersection(frame)) {
     next_stage_ = ScenarioConfig::NO_STAGE;
     return Stage::FINISHED;
   }
@@ -58,6 +58,33 @@ Stage::StageStatus StopSignUnprotectedIntersectionCruise::Process(
   return Stage::RUNNING;
 }
 
+bool StopSignUnprotectedIntersectionCruise::CheckPassIntersection(
+    Frame* frame) {
+  // TODO(all): update when pnc-junction is ready
+  constexpr double kIntersectionLength = 10.0;  // unit: m
+
+  auto& reference_line_info = frame->mutable_reference_line_info()->front();
+  double stop_sign_oberlap_end_s = 0;
+  const std::vector<PathOverlap>& stop_sign_overlaps =
+      reference_line_info.reference_line().map_path().stop_sign_overlaps();
+  for (const PathOverlap& stop_sign_overlap : stop_sign_overlaps) {
+    if (GetContext()->stop_sign_id == stop_sign_overlap.object_id) {
+      stop_sign_oberlap_end_s = stop_sign_overlap.end_s;
+      break;
+    }
+  }
+
+  const double adc_back_edge_s = reference_line_info.AdcSlBoundary().start_s();
+  ADEBUG << "adc_back_edge_s[" << adc_back_edge_s
+      << "] stop_sign_overlap_end_s[" << stop_sign_oberlap_end_s << "]";
+  if (adc_back_edge_s - stop_sign_oberlap_end_s >
+      kIntersectionLength) {
+    return true;
+  }
+  return false;
+}
+
+
 }  // namespace stop_sign
 }  // namespace scenario
 }  // namespace planning

modules/planning/scenarios/stop_sign/stop_sign_unprotected/stop_sign_unprotected_intersection_cruise.h

@@ -36,8 +36,24 @@ namespace stop_sign {
 
 struct StopSignUnprotectedContext;
 
-DECLARE_STAGE(StopSignUnprotectedIntersectionCruise,
-              StopSignUnprotectedContext);
+class StopSignUnprotectedIntersectionCruise : public Stage {
+ public:
+  explicit StopSignUnprotectedIntersectionCruise(
+      const ScenarioConfig::StageConfig& config) : Stage(config) {}
+
+ private:
+  Stage::StageStatus Process(const common::TrajectoryPoint& planning_init_point,
+                             Frame* frame) override;
+
+  StopSignUnprotectedContext* GetContext() {
+    return GetContextAs<StopSignUnprotectedContext>();
+  }
+
+  bool CheckPassIntersection(Frame* frame);
+
+ private:
+  ScenarioStopSignUnprotectedConfig scenario_config_;
+};
 
 }  // namespace stop_sign
 }  // namespace scenario

modules/planning/scenarios/stop_sign/stop_sign_unprotected/stop_sign_unprotected_scenario.cc

@@ -131,11 +131,11 @@ bool StopSignUnprotectedScenario::IsTransferable(
     const Scenario& current_scenario,
     const common::TrajectoryPoint& ego_point,
     const Frame& frame) {
-  const std::string stop_sign_overlap_id =
-      PlanningContext::GetScenarioInfo()->next_stop_sign_overlap.object_id;
-  if (stop_sign_overlap_id.empty()) {
-    return false;
-  }
+  // const std::string stop_sign_overlap_id =
+  //     PlanningContext::GetScenarioInfo()->next_stop_sign_overlap.object_id;
+  // if (stop_sign_overlap_id.empty()) {
+  //   return false;
+  // }
 
   const auto& reference_line_info = frame.reference_line_info().front();
   const double adc_front_edge_s = reference_line_info.AdcSlBoundary().end_s();
@@ -149,23 +149,28 @@ bool StopSignUnprotectedScenario::IsTransferable(
       static_cast<uint32_t>(ceil(
           adc_distance_to_stop_sign / adc_speed)) : 0;
   ADEBUG << "adc_distance_to_stop_sign[" << adc_distance_to_stop_sign
+      << "] stop_sign_overlap_start_s[" << stop_sign_overlap_start_s
       << "] adc_speed[" << adc_speed
       << "] time_distance[" << time_distance << "]";
+  ADEBUG << "IsTransferable: current: " << current_scenario.Name()
+      << "; status: " << current_scenario.GetStatus();
 
   switch (current_scenario.scenario_type()) {
     case ScenarioConfig::LANE_FOLLOW:
     case ScenarioConfig::CHANGE_LANE:
     case ScenarioConfig::SIDE_PASS:
     case ScenarioConfig::APPROACH:
-      return (adc_distance_to_stop_sign <=
-          config_.stop_sign_unprotected_config().
-              start_stop_sign_scenario_distance() ||
-          time_distance <= config_.stop_sign_unprotected_config().
-              start_stop_sign_scenario_timer());
+      return (adc_distance_to_stop_sign > 0 &&
+          (adc_distance_to_stop_sign <=
+              config_.stop_sign_unprotected_config().
+                  start_stop_sign_scenario_distance() ||
+           time_distance <= config_.stop_sign_unprotected_config().
+              start_stop_sign_scenario_timer()));
     case ScenarioConfig::STOP_SIGN_PROTECTED:
       return false;
     case ScenarioConfig::STOP_SIGN_UNPROTECTED:
-      return true;
+      return (current_scenario.GetStatus() !=
+              Scenario::ScenarioStatus::STATUS_DONE);
     case ScenarioConfig::TRAFFIC_LIGHT_LEFT_TURN_PROTECTED:
     case ScenarioConfig::TRAFFIC_LIGHT_LEFT_TURN_UNPROTECTED:
     case ScenarioConfig::TRAFFIC_LIGHT_RIGHT_TURN_PROTECTED: