From 649aecac92284800ac766e1b46be9bb3a5545859 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Wed, 4 Sep 2024 11:43:11 +0900 Subject: [PATCH] fix(static_obstacle_avoidance): ignore objects which has already been decided to avoid (#8754) Signed-off-by: satoshi-ota --- .../src/debug.cpp | 1 + .../src/shift_line_generator.cpp | 12 +++++++++++- 2 files changed, 12 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp index 536e350e50bf1..5c1dbd364da90 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp +++ b/planning/behavior_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/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 e9950b56ce174..10d003fcad90f 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 @@ -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; }