fix(static_obstacle_avoidance): ignore objects which has already been…

… decided to avoid (autowarefoundation#8754) Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
1222-takeshi · Sep 4, 2024 · 649aeca · 649aeca
1 parent 3a6c10c
commit 649aeca
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 1 deletion.
diff --git a/...havior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp b/...havior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp
@@ -561,6 +561,7 @@ MarkerArray createDebugMarkerArray(
     addObjects(data.other_objects, ObjectInfo::DEVIATING_FROM_EGO_LANE);
     addObjects(data.other_objects, ObjectInfo::UNSTABLE_OBJECT);
     addObjects(data.other_objects, ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE);
+    addObjects(data.other_objects, ObjectInfo::INVALID_SHIFT_LINE);
   }
 
   if (parameters->enable_shift_line_marker) {

diff --git a/...nner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp b/...nner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp
@@ -248,6 +248,11 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline(
     return s.start_longitudinal > 0.0 && s.start_longitudinal < s.end_longitudinal;
   };
 
+  const auto is_approved = [this](const auto & object) {
+    return (helper_->getShift(object.getPosition()) > 0.0 && isOnRight(object)) ||
+           (helper_->getShift(object.getPosition()) < 0.0 && !isOnRight(object));
+  };
+
   ObjectDataArray unavoidable_objects;
 
   // target objects are sorted by longitudinal distance.
@@ -284,6 +289,11 @@ 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 (is_approved(o)) {
+        // the avoidance path for this object has already approved
+        o.is_avoidable = true;
+        continue;
+      }
       if (o.avoid_required && is_forward_object(o) && is_on_path(o)) {
         break;
       } else {
@@ -394,7 +404,7 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline(
       outlines.emplace_back(al_avoid, std::nullopt);
     } else if (is_valid_shift_line(al_avoid) && is_valid_shift_line(al_return)) {
       outlines.emplace_back(al_avoid, al_return);
-    } else {
+    } else if (!is_approved(o)) {
       o.info = ObjectInfo::INVALID_SHIFT_LINE;
       continue;
     }