move the marker creation to marker_utils

Signed-off-by: Zulfaqar Azmi <[email protected]>

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/markers.hpp

@@ -15,9 +15,11 @@
 #ifndef BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_
 #define BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_
 
+#include "behavior_path_lane_change_module/utils/debug_structs.hpp"
 #include "behavior_path_lane_change_module/utils/path.hpp"
 #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp"
 
+#include <visualization_msgs/msg/detail/marker_array__struct.hpp>
 #include <visualization_msgs/msg/marker_array.hpp>
 
 #include <string>
@@ -28,6 +30,7 @@ namespace marker_utils::lane_change_markers
 using behavior_path_planner::LaneChangePath;
 using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObjects;
 using visualization_msgs::msg::MarkerArray;
+using LaneChangeDebug = behavior_path_planner::data::lane_change::Debug;
 MarkerArray showAllValidLaneChangePath(
   const std::vector<LaneChangePath> & lanes, std::string && ns);
 MarkerArray createLaneChangingVirtualWallMarker(
@@ -37,5 +40,6 @@ MarkerArray showFilteredObjects(
   const ExtendedPredictedObjects & current_lane_objects,
   const ExtendedPredictedObjects & target_lane_objects,
   const ExtendedPredictedObjects & other_lane_objects, const std::string & ns);
+MarkerArray createDebugMarkerArray(const LaneChangeDebug & debug_data);
 }  // namespace marker_utils::lane_change_markers
 #endif  // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_

planning/behavior_path_lane_change_module/src/interface.cpp

@@ -316,48 +316,13 @@ bool LaneChangeInterface::canTransitIdleToRunningState()
 
 void LaneChangeInterface::updateDebugMarker() const
 {
-  debug_marker_.markers.clear();
   if (!parameters_->publish_debug_marker) {
     return;
   }
-  using marker_utils::showPolygon;
-  using marker_utils::showPredictedPath;
-  using marker_utils::showSafetyCheckInfo;
-  using marker_utils::lane_change_markers::showAllValidLaneChangePath;
-  using marker_utils::lane_change_markers::showFilteredObjects;
-
-  const auto & debug_data = module_type_->getDebugData();
-  const auto & debug_collision_check_object = debug_data.collision_check_objects;
-  const auto & debug_collision_check_object_after_approval =
-    debug_data.collision_check_objects_after_approval;
-  const auto & debug_valid_paths = debug_data.valid_paths;
-  const auto & debug_filtered_objects = debug_data.filtered_objects;
 
   debug_marker_.markers.clear();
-  const auto add = [this](const MarkerArray & added) {
-    tier4_autoware_utils::appendMarkerArray(added, &debug_marker_);
-  };
-
-  add(showAllValidLaneChangePath(debug_valid_paths, "lane_change_valid_paths"));
-  add(showFilteredObjects(
-    debug_filtered_objects.current_lane, debug_filtered_objects.target_lane,
-    debug_filtered_objects.other_lane, "object_filtered"));
-
-  if (!debug_collision_check_object.empty()) {
-    add(showSafetyCheckInfo(debug_collision_check_object, "collision_check_object_info"));
-    add(showPredictedPath(debug_collision_check_object, "ego_predicted_path"));
-    add(showPolygon(debug_collision_check_object, "ego_and_target_polygon_relation"));
-  }
-
-  if (!debug_collision_check_object_after_approval.empty()) {
-    add(showSafetyCheckInfo(
-      debug_collision_check_object_after_approval, "object_debug_info_after_approval"));
-    add(showPredictedPath(
-      debug_collision_check_object_after_approval, "ego_predicted_path_after_approval"));
-    add(showPolygon(
-      debug_collision_check_object_after_approval,
-      "ego_and_target_polygon_relation_after_approval"));
-  }
+  using marker_utils::lane_change_markers::createDebugMarkerArray;
+  debug_marker_ = createDebugMarkerArray(module_type_->getDebugData());
 }
 
 MarkerArray LaneChangeInterface::getModuleVirtualWall()

planning/behavior_path_lane_change_module/src/utils/markers.cpp

@@ -22,6 +22,7 @@
 #include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 #include <visualization_msgs/msg/detail/marker__struct.hpp>
+#include <visualization_msgs/msg/detail/marker_array__struct.hpp>
 
 #include <cstdint>
 #include <cstdlib>
@@ -120,4 +121,47 @@ MarkerArray showFilteredObjects(
     marker_array.markers.end(), other_marker.markers.begin(), other_marker.markers.end());
   return marker_array;
 }
+
+MarkerArray createDebugMarkerArray(const LaneChangeDebug & debug_data)
+{
+  using marker_utils::showPolygon;
+  using marker_utils::showPredictedPath;
+  using marker_utils::showSafetyCheckInfo;
+  using marker_utils::lane_change_markers::showAllValidLaneChangePath;
+  using marker_utils::lane_change_markers::showFilteredObjects;
+
+  const auto & debug_collision_check_object = debug_data.collision_check_objects;
+  const auto & debug_collision_check_object_after_approval =
+    debug_data.collision_check_objects_after_approval;
+  const auto & debug_valid_paths = debug_data.valid_paths;
+  const auto & debug_filtered_objects = debug_data.filtered_objects;
+
+  MarkerArray debug_marker;
+  const auto add = [&debug_marker](const MarkerArray & added) {
+    tier4_autoware_utils::appendMarkerArray(added, &debug_marker);
+  };
+
+  add(showAllValidLaneChangePath(debug_valid_paths, "lane_change_valid_paths"));
+  add(showFilteredObjects(
+    debug_filtered_objects.current_lane, debug_filtered_objects.target_lane,
+    debug_filtered_objects.other_lane, "object_filtered"));
+
+  if (!debug_collision_check_object.empty()) {
+    add(showSafetyCheckInfo(debug_collision_check_object, "collision_check_object_info"));
+    add(showPredictedPath(debug_collision_check_object, "ego_predicted_path"));
+    add(showPolygon(debug_collision_check_object, "ego_and_target_polygon_relation"));
+  }
+
+  if (!debug_collision_check_object_after_approval.empty()) {
+    add(showSafetyCheckInfo(
+      debug_collision_check_object_after_approval, "object_debug_info_after_approval"));
+    add(showPredictedPath(
+      debug_collision_check_object_after_approval, "ego_predicted_path_after_approval"));
+    add(showPolygon(
+      debug_collision_check_object_after_approval,
+      "ego_and_target_polygon_relation_after_approval"));
+  }
+
+  return debug_marker;
+}
 }  // namespace marker_utils::lane_change_markers