From 92568927758742806b02ad2252fcf0b9ee3bec31 Mon Sep 17 00:00:00 2001
From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com>
Date: Thu, 18 Jul 2024 15:49:25 +0900
Subject: [PATCH] fix(static_obstacle_avoidance): avoid object behind
 unavoidance object if unavoidable is not on the path (#8066)

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
---
 .../src/shift_line_generator.cpp              | 34 +++++++++++++++----
 1 file changed, 27 insertions(+), 7 deletions(-)

diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp
index c3a67eb074d73..5e58466fa4229 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp
@@ -239,10 +239,18 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline(
 
   const auto is_forward_object = [](const auto & object) { return object.longitudinal > 0.0; };
 
+  const auto is_on_path = [this](const auto & object) {
+    const auto [overhang, point] = object.overhang_points.front();
+    return std::abs(overhang) < 0.5 * data_->parameters.vehicle_width;
+  };
+
   const auto is_valid_shift_line = [](const auto & s) {
     return s.start_longitudinal > 0.0 && s.start_longitudinal < s.end_longitudinal;
   };
 
+  ObjectDataArray unavoidable_objects;
+
+  // target objects are sorted by longitudinal distance.
   AvoidOutlines outlines;
   for (auto & o : data.target_objects) {
     if (!o.avoid_margin.has_value()) {
@@ -253,22 +261,22 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline(
       } else {
         o.info = ObjectInfo::INSUFFICIENT_DRIVABLE_SPACE;
       }
-      if (o.avoid_required && is_forward_object(o)) {
+      if (o.avoid_required && is_forward_object(o) && is_on_path(o)) {
         break;
       } else {
+        unavoidable_objects.push_back(o);
         continue;
       }
     }
 
-    const auto is_object_on_right = utils::static_obstacle_avoidance::isOnRight(o);
     const auto desire_shift_length =
-      helper_->getShiftLength(o, is_object_on_right, o.avoid_margin.value());
-    if (utils::static_obstacle_avoidance::isSameDirectionShift(
-          is_object_on_right, desire_shift_length)) {
+      helper_->getShiftLength(o, isOnRight(o), o.avoid_margin.value());
+    if (utils::static_obstacle_avoidance::isSameDirectionShift(isOnRight(o), desire_shift_length)) {
       o.info = ObjectInfo::SAME_DIRECTION_SHIFT;
-      if (o.avoid_required && is_forward_object(o)) {
+      if (o.avoid_required && is_forward_object(o) && is_on_path(o)) {
         break;
       } else {
+        unavoidable_objects.push_back(o);
         continue;
       }
     }
@@ -276,13 +284,25 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline(
     // calculate feasible shift length based on behavior policy
     const auto feasible_shift_profile = get_shift_profile(o, desire_shift_length);
     if (!feasible_shift_profile.has_value()) {
-      if (o.avoid_required && is_forward_object(o)) {
+      if (o.avoid_required && is_forward_object(o) && is_on_path(o)) {
         break;
       } else {
+        unavoidable_objects.push_back(o);
         continue;
       }
     }
 
+    // If there is an object that cannot be avoided, this module only avoids object on the same side
+    // as unavoidable object.
+    if (!unavoidable_objects.empty()) {
+      if (isOnRight(unavoidable_objects.front()) && !isOnRight(o)) {
+        break;
+      }
+      if (!isOnRight(unavoidable_objects.front()) && isOnRight(o)) {
+        break;
+      }
+    }
+
     // use absolute dist for return-to-center, relative dist from current for avoiding.
     const auto feasible_return_distance =
       helper_->getMaxAvoidanceDistance(feasible_shift_profile.value().first);