planning: added state tracking for junction right of way

modules/planning/common/BUILD

@@ -186,6 +186,7 @@ cc_library(
         ":planning_gflags",
         "//modules/common:log",
         "//modules/common/proto:pnc_point_proto",
+        "//modules/common/util:dropbox",
         "//modules/common/vehicle_state:vehicle_state_provider",
         "//modules/map/pnc_map",
         "//modules/map/proto:map_proto",

modules/planning/common/reference_line_info.cc

@@ -28,9 +28,11 @@
 #include "modules/planning/proto/sl_boundary.pb.h"
 
 #include "modules/common/configs/vehicle_config_helper.h"
+#include "modules/common/util/dropbox.h"
 #include "modules/common/util/string_util.h"
 #include "modules/common/util/util.h"
 #include "modules/map/hdmap/hdmap_common.h"
+#include "modules/map/hdmap/hdmap_util.h"
 #include "modules/planning/common/planning_gflags.h"
 
 namespace apollo {
@@ -52,6 +54,12 @@ ReferenceLineInfo::ReferenceLineInfo(const common::VehicleState& vehicle_state,
       reference_line_(reference_line),
       lanes_(segments) {}
 
+namespace {
+std::string junction_dropbox_id(const std::string& junction_id) {
+  return "junction_protection_" + junction_id;
+}
+}
+
 bool ReferenceLineInfo::Init() {
   const auto& param = VehicleConfigHelper::GetConfig().vehicle_param();
   const auto& path_point = adc_planning_point_.path_point();
@@ -80,6 +88,42 @@ bool ReferenceLineInfo::Init() {
   return true;
 }
 
+bool WithinOverlap(const hdmap::PathOverlap& overlap, double s) {
+  constexpr double kEpsilon = 1e-2;
+  return overlap.start_s - kEpsilon <= s && s <= overlap.end_s + kEpsilon;
+}
+
+void ReferenceLineInfo::SetJunctionRightOfWay(double junction_s,
+                                              bool is_protected) {
+  auto* junction_store = common::util::Dropbox<bool>::Open();
+  for (const auto& overlap : reference_line_.map_path().junction_overlaps()) {
+    if (WithinOverlap(overlap, junction_s)) {
+      junction_store->Set(junction_dropbox_id(overlap.object_id), is_protected);
+    }
+  }
+}
+
+ADCTrajectory::RightOfWayStatus ReferenceLineInfo::GetRightOfWayStatus() const {
+  auto* junction_store = common::util::Dropbox<bool>::Open();
+  for (const auto& overlap : reference_line_.map_path().junction_overlaps()) {
+    if (overlap.end_s < adc_sl_boundary_.start_s()) {
+      junction_store->Remove(junction_dropbox_id(overlap.object_id));
+    } else if (WithinOverlap(overlap, adc_sl_boundary_.end_s())) {
+      auto* is_protected =
+          junction_store->Get(junction_dropbox_id(overlap.object_id));
+      if (!is_protected) {
+        continue;
+      }
+      if (*is_protected) {
+        return ADCTrajectory::PROTECTED;
+      } else {
+        return ADCTrajectory::UNPROTECTED;
+      }
+    }
+  }
+  return ADCTrajectory::UNPROTECTED;
+}
+
 const hdmap::RouteSegments& ReferenceLineInfo::Lanes() const { return lanes_; }
 
 const SLBoundary& ReferenceLineInfo::AdcSlBoundary() const {

modules/planning/common/reference_line_info.h

@@ -111,10 +111,9 @@ class ReferenceLineInfo {
 
   void ExportDecision(DecisionResult* decision_result) const;
 
-  void SetRightOfWayStatus() { status_ = ADCTrajectory::PROTECTED; }
-  ADCTrajectory::RightOfWayStatus GetRightOfWayStatus() const {
-    return status_;
-  }
+  void SetJunctionRightOfWay(double junction_s, bool is_protected);
+
+  ADCTrajectory::RightOfWayStatus GetRightOfWayStatus() const;
 
  private:
   void ExportTurnSignal(common::VehicleSignal* signal) const;

modules/planning/proto/BUILD

@@ -16,9 +16,9 @@ proto_library(
         "planning.proto",
         "planning_config.proto",
         "planning_internal.proto",
+        "planning_stats.proto",
         "qp_st_speed_config.proto",
         "sl_boundary.proto",
-        "planning_stats.proto",
     ],
     deps = [
         ":dp_poly_path_config_proto_lib",

modules/planning/tasks/traffic_decider/BUILD

@@ -34,6 +34,7 @@ cc_library(
         "//modules/planning/common:frame",
         "//modules/planning/common:planning_gflags",
         "//modules/planning/common:reference_line_info",
+        "//modules/planning/proto:planning_proto",
     ],
 )
 

modules/planning/tasks/traffic_decider/signal_light.cc

@@ -124,9 +124,13 @@ void SignalLight::MakeDecisions(Frame* frame,
       }
       signal_debug->set_is_stop_wall_created(true);
     }
-  }
-  if (!has_stop) {
-    reference_line_info->SetRightOfWayStatus();
+    if (has_stop) {
+      reference_line_info->SetJunctionRightOfWay(signal_light->start_s,
+                                                 false);  // not protected
+    } else {
+      reference_line_info->SetJunctionRightOfWay(signal_light->start_s, true);
+      // is protected
+    }
   }
 }