feat(avoidance): keep enough distance to avoid object in front of red…

… signal (autowarefoundation#6898) (#1335) Signed-off-by: satoshi-ota <[email protected]>
tier4 · Jun 13, 2024 · 85be38e · 85be38e
1 parent cadba8e
commit 85be38e
Show file tree

Hide file tree

Showing 4 changed files with 19 additions and 1 deletion.
diff --git a/...ng/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp b/...ng/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
@@ -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
@@ -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
@@ -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 {