feat(avoidance): output overhang lanelet

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

Author: satoshi-ota

planning/behavior_path_avoidance_module/src/debug.cpp

@@ -188,10 +188,25 @@ MarkerArray createObjectInfoMarkerArray(const ObjectDataArray & objects, std::st
   return msg;
 }
 
+MarkerArray createOverhangLaneletMarkerArray(const ObjectDataArray & objects, std::string && ns)
+{
+  MarkerArray msg;
+  msg.markers.reserve(objects.size());
+
+  for (const auto & object : objects) {
+    appendMarkerArray(
+      marker_utils::createLaneletsAreaMarkerArray(
+        {object.overhang_lanelet}, std::string(ns), 0.0, 0.0, 1.0),
+      &msg);
+  }
+
+  return msg;
+}
+
 MarkerArray avoidableObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns)
 {
   MarkerArray msg;
-  msg.markers.reserve(objects.size() * 4);
+  msg.markers.reserve(objects.size() * 5);
 
   appendMarkerArray(
     createObjectsCubeMarkerArray(
@@ -202,14 +217,15 @@ MarkerArray avoidableObjectsMarkerArray(const ObjectDataArray & objects, std::st
   appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info"), &msg);
   appendMarkerArray(createObjectPolygonMarkerArray(objects, ns + "_envelope_polygon"), &msg);
   appendMarkerArray(createToDrivableBoundDistance(objects, ns + "_to_drivable_bound"), &msg);
+  appendMarkerArray(createOverhangLaneletMarkerArray(objects, ns + "_overhang_lanelet"), &msg);
 
   return msg;
 }
 
 MarkerArray unAvoidableObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns)
 {
   MarkerArray msg;
-  msg.markers.reserve(objects.size() * 4);
+  msg.markers.reserve(objects.size() * 5);
 
   appendMarkerArray(
     createObjectsCubeMarkerArray(
@@ -220,6 +236,7 @@ MarkerArray unAvoidableObjectsMarkerArray(const ObjectDataArray & objects, std::
   appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info"), &msg);
   appendMarkerArray(createObjectPolygonMarkerArray(objects, ns + "_envelope_polygon"), &msg);
   appendMarkerArray(createToDrivableBoundDistance(objects, ns + "_to_drivable_bound"), &msg);
+  appendMarkerArray(createOverhangLaneletMarkerArray(objects, ns + "_overhang_lanelet"), &msg);
 
   return msg;
 }
@@ -451,6 +468,10 @@ MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, const
   appendMarkerArray(
     createObjectInfoMarkerArray(filtered_objects, "others_" + convertToSnakeCase(ns) + "_info"),
     &msg);
+  appendMarkerArray(
+    createOverhangLaneletMarkerArray(
+      filtered_objects, "others_" + convertToSnakeCase(ns) + "_overhang_lanelet"),
+    &msg);
 
   return msg;
 }