From c3bec4f93d944085ff48d5e1d3c18f2cd7764243 Mon Sep 17 00:00:00 2001
From: Dong Li <lidong.main@gmail.com>
Date: Wed, 3 Jan 2018 21:00:17 -0800
Subject: [PATCH] planning: add protected mode when have priority in junction

---
 .../planning/common/reference_line_info.cc    | 28 +++++++++++--------
 1 file changed, 17 insertions(+), 11 deletions(-)

diff --git a/modules/planning/common/reference_line_info.cc b/modules/planning/common/reference_line_info.cc
index 8e740e5891..a56bfe1823 100644
--- a/modules/planning/common/reference_line_info.cc
+++ b/modules/planning/common/reference_line_info.cc
@@ -122,13 +122,19 @@ ADCTrajectory::RightOfWayStatus ReferenceLineInfo::GetRightOfWayStatus() const {
     } 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) {
+      if (is_protected != nullptr && *is_protected) {
         return ADCTrajectory::PROTECTED;
       } else {
-        return ADCTrajectory::UNPROTECTED;
+        double junction_s = (overlap.end_s + overlap.start_s) / 2.0;
+        auto ref_point = reference_line_.GetReferencePoint(junction_s);
+        if (ref_point.lane_waypoints().empty()) {
+          return ADCTrajectory::PROTECTED;
+        }
+        for (const auto& waypoint : ref_point.lane_waypoints()) {
+          if (waypoint.lane->lane().turn() == hdmap::Lane::NO_TURN) {
+            return ADCTrajectory::PROTECTED;
+          }
+        }
       }
     }
   }
@@ -574,13 +580,13 @@ void ReferenceLineInfo::ExportEngageAdvice(EngageAdvice* engage_advice) const {
       }
       engage_advice->set_reason("Vehicle heading is not aligned");
     } else {
-        if (vehicle_state_.driving_mode() !=
-            Chassis::DrivingMode::Chassis_DrivingMode_COMPLETE_AUTO_DRIVE) {
-          engage_advice->set_advice(EngageAdvice::READY_TO_ENGAGE);
-        } else {
-          engage_advice->set_advice(EngageAdvice::KEEP_ENGAGED);
-        }
+      if (vehicle_state_.driving_mode() !=
+          Chassis::DrivingMode::Chassis_DrivingMode_COMPLETE_AUTO_DRIVE) {
+        engage_advice->set_advice(EngageAdvice::READY_TO_ENGAGE);
+      } else {
+        engage_advice->set_advice(EngageAdvice::KEEP_ENGAGED);
       }
+    }
   }
   *prev_advice = *engage_advice;
 }
-- 
GitLab