feat(avoidance): keep enough distance to avoid object in front of red signal (#6898)

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

Author: satoshi-ota

planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_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,
@@ -546,6 +547,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{};
 

planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/helper.hpp

@@ -357,6 +357,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;
   }
 

planning/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp

@@ -210,6 +210,9 @@ void StaticObstacleAvoidanceModule::fillFundamentalData(
     utils::static_obstacle_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_);
     });
@@ -229,6 +232,7 @@ void StaticObstacleAvoidanceModule::fillFundamentalData(
       } else {
         data.drivable_lanes.push_back(
           utils::static_obstacle_avoidance::generateNotExpandedDrivableLanes(lanelet));
+        data.red_signal_lane = lanelet;
       }
     });
 

planning/autoware_behavior_path_static_obstacle_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 {