fix(intersection): judge pass judge at intersection without tl with o…

…cclusion detection Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
autowarefoundation · Jan 29, 2024 · 5289b18 · 5289b18
1 parent daf0824
commit 5289b18
Show file tree

Hide file tree

Showing 2 changed files with 36 additions and 11 deletions.
diff --git a/planning/behavior_velocity_intersection_module/src/decision_result.cpp b/planning/behavior_velocity_intersection_module/src/decision_result.cpp
@@ -19,23 +19,23 @@ namespace behavior_velocity_planner::intersection
 std::string formatDecisionResult(const DecisionResult & decision_result)
 {
   if (std::holds_alternative<InternalError>(decision_result)) {
-    const auto state = std::get<InternalError>(decision_result);
+    const auto & state = std::get<InternalError>(decision_result);
     return "InternalError because " + state.error;
   }
   if (std::holds_alternative<OverPassJudge>(decision_result)) {
-    const auto state = std::get<OverPassJudge>(decision_result);
+    const auto & state = std::get<OverPassJudge>(decision_result);
     return "OverPassJudge:\nsafety_report:" + state.safety_report + "\nevasive_report:\n" +
            state.evasive_report;
   }
   if (std::holds_alternative<StuckStop>(decision_result)) {
     return "StuckStop";
   }
   if (std::holds_alternative<YieldStuckStop>(decision_result)) {
-    const auto state = std::get<YieldStuckStop>(decision_result);
+    const auto & state = std::get<YieldStuckStop>(decision_result);
     return "YieldStuckStop:\nsafety_report:" + state.safety_report;
   }
   if (std::holds_alternative<NonOccludedCollisionStop>(decision_result)) {
-    const auto state = std::get<NonOccludedCollisionStop>(decision_result);
+    const auto & state = std::get<NonOccludedCollisionStop>(decision_result);
     return "NonOccludedCollisionStop\nsafety_report:" + state.safety_report;
   }
   if (std::holds_alternative<FirstWaitBeforeOcclusion>(decision_result)) {
@@ -45,18 +45,18 @@ std::string formatDecisionResult(const DecisionResult & decision_result)
     return "PeekingTowardOcclusion";
   }
   if (std::holds_alternative<OccludedCollisionStop>(decision_result)) {
-    const auto state = std::get<OccludedCollisionStop>(decision_result);
+    const auto & state = std::get<OccludedCollisionStop>(decision_result);
     return "OccludedCollisionStop\nsafety_report:" + state.safety_report;
   }
   if (std::holds_alternative<OccludedAbsenceTrafficLight>(decision_result)) {
-    const auto state = std::get<OccludedAbsenceTrafficLight>(decision_result);
+    const auto & state = std::get<OccludedAbsenceTrafficLight>(decision_result);
     return "OccludedAbsenceTrafficLight\nsafety_report:" + state.safety_report;
   }
   if (std::holds_alternative<Safe>(decision_result)) {
     return "Safe";
   }
   if (std::holds_alternative<FullyPrioritized>(decision_result)) {
-    const auto state = std::get<FullyPrioritized>(decision_result);
+    const auto & state = std::get<FullyPrioritized>(decision_result);
     return "FullyPrioritized\nsafety_report:" + state.safety_report;
   }
   return "";

diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp
@@ -354,8 +354,18 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
         : false;
     if (!has_traffic_light_) {
       if (fromEgoDist(occlusion_wo_tl_pass_judge_line_idx) < 0) {
-        return intersection::InternalError{
-          "already passed maximum peeking line in the absence of traffic light"};
+        if (has_collision) {
+          const auto closest_idx = intersection_stoplines.closest_idx;
+          const std::string evasive_diag = generateEgoRiskEvasiveDiagnosis(
+            *path, closest_idx, time_distance_array, too_late_detect_objects, misjudge_objects);
+          return intersection::OverPassJudge{
+            "already passed maximum peeking line in the absence of traffic light.\n" +
+              safety_report,
+            evasive_diag};
+        }
+        return intersection::OverPassJudge{
+          "already passed maximum peeking line in the absence of traffic light safely",
+          "no evasive action required"};
       }
       return intersection::OccludedAbsenceTrafficLight{
         is_occlusion_cleared_with_margin,
@@ -364,7 +374,7 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
         closest_idx,
         first_attention_stopline_idx,
         occlusion_wo_tl_pass_judge_line_idx,
-        safety_report};
+        safety_diag};
     }
 
     // ==========================================================================================
@@ -1251,7 +1261,22 @@ IntersectionModule::PassJudgeStatus IntersectionModule::isOverPassJudgeLinesStat
     return first_pass_judge_line_idx;
   }();
 
-  const bool was_safe = std::holds_alternative<intersection::Safe>(prev_decision_result_);
+  // ==========================================================================================
+  // at intersection without traffic light, this module ignores occlusion even if occlusion is
+  // detected for real, so if collision is not detected in that context, that should be interpreted
+  // as "was_safe"
+  // ==========================================================================================
+  const bool was_safe = [&]() {
+    if (std::holds_alternative<intersection::Safe>(prev_decision_result_)) {
+      return true;
+    }
+    if (std::holds_alternative<intersection::OccludedAbsenceTrafficLight>(prev_decision_result_)) {
+      const auto & state =
+        std::get<intersection::OccludedAbsenceTrafficLight>(prev_decision_result_);
+      return !state.collision_detected;
+    }
+    return false;
+  }();
 
   const bool is_over_1st_pass_judge_line =
     util::isOverTargetIndex(path, closest_idx, current_pose, pass_judge_line_idx);