fix(planning_data_analyzer): preserve valid message stamps for gt_trajectory evaluation

Author: beomseok-kimm

planning/autoware_planning_data_analyzer/src/bag_handler.hpp

@@ -287,7 +287,7 @@ typename std::enable_if<has_header_stamp<MessageType>::value, void>::type
 set_header_timestamp_if_needed(
   MessageType & msg, bool use_bag_timestamp, const rclcpp::Time & bag_time)
 {
-  if (use_bag_timestamp && msg.header.stamp != rclcpp::Time(0)) {
+  if (use_bag_timestamp && msg.header.stamp == rclcpp::Time(0)) {
     msg.header.stamp = bag_time;
   }
 }

planning/autoware_planning_data_analyzer/test/test_bag_handler.cpp

@@ -82,3 +82,25 @@ TEST_F(BagHandlerTest, GetLatestBeforeOrEqual)
   EXPECT_EQ(
     buffer.get_latest_before_or_equal(rclcpp::Time(10, 0).nanoseconds())->header.stamp.sec, 3);
 }
+
+TEST_F(BagHandlerTest, SetHeaderTimestampIfNeededPreservesValidStamp)
+{
+  nav_msgs::msg::Odometry msg;
+  msg.header.stamp = rclcpp::Time(100, 0);
+
+  autoware::planning_data_analyzer::set_header_timestamp_if_needed(
+    msg, true, rclcpp::Time(200, 0));
+
+  EXPECT_EQ(rclcpp::Time(msg.header.stamp).nanoseconds(), rclcpp::Time(100, 0).nanoseconds());
+}
+
+TEST_F(BagHandlerTest, SetHeaderTimestampIfNeededFillsZeroStampFromBagTime)
+{
+  nav_msgs::msg::Odometry msg;
+  msg.header.stamp = rclcpp::Time(0, 0);
+
+  autoware::planning_data_analyzer::set_header_timestamp_if_needed(
+    msg, true, rclcpp::Time(200, 0));
+
+  EXPECT_EQ(rclcpp::Time(msg.header.stamp).nanoseconds(), rclcpp::Time(200, 0).nanoseconds());
+}