planning: add wait_for_obstacle to stop_sign's STOP decision

modules/planning/conf/traffic_rule_config.pb.txt

@@ -105,7 +105,7 @@ config: {
     max_stop_deceleration: 6.0
     min_pass_s_distance: 3.0
     max_stop_speed: 0.3
-    max_valid_stop_distance: 3.0
+    max_valid_stop_distance: 3.5
     stop_duration : 1.0
     watch_vehicle_max_valid_stop_speed: 0.5
     watch_vehicle_max_valid_stop_distance: 5.0

modules/planning/integration_tests/sunnyvale_big_loop_test.cc

@@ -233,6 +233,7 @@ TEST_F(SunnyvaleBigLoopTest, stop_sign_06) {
 
   // set PlanningStatus
   auto* stop_sign_status = GetPlanningStatus()->mutable_stop_sign();
+  stop_sign_status->set_status(StopSignStatus::STOP);
   double stop_start_time = Clock::NowInSeconds() - wait_time;
   stop_sign_status->set_stop_start_time(stop_start_time);
 
@@ -256,6 +257,7 @@ TEST_F(SunnyvaleBigLoopTest, stop_sign_06) {
   // previously watch vehicles are gone
 
   // set PlanningStatus
+  stop_sign_status->set_status(StopSignStatus::STOP);
   stop_start_time = Clock::NowInSeconds() - wait_time;
   stop_sign_status->set_stop_start_time(stop_start_time);
 

modules/planning/proto/decision.proto

@@ -41,6 +41,7 @@ message ObjectStop {
   optional apollo.common.PointENU stop_point =3;
   // When stopped, the heading of the vehicle should be stop_heading.
   optional double stop_heading = 4;
+  repeated string wait_for_obstacle = 5;
 }
 
 // dodge the obstacle in lateral direction when driving
@@ -88,9 +89,6 @@ message ObjectSidePass {
 message ObjectAvoid {
 }
 
-message ObjectWait {
-}
-
 message ObjectDecisionType {
   oneof object_tag {
     ObjectIgnore ignore = 1;
@@ -101,7 +99,6 @@ message ObjectDecisionType {
     ObjectNudge nudge = 6;
     ObjectSidePass sidepass = 7;
     ObjectAvoid avoid = 8;
-    ObjectWait wait = 9;
   }
 }
 

modules/planning/tasks/traffic_decider/stop_sign.cc

@@ -122,7 +122,8 @@ void StopSign::MakeDecisions(Frame* frame,
             reference_line_info->AdcSlBoundary().end_s());
     BuildStopDecision(frame, reference_line_info,
                       const_cast<PathOverlap*>(next_overlap),
-                      config_.stop_sign().creep().stop_distance());
+                      config_.stop_sign().creep().stop_distance(),
+                      nullptr);
   } else {
     // stop decision
     double stop_deceleration = util::GetADCStopDeceleration(
@@ -131,13 +132,10 @@ void StopSign::MakeDecisions(Frame* frame,
     if (stop_deceleration < config_.stop_sign().max_stop_deceleration()) {
       BuildStopDecision(frame, reference_line_info,
                         const_cast<PathOverlap*>(next_stop_sign_overlap_),
-                        config_.stop_sign().stop_distance());
+                        config_.stop_sign().stop_distance(),
+                        &watch_vehicles);
     }
     ADEBUG << "stop_sign_id[" << stop_sign_id << "] STOP";
-
-    if (stop_status_ == StopSignStatus::WAIT) {
-      // wait decision(s)
-    }
   }
 }
 
@@ -228,6 +226,18 @@ int StopSign::GetAssociatedLanes(const StopSignInfo& stop_sign_info) {
   return 0;
 }
 
+/**
+ * @brief: check if next step of CREEP or not
+ */
+bool StopSign::CheckCreep(const hdmap::StopSignInfo& stop_sign_info) {
+  if (config_.stop_sign().creep().enabled() &&
+      (stop_sign_info.stop_sign().type() == hdmap::StopSign::ONE_WAY ||
+       stop_sign_info.stop_sign().type() == hdmap::StopSign::TWO_WAY)) {
+    return true;
+  }
+  return false;
+}
+
 /**
  * @brief: process & update stop status
  *         UNKNOWN/DRIVE -> STOP -> WAIT -> CREEP -> DONE
@@ -480,7 +490,7 @@ int StopSign::AddWatchVehicle(const PathObstacle& path_obstacle,
   auto over_lap_info = assoc_lane_it->second.get()->GetObjectOverlapInfo(
       obstacle_lane.get()->id());
   if (over_lap_info == nullptr) {
-    ADEBUG << "can't find over_lap_info for id: " << obstable_lane_id;
+    AERROR << "can't find over_lap_info for id: " << obstable_lane_id;
     return -1;
   }
   double stop_line_s = over_lap_info->lane_overlap_info().start_s();
@@ -632,8 +642,9 @@ int StopSign::RemoveWatchVehicle(
   return 0;
 }
 
-int StopSign::ClearWatchVehicle(ReferenceLineInfo* const reference_line_info,
-                                StopSignLaneVehicles* watch_vehicles) {
+int StopSign::ClearWatchVehicle(
+    ReferenceLineInfo* const reference_line_info,
+    StopSignLaneVehicles* watch_vehicles) {
   CHECK_NOTNULL(reference_line_info);
   CHECK_NOTNULL(watch_vehicles);
 
@@ -686,10 +697,15 @@ int StopSign::ClearWatchVehicle(ReferenceLineInfo* const reference_line_info,
   return 0;
 }
 
-bool StopSign::BuildStopDecision(Frame* frame,
-                                 ReferenceLineInfo* const reference_line_info,
-                                 PathOverlap* const overlap,
-                                 const double stop_distance) {
+/**
+ * @brief: build stop decision
+ */
+int StopSign::BuildStopDecision(
+    Frame* frame,
+    ReferenceLineInfo* const reference_line_info,
+    PathOverlap* const overlap,
+    const double stop_distance,
+    StopSignLaneVehicles* watch_vehicles) {
   CHECK_NOTNULL(frame);
   CHECK_NOTNULL(reference_line_info);
   CHECK_NOTNULL(overlap);
@@ -698,7 +714,7 @@ bool StopSign::BuildStopDecision(Frame* frame,
   const auto& reference_line = reference_line_info->reference_line();
   if (!WithinBound(0.0, reference_line.Length(), overlap->start_s)) {
     ADEBUG << "stop_sign " << overlap->object_id << " is not on reference line";
-    return true;
+    return 0;
   }
 
   // create virtual stop wall
@@ -707,12 +723,12 @@ bool StopSign::BuildStopDecision(Frame* frame,
       reference_line_info, virtual_obstacle_id, overlap->start_s);
   if (!obstacle) {
     AERROR << "Failed to create obstacle [" << virtual_obstacle_id << "]";
-    return false;
+    return -1;
   }
   PathObstacle* stop_wall = reference_line_info->AddObstacle(obstacle);
   if (!stop_wall) {
     AERROR << "Failed to create path_obstacle for: " << virtual_obstacle_id;
-    return false;
+    return -1;
   }
 
   // build stop decision
@@ -729,20 +745,23 @@ bool StopSign::BuildStopDecision(Frame* frame,
   stop_decision->mutable_stop_point()->set_y(stop_point.y());
   stop_decision->mutable_stop_point()->set_z(0.0);
 
+  if (stop_status_ == StopSignStatus::WAIT) {
+    if (watch_vehicles != nullptr && !watch_vehicles->empty()) {
+      for (auto it = watch_vehicles->begin();
+          it != watch_vehicles->end(); ++it) {
+        for (size_t i = 0; i < it->second.size(); ++i) {
+          std::string obstacle_id = it->second[i];
+          stop_decision->add_wait_for_obstacle(obstacle_id);
+        }
+      }
+    }
+  }
+
   auto* path_decision = reference_line_info->path_decision();
   path_decision->AddLongitudinalDecision(
       TrafficRuleConfig::RuleId_Name(config_.rule_id()), stop_wall->Id(), stop);
 
-  return true;
-}
-
-bool StopSign::CheckCreep(const hdmap::StopSignInfo& stop_sign_info) {
-  if (config_.stop_sign().creep().enabled() &&
-      (stop_sign_info.stop_sign().type() == hdmap::StopSign::ONE_WAY ||
-       stop_sign_info.stop_sign().type() == hdmap::StopSign::TWO_WAY)) {
-    return true;
-  }
-  return false;
+  return 0;
 }
 
 }  // namespace planning

modules/planning/tasks/traffic_decider/stop_sign.h

@@ -48,6 +48,7 @@ class StopSign : public TrafficRule {
                      ReferenceLineInfo* const reference_line_info);
   bool FindNextStopSign(ReferenceLineInfo* const reference_line_info);
   int GetAssociatedLanes(const hdmap::StopSignInfo& stop_sign_info);
+  bool CheckCreep(const hdmap::StopSignInfo& stop_sign_info);
   int ProcessStopStatus(ReferenceLineInfo* const reference_line_info,
                         const hdmap::StopSignInfo& stop_sign_info,
                         StopSignLaneVehicles* watch_vehicles);
@@ -62,11 +63,11 @@ class StopSign : public TrafficRule {
                          StopSignLaneVehicles* watch_vehicles);
   int ClearWatchVehicle(ReferenceLineInfo* const reference_line_info,
                         StopSignLaneVehicles* watch_vehicles);
-  bool BuildStopDecision(Frame* const frame,
-                         ReferenceLineInfo* const reference_line_info,
-                         hdmap::PathOverlap* const overlap,
-                         const double stop_distance);
-  bool CheckCreep(const hdmap::StopSignInfo& stop_sign_info);
+  int BuildStopDecision(Frame* const frame,
+                        ReferenceLineInfo* const reference_line_info,
+                        hdmap::PathOverlap* const overlap,
+                        const double stop_distance,
+                        StopSignLaneVehicles* watch_vehicles);
 
  private:
   static constexpr char const* const STOP_SIGN_VO_ID_PREFIX = "SS_";

modules/planning/testdata/sunnyvale_big_loop_test/result_stop_sign_06_1.pb.txt

@@ -1750,6 +1750,7 @@ decision {
             z: 0
           }
           stop_heading: -2.0433045867444397
+          wait_for_obstacle: "4059"
         }
       }
     }