From a2abc68813a95f425c48b1d4d5e9fcd5c9107f8b Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Sat, 29 Apr 2023 16:17:03 +0900 Subject: [PATCH] fix(avoidance): ignore objects whose shift length is less than execution threshold Signed-off-by: satoshi-ota --- .../src/scene_module/avoidance/avoidance_module.cpp | 1 + .../behavior_path_planner/src/utils/avoidance/utils.cpp | 6 ++++++ 2 files changed, 7 insertions(+) diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index d028fc9fc3800..e6e8e5ae740f6 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -3314,6 +3314,7 @@ void AvoidanceModule::updateDebugMarker( add(createOtherObjectsMarkerArray(data.other_objects, std::string("MovingObject"))); add(createOtherObjectsMarkerArray(data.other_objects, std::string("OutOfTargetArea"))); add(createOtherObjectsMarkerArray(data.other_objects, std::string("NotNeedAvoidance"))); + add(createOtherObjectsMarkerArray(data.other_objects, std::string("LessThanExecutionThreshold"))); add(makeOverhangToRoadShoulderMarkerArray(data.target_objects, "overhang")); add(createOverhangFurthestLineStringMarkerArray( diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index 893d15f9bb5ea..7af9a5b33b904 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -716,6 +716,12 @@ void filterTargetObjects( data.other_objects.push_back(o); continue; } + + if (std::abs(shift_length) < parameters->avoidance_execution_lateral_threshold) { + o.reason = "LessThanExecutionThreshold"; + data.other_objects.push_back(o); + continue; + } } // force avoidance for stopped vehicle