From d826b40eff19b48515a71992aa5f2e656bfa65e3 Mon Sep 17 00:00:00 2001
From: Maxime CLEMENT <maxime.clement@tier4.jp>
Date: Wed, 22 Jan 2025 10:20:22 +0900
Subject: [PATCH] update debug markers for new object structure

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>
---
 .../src/debug.hpp                             | 91 +++++++++++--------
 .../src/run_out_module.cpp                    |  2 +-
 2 files changed, 56 insertions(+), 37 deletions(-)

diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/debug.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/debug.hpp
index 7a5571618f3ae..cc94f96d18812 100644
--- a/planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/debug.hpp
+++ b/planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/debug.hpp
@@ -99,42 +99,61 @@ inline MarkerArray make_debug_footprint_markers(
   return markers;
 }
 
-// inline MarkerArray make_debug_collisions_markers(
-//   const std::vector<CollisionsWithObject> & collisions_per_object)
-// {
-//   MarkerArray markers;
-//   Marker m;
-//   m.header.frame_id = "base_link";
-//   m.ns = "collisions";
-//   m.type = Marker::TEXT_VIEW_FACING;
-//   m.color = universe_utils::createMarkerColor(1.0, 1.0, 0.0, 0.75);
-//   m.scale = universe_utils::createMarkerScale(0.1, 0.0, 0.5);
-//   std::stringstream ss;
-//   ss << std::setprecision(4);
-//   ss << std::fixed;
-//   ss << std::left;
-//   ss << std::setw(25) << "collision";
-//   ss << "|";
-//   ss << std::setw(20) << "ego_time_interval";
-//   ss << "|";
-//   ss << std::setw(20) << "object_time_interval";
-//   ss << "\n";
-//   for (const auto & collisions : collisions_per_object) {
-//     for (const auto & collision : collisions) {
-//       ss << std::setw(25) << magic_enum::enum_name(collision.type);
-//       ss << "|";
-//       ss << std::setw(20) << collision.ego_time_interval;
-//       ss << "|";
-//       ss << std::setw(20) << collision.object_time_interval;
-//       ss << "\n";
-//     }
-//   }
-//   m.text = ss.str();
-//   std::cout << ss.str() << std::endl;
-//   markers.markers.push_back(m);
-
-//   return markers;
-// }
+inline MarkerArray make_debug_object_markers(const std::vector<Object> & objects)
+{
+  MarkerArray markers;
+  Marker m;
+  m.header.frame_id = "base_link";
+  m.ns = "collisions_table";
+  m.type = Marker::TEXT_VIEW_FACING;
+  m.color = universe_utils::createMarkerColor(1.0, 1.0, 0.0, 0.75);
+  m.scale = universe_utils::createMarkerScale(0.1, 0.0, 0.5);
+  std::stringstream ss;
+  ss << std::setprecision(4);
+  ss << std::fixed;
+  ss << std::left;
+  ss << std::setw(25) << "object id";
+  ss << "|";
+  ss << std::setw(25) << "collision";
+  ss << "|";
+  ss << std::setw(20) << "ego_time_interval";
+  ss << "|";
+  ss << std::setw(20) << "object_time_interval";
+  ss << "\n";
+  for (const auto & o : objects) {
+    for (const auto & collision : o.collisions) {
+      ss << std::setw(25) << o.uuid;
+      ss << "|";
+      ss << std::setw(25) << magic_enum::enum_name(collision.type);
+      ss << "|";
+      ss << std::setw(20) << collision.ego_time_interval;
+      ss << "|";
+      ss << std::setw(20) << collision.object_time_interval;
+      ss << "\n";
+    }
+  }
+  m.text = ss.str();
+  markers.markers.push_back(m);
+  m.ns = "collisions_points";
+  m.header.frame_id = "map";
+  m.type = Marker::POINTS;
+  m.color = universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.75);
+  m.scale = universe_utils::createMarkerScale(0.5, 0.5, 0.5);
+  for (const auto & o : objects) {
+    for (const auto & c : o.collisions) {
+      for (const auto & p : {
+             c.object_time_interval.first_intersection.intersection,
+             c.object_time_interval.last_intersection.intersection,
+             c.ego_time_interval.first_intersection.intersection,
+             c.ego_time_interval.last_intersection.intersection,
+           }) {
+        m.points.push_back(universe_utils::createPoint(p.x(), p.y(), 0.0));
+      }
+    }
+  }
+  markers.markers.push_back(m);
+  return markers;
+}
 
 inline MarkerArray make_debug_decisions_markers(const ObjectDecisionsTracker & decisions_tracker)
 {
diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/run_out_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/run_out_module.cpp
index 4e7824f0c3ae8..a4d1bd7dfd70e 100644
--- a/planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/run_out_module.cpp
+++ b/planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/run_out_module.cpp
@@ -176,7 +176,7 @@ VelocityPlanningResult RunOutModule::plan(
     result, ego_trajectory_points, planner_data->vehicle_info_.max_longitudinal_offset_m));
   virtual_wall_publisher_->publish(virtual_wall_marker_creator.create_markers(now));
   debug_publisher_->publish(run_out::make_debug_footprint_markers(ego_footprint, filtered_objects));
-  // debug_publisher_->publish(run_out::make_debug_collisions_markers(filtered_objects));
+  debug_publisher_->publish(run_out::make_debug_object_markers(filtered_objects));
   debug_publisher_->publish(run_out::make_debug_decisions_markers(decisions_tracker_));
   time_keeper_->end_track("publish_debug()");
   time_keeper_->end_track("plan()");