refactor(start_planner): separate debug and info marker for rviz visualization (autowarefoundation#6376)

* separate debug and info marker

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* replace util function for footprint

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

---------

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

Author: kyoichi-sugahara

planning/behavior_path_start_planner_module/src/start_planner_module.cpp

@@ -139,6 +139,7 @@ void StartPlannerModule::initVariables()
   resetPathCandidate();
   resetPathReference();
   debug_marker_.markers.clear();
+  info_marker_.markers.clear();
   initializeSafetyCheckParameters();
   initializeCollisionCheckDebugMap(debug_data_.collision_check);
   updateDepartureCheckLanes();
@@ -1456,25 +1457,35 @@ void StartPlannerModule::setDebugData()
   const auto white_color = createMarkerColor(1.0, 1.0, 1.0, 0.99);
 
   const auto life_time = rclcpp::Duration::from_seconds(1.5);
-  auto add = [&](MarkerArray added) {
+  auto add = [&](MarkerArray added, MarkerArray & target_marker_array) {
     for (auto & marker : added.markers) {
       marker.lifetime = life_time;
     }
-    tier4_autoware_utils::appendMarkerArray(added, &debug_marker_);
+    tier4_autoware_utils::appendMarkerArray(added, &target_marker_array);
   };
 
   debug_marker_.markers.clear();
-  add(createPoseMarkerArray(status_.pull_out_start_pose, "back_end_pose", 0, 0.9, 0.3, 0.3));
-  add(createPoseMarkerArray(status_.pull_out_path.start_pose, "start_pose", 0, 0.3, 0.9, 0.3));
-  add(createPoseMarkerArray(status_.pull_out_path.end_pose, "end_pose", 0, 0.9, 0.9, 0.3));
-  add(createFootprintMarkerArray(
-    debug_data_.refined_start_pose, vehicle_info_, "refined_start_pose", 0, 0.9, 0.9, 0.3));
-  add(createPathMarkerArray(getFullPath(), "full_path", 0, 0.0, 0.5, 0.9));
-  add(createPathMarkerArray(status_.backward_path, "backward_driving_path", 0, 0.0, 0.9, 0.0));
+  info_marker_.markers.clear();
+  add(
+    createPoseMarkerArray(status_.pull_out_start_pose, "back_end_pose", 0, 0.9, 0.3, 0.3),
+    info_marker_);
+  add(
+    createPoseMarkerArray(status_.pull_out_path.start_pose, "start_pose", 0, 0.3, 0.9, 0.3),
+    info_marker_);
+  add(
+    createPoseMarkerArray(status_.pull_out_path.end_pose, "end_pose", 0, 0.9, 0.9, 0.3),
+    info_marker_);
+  add(
+    createFootprintMarkerArray(
+      debug_data_.refined_start_pose, vehicle_info_, "refined_start_pose", 0, 0.9, 0.9, 0.3),
+    debug_marker_);
+  add(createPathMarkerArray(getFullPath(), "full_path", 0, 0.0, 0.5, 0.9), debug_marker_);
+  add(
+    createPathMarkerArray(status_.backward_path, "backward_driving_path", 0, 0.0, 0.9, 0.0),
+    debug_marker_);
 
   // visualize collision_check_end_pose and footprint
   {
-    const auto local_footprint = vehicle_info_.createFootprint();
     std::map<PlannerType, double> collision_check_distances = {
       {PlannerType::SHIFT, parameters_->shift_collision_check_distance_from_end},
       {PlannerType::GEOMETRIC, parameters_->geometric_collision_check_distance_from_end}};
@@ -1484,24 +1495,16 @@ void StartPlannerModule::setDebugData()
       getFullPath().points, status_.pull_out_path.end_pose.position,
       collision_check_distance_from_end);
     if (collision_check_end_pose) {
-      add(createPoseMarkerArray(
-        *collision_check_end_pose, "static_collision_check_end_pose", 0, 1.0, 0.0, 0.0));
+      add(
+        createPoseMarkerArray(
+          *collision_check_end_pose, "static_collision_check_end_pose", 0, 1.0, 0.0, 0.0),
+        info_marker_);
       auto marker = createDefaultMarker(
         "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "static_collision_check_end_polygon", 0,
-        Marker::LINE_LIST, createMarkerScale(0.1, 0.1, 0.1), red_color);
-      const auto footprint = transformVector(
-        local_footprint, tier4_autoware_utils::pose2transform(*collision_check_end_pose));
-      const double ego_z = planner_data_->self_odometry->pose.pose.position.z;
-      for (size_t i = 0; i < footprint.size(); i++) {
-        const auto & current_point = footprint.at(i);
-        const auto & next_point = footprint.at((i + 1) % footprint.size());
-        marker.points.push_back(
-          tier4_autoware_utils::createPoint(current_point.x(), current_point.y(), ego_z));
-        marker.points.push_back(
-          tier4_autoware_utils::createPoint(next_point.x(), next_point.y(), ego_z));
-      }
+        Marker::LINE_STRIP, createMarkerScale(0.1, 0.1, 0.1), red_color);
+      addFootprintMarker(marker, *collision_check_end_pose, vehicle_info_);
       marker.lifetime = life_time;
-      debug_marker_.markers.push_back(marker);
+      info_marker_.markers.push_back(marker);
     }
   }
   // start pose candidates
@@ -1528,8 +1531,8 @@ void StartPlannerModule::setDebugData()
       start_pose_text_marker_array.markers.push_back(text_marker);
     }
 
-    add(start_pose_footprint_marker_array);
-    add(start_pose_text_marker_array);
+    add(start_pose_footprint_marker_array, debug_marker_);
+    add(start_pose_text_marker_array, debug_marker_);
   }
 
   // visualize the footprint from pull_out_start pose to pull_out_end pose along the path
@@ -1561,26 +1564,32 @@ void StartPlannerModule::setDebugData()
       pull_out_path_footprint_marker_array.markers.push_back(pull_out_path_footprint_marker);
     }
 
-    add(pull_out_path_footprint_marker_array);
+    add(pull_out_path_footprint_marker_array, debug_marker_);
   }
 
   // safety check
   if (parameters_->safety_check_params.enable_safety_check) {
     if (debug_data_.ego_predicted_path.size() > 0) {
       const auto & ego_predicted_path = utils::path_safety_checker::convertToPredictedPath(
         debug_data_.ego_predicted_path, ego_predicted_path_params_->time_resolution);
-      add(createPredictedPathMarkerArray(
-        ego_predicted_path, vehicle_info_, "ego_predicted_path_start_planner", 0, 0.0, 0.5, 0.9));
+      add(
+        createPredictedPathMarkerArray(
+          ego_predicted_path, vehicle_info_, "ego_predicted_path_start_planner", 0, 0.0, 0.5, 0.9),
+        debug_marker_);
     }
 
     if (debug_data_.filtered_objects.objects.size() > 0) {
-      add(createObjectsMarkerArray(
-        debug_data_.filtered_objects, "filtered_objects", 0, 0.0, 0.5, 0.9));
+      add(
+        createObjectsMarkerArray(
+          debug_data_.filtered_objects, "filtered_objects", 0, 0.0, 0.5, 0.9),
+        info_marker_);
     }
 
-    add(showSafetyCheckInfo(debug_data_.collision_check, "object_debug_info"));
-    add(showPredictedPath(debug_data_.collision_check, "ego_predicted_path"));
-    add(showPolygon(debug_data_.collision_check, "ego_and_target_polygon_relation"));
+    add(showSafetyCheckInfo(debug_data_.collision_check, "object_debug_info"), debug_marker_);
+    add(
+      showPredictedPath(debug_data_.collision_check, "predicted_path_for_safety_check"),
+      info_marker_);
+    add(showPolygon(debug_data_.collision_check, "ego_and_target_polygon_relation"), info_marker_);
 
     // set objects of interest
     for (const auto & [uuid, data] : debug_data_.collision_check) {
@@ -1608,17 +1617,21 @@ void StartPlannerModule::setDebugData()
                    std::to_string(status_.pull_out_path.partial_paths.size() - 1);
     marker.lifetime = life_time;
     planner_type_marker_array.markers.push_back(marker);
-    add(planner_type_marker_array);
+    add(planner_type_marker_array, info_marker_);
   }
 
-  add(laneletsAsTriangleMarkerArray(
-    "departure_check_lanes_for_shift_pull_out_path", debug_data_.departure_check_lanes,
-    cyan_color));
+  add(
+    laneletsAsTriangleMarkerArray(
+      "departure_check_lanes_for_shift_pull_out_path", debug_data_.departure_check_lanes,
+      cyan_color),
+    debug_marker_);
 
   const auto pull_out_lanes = start_planner_utils::getPullOutLanes(
     planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance);
-  add(laneletsAsTriangleMarkerArray(
-    "pull_out_lanes_for_static_objects_collision_check", pull_out_lanes, pink_color));
+  add(
+    laneletsAsTriangleMarkerArray(
+      "pull_out_lanes_for_static_objects_collision_check", pull_out_lanes, pink_color),
+    debug_marker_);
 }
 
 void StartPlannerModule::logPullOutStatus(rclcpp::Logger::Level log_level) const