feat(avoidance): separate debug marker namespace for each ignore reas…

…on (autowarefoundation#3507) Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> Signed-off-by: Mingyu Li <mingyu.li@tier4.jp>
Mingyu1991 · Jun 26, 2023 · 179298a · 179298a
1 parent 32ee4a5
commit 179298a
Show file tree

Hide file tree

Showing 3 changed files with 36 additions and 8 deletions.
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_util/avoidance/debug.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_util/avoidance/debug.hpp
@@ -64,7 +64,7 @@ MarkerArray createAvoidableTargetObjectsMarkerArray(
 MarkerArray createUnavoidableTargetObjectsMarkerArray(
   const ObjectDataArray & objects, std::string && ns);
 
-MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns);
+MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, const std::string & ns);
 
 MarkerArray createUnsafeObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns);
 

diff --git a/planning/behavior_path_planner/src/marker_util/avoidance/debug.cpp b/planning/behavior_path_planner/src/marker_util/avoidance/debug.cpp
@@ -412,18 +412,33 @@ MarkerArray createUnavoidableTargetObjectsMarkerArray(
 }
 
 MarkerArray createOtherObjectsMarkerArray(
-  const behavior_path_planner::ObjectDataArray & objects, std::string && ns)
+  const behavior_path_planner::ObjectDataArray & objects, const std::string & ns)
 {
+  using behavior_path_planner::utils::convertToSnakeCase;
+
+  const auto filtered_objects = [&objects, &ns]() {
+    ObjectDataArray ret{};
+    for (const auto & o : objects) {
+      if (o.reason != ns) {
+        continue;
+      }
+      ret.push_back(o);
+    }
+
+    return ret;
+  }();
+
   MarkerArray msg;
-  msg.markers.reserve(objects.size() * 2);
+  msg.markers.reserve(filtered_objects.size() * 2);
 
   appendMarkerArray(
     createObjectsCubeMarkerArray(
-      objects, ns + "_cube", createMarkerScale(3.0, 1.5, 1.5),
-      createMarkerColor(0.0, 1.0, 0.0, 0.8)),
+      filtered_objects, "others_" + convertToSnakeCase(ns) + "_cube",
+      createMarkerScale(3.0, 1.5, 1.5), createMarkerColor(0.0, 1.0, 0.0, 0.8)),
+    &msg);
+  appendMarkerArray(
+    createObjectInfoMarkerArray(filtered_objects, "others_" + convertToSnakeCase(ns) + "_info"),
     &msg);
-
-  appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info"), &msg);
 
   return msg;
 }

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
@@ -3310,7 +3310,20 @@ void AvoidanceModule::setDebugData(
     createAvoidableTargetObjectsMarkerArray(avoidable_target_objects, "avoidable_target_objects"));
   add(createUnavoidableTargetObjectsMarkerArray(
     unavoidable_target_objects, "unavoidable_target_objects"));
-  add(createOtherObjectsMarkerArray(data.other_objects, "other_objects"));
+
+  add(createOtherObjectsMarkerArray(
+    data.other_objects, AvoidanceDebugFactor::OBJECT_IS_BEHIND_THRESHOLD));
+  add(createOtherObjectsMarkerArray(
+    data.other_objects, AvoidanceDebugFactor::OBJECT_IS_IN_FRONT_THRESHOLD));
+  add(createOtherObjectsMarkerArray(
+    data.other_objects, AvoidanceDebugFactor::OBJECT_BEHIND_PATH_GOAL));
+  add(createOtherObjectsMarkerArray(
+    data.other_objects, AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE));
+  add(createOtherObjectsMarkerArray(data.other_objects, AvoidanceDebugFactor::OBJECT_IS_NOT_TYPE));
+  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(makeOverhangToRoadShoulderMarkerArray(data.target_objects, "overhang"));
   add(createOverhangFurthestLineStringMarkerArray(
     debug.bounds, "farthest_linestring_from_overhang", 1.0, 0.0, 1.0));