fix(avoidance): fix visualization bug (autowarefoundation#3586)

fix(avoidance): ignore objects whose shift line length is less than threshold Signed-off-by: satoshi-ota <[email protected]>
satoshi-ota · May 8, 2023 · 161e23d · 161e23d
1 parent ca9e277
commit 161e23d
Show file tree

Hide file tree

Showing 3 changed files with 38 additions and 0 deletions.
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp
@@ -34,6 +34,8 @@ bool isTargetObjectType(
 double calcShiftLength(
   const bool & is_object_on_right, const double & overhang_dist, const double & avoid_margin);
 
+bool isShiftNecessary(const bool & is_object_on_right, const double & shift_length);
+
 bool isSameDirectionShift(const bool & is_object_on_right, const double & shift_length);
 
 size_t findPathIndexFromArclength(

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
@@ -3313,6 +3313,7 @@ void AvoidanceModule::updateDebugMarker(
   add(createOtherObjectsMarkerArray(data.other_objects, AvoidanceDebugFactor::NOT_PARKING_OBJECT));
   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(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
@@ -96,6 +96,32 @@ double calcShiftLength(
   return std::fabs(shift_length) > 1e-3 ? shift_length : 0.0;
 }
 
+bool isShiftNecessary(const bool & is_object_on_right, const double & shift_length)
+{
+  /**
+   *   ^
+   *   |
+   * --+----x-------------------------------x--->
+   *   |                 x     x
+   *   |                 ==obj==
+   */
+  if (is_object_on_right && shift_length < 0.0) {
+    return false;
+  }
+
+  /**
+   *   ^                 ==obj==
+   *   |                 x     x
+   * --+----x-------------------------------x--->
+   *   |
+   */
+  if (!is_object_on_right && shift_length > 0.0) {
+    return false;
+  }
+
+  return true;
+}
+
 bool isSameDirectionShift(const bool & is_object_on_right, const double & shift_length)
 {
   return (is_object_on_right == std::signbit(shift_length));
@@ -683,6 +709,15 @@ void filterTargetObjects(
       return std::min(max_allowable_lateral_distance, max_avoid_margin);
     }();
 
+    if (!!avoid_margin) {
+      const auto shift_length = calcShiftLength(isOnRight(o), o.overhang_dist, avoid_margin.get());
+      if (!isShiftNecessary(isOnRight(o), shift_length)) {
+        o.reason = "NotNeedAvoidance";
+        data.other_objects.push_back(o);
+        continue;
+      }
+    }
+
     // force avoidance for stopped vehicle
     {
       const auto to_traffic_light =