From 27748d8c31accab4398732836f4f79495ac9e86d Mon Sep 17 00:00:00 2001
From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com>
Date: Mon, 10 Jun 2024 21:34:19 +0900
Subject: [PATCH] feat(avoidance): keep enough distance to avoid object in
 front of red signal (#6898)

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
---
 .../behavior_path_avoidance_module/data_structs.hpp       | 3 +++
 .../include/behavior_path_avoidance_module/helper.hpp     | 5 +++++
 planning/behavior_path_avoidance_module/src/scene.cpp     | 4 ++++
 .../src/shift_line_generator.cpp                          | 8 +++++++-
 4 files changed, 19 insertions(+), 1 deletion(-)

diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp
index 7958a8a2dcbd4..77ce52593d776 100644
--- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp
+++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp
@@ -59,6 +59,7 @@ enum class ObjectInfo {
   // unavoidable reasons
   NEED_DECELERATION,
   SAME_DIRECTION_SHIFT,
+  LIMIT_DRIVABLE_SPACE_TEMPORARY,
   INSUFFICIENT_DRIVABLE_SPACE,
   INSUFFICIENT_LONGITUDINAL_DISTANCE,
   INVALID_SHIFT_LINE,
@@ -548,6 +549,8 @@ struct AvoidancePlanningData
   // nearest object that should be avoid
   std::optional<ObjectData> stop_target_object{std::nullopt};
 
+  std::optional<lanelet::ConstLanelet> red_signal_lane{std::nullopt};
+
   // new shift point
   AvoidLineArray new_shift_line{};
 
diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp
index 8de39d00af130..0e6592c6bdefd 100644
--- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp
+++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp
@@ -348,6 +348,11 @@ class AvoidanceHelper
       return false;
     }
 
+    // can avoid it after relax drivable space limitation.
+    if (object.info == ObjectInfo::LIMIT_DRIVABLE_SPACE_TEMPORARY) {
+      return false;
+    }
+
     return true;
   }
 
diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp
index 5d7ff7f233344..e8a7d43107eaa 100644
--- a/planning/behavior_path_avoidance_module/src/scene.cpp
+++ b/planning/behavior_path_avoidance_module/src/scene.cpp
@@ -204,6 +204,9 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat
     utils::avoidance::isWithinLanes(data.current_lanelets, planner_data_);
   const auto red_signal_lane_itr = std::find_if(
     data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) {
+      if (utils::traffic_light::isTrafficSignalStop({lanelet}, planner_data_)) {
+        return true;
+      }
       const auto next_lanes = planner_data_->route_handler->getNextLanelets(lanelet);
       return utils::traffic_light::isTrafficSignalStop(next_lanes, planner_data_);
     });
@@ -220,6 +223,7 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat
           utils::avoidance::generateExpandedDrivableLanes(lanelet, planner_data_, parameters_));
       } else {
         data.drivable_lanes.push_back(utils::avoidance::generateNotExpandedDrivableLanes(lanelet));
+        data.red_signal_lane = lanelet;
       }
     });
 
diff --git a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp
index 84b2b9b61a702..28fb14382093c 100644
--- a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp
+++ b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp
@@ -246,7 +246,13 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline(
   AvoidOutlines outlines;
   for (auto & o : data.target_objects) {
     if (!o.avoid_margin.has_value()) {
-      o.info = ObjectInfo::INSUFFICIENT_DRIVABLE_SPACE;
+      if (!data.red_signal_lane.has_value()) {
+        o.info = ObjectInfo::INSUFFICIENT_DRIVABLE_SPACE;
+      } else if (data.red_signal_lane.value().id() == o.overhang_lanelet.id()) {
+        o.info = ObjectInfo::LIMIT_DRIVABLE_SPACE_TEMPORARY;
+      } else {
+        o.info = ObjectInfo::INSUFFICIENT_DRIVABLE_SPACE;
+      }
       if (o.avoid_required && is_forward_object(o)) {
         break;
       } else {