reduce number of arguments of the add_polygons_markers function

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp

@@ -58,18 +58,15 @@ visualization_msgs::msg::Marker get_base_marker()
 }
 void add_polygons_markers(
   visualization_msgs::msg::MarkerArray & debug_marker_array,
-  const lanelet::BasicPolygons2d & polygons, const double z, const std::string & ns,
-  const std_msgs::msg::ColorRGBA & color = universe_utils::createMarkerColor(1.0, 0.0, 0.0, 1.0))
+  const visualization_msgs::msg::Marker & base_marker, const lanelet::BasicPolygons2d & polygons)
 {
-  auto debug_marker = get_base_marker();
-  debug_marker.color = color;
-  debug_marker.ns = ns;
+  auto debug_marker = base_marker;
   debug_marker.type = visualization_msgs::msg::Marker::LINE_LIST;
   for (const auto & f : polygons) {
     boost::geometry::for_each_segment(f, [&](const auto & s) {
       const auto & [p1, p2] = s;
-      debug_marker.points.push_back(universe_utils::createMarkerPosition(p1.x(), p1.y(), z + 0.5));
-      debug_marker.points.push_back(universe_utils::createMarkerPosition(p2.x(), p2.y(), z + 0.5));
+      debug_marker.points.push_back(universe_utils::createMarkerPosition(p1.x(), p1.y(), 0.0));
+      debug_marker.points.push_back(universe_utils::createMarkerPosition(p2.x(), p2.y(), 0.0));
     });
   }
   debug_marker_array.markers.push_back(debug_marker);
@@ -163,25 +160,29 @@ visualization_msgs::msg::MarkerArray create_debug_marker_array(
   const auto z = ego_data.pose.position.z;
   visualization_msgs::msg::MarkerArray debug_marker_array;
 
-  add_polygons_markers(
-    debug_marker_array, ego_data.trajectory_footprints, z, "footprints",
-    universe_utils::createMarkerColor(1.0, 1.0, 1.0, 1.0));
+  auto base_marker = get_base_marker();
+  base_marker.pose.position.z = z + 0.5;
+  base_marker.ns = "footprints";
+  base_marker.color = universe_utils::createMarkerColor(1.0, 1.0, 1.0, 1.0);
+  // TODO(Maxime): move the debug marker publishing AFTER the trajectory generation
+  // disabled to prevent performance issues when publishing the debug markers
+  // add_polygons_markers(debug_marker_array, base_marker, ego_data.trajectory_footprints);
 
   lanelet::BasicPolygons2d drivable_lane_polygons;
   for (const auto & poly : ego_data.drivable_lane_polygons) {
     drivable_lane_polygons.push_back(poly.outer);
   }
-  add_polygons_markers(
-    debug_marker_array, drivable_lane_polygons, z, "ego_lane",
-    universe_utils::createMarkerColor(0.0, 0.0, 1.0, 1.0));
+  base_marker.ns = "ego_lane";
+  base_marker.color = universe_utils::createMarkerColor(0.0, 0.0, 1.0, 1.0);
+  add_polygons_markers(debug_marker_array, base_marker, drivable_lane_polygons);
 
   lanelet::BasicPolygons2d out_of_lane_areas;
   for (const auto & p : out_of_lane_data.outside_points) {
     out_of_lane_areas.push_back(p.outside_ring);
   }
-  add_polygons_markers(
-    debug_marker_array, out_of_lane_areas, z, "out_of_lane_areas",
-    universe_utils::createMarkerColor(1.0, 0.0, 0.0, 1.0));
+  base_marker.ns = "out_of_lane_areas";
+  base_marker.color = universe_utils::createMarkerColor(1.0, 0.0, 0.0, 1.0);
+  add_polygons_markers(debug_marker_array, base_marker, out_of_lane_areas);
   for (const auto & [bbox, i] : out_of_lane_data.outside_areas_rtree) {
     const auto & a = out_of_lane_data.outside_points[i];
     debug_marker_array.markers.back().points.push_back(
@@ -204,9 +205,9 @@ visualization_msgs::msg::MarkerArray create_debug_marker_array(
       }
     }
   }
-  add_polygons_markers(
-    debug_marker_array, object_polygons, z, "objects",
-    universe_utils::createMarkerColor(0.0, 1.0, 0.0, 1.0));
+  base_marker.ns = "objects";
+  base_marker.color = universe_utils::createMarkerColor(0.0, 1.0, 0.0, 1.0);
+  add_polygons_markers(debug_marker_array, base_marker, object_polygons);
 
   add_current_overlap_marker(debug_marker_array, ego_data.current_footprint, z);