From 7fe080f8c5c868054533ff8a3ebef60b1e85d1d9 Mon Sep 17 00:00:00 2001
From: TadaKazuto <kazuto.tada@tier4.jp>
Date: Sun, 9 Mar 2025 16:19:31 +0900
Subject: [PATCH] Add debug output

---
 .../src/multi_object_tracker_core.cpp                 | 11 +++++++++++
 1 file changed, 11 insertions(+)

diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
index c8cfd71fc15fa..5dc811defab0b 100644
--- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
+++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
@@ -35,6 +35,7 @@
 #include <Eigen/Core>
 #include <Eigen/Geometry>
 #include <rclcpp_components/register_node_macro.hpp>
+#include <rclcpp/logging.hpp>
 
 using Label = autoware_auto_perception_msgs::msg::ObjectClassification;
 
@@ -279,6 +280,16 @@ void MultiObjectTracker::onMeasurement(
         ->updateWithMeasurement(
           transformed_objects.objects.at(direct_assignment.find(tracker_idx)->second),
           measurement_time, *self_transform);
+
+        // dbg output
+        rclcpp::Time time;
+        autoware_auto_perception_msgs::msg::TrackedObject object;
+        (*tracker_itr)->getTrackedObject(time, object);
+        if ((object.kinematics.pose_with_covariance.pose.position.y > -1.0) && (object.kinematics.pose_with_covariance.pose.position.y < 1.0) )
+        {
+          RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "track meas update idx[%d], x[%f], y[%f]", tracker_idx, object.kinematics.pose_with_covariance.pose.position.x, object.kinematics.pose_with_covariance.pose.position.y);
+        }
+
     } else {  // not found
       (*(tracker_itr))->updateWithoutMeasurement();
     }