From 81ec941d6966c0dcefeffa2055c5c5df67fb212c Mon Sep 17 00:00:00 2001
From: vividf <yihsiang.fang@tier4.jp>
Date: Fri, 7 Feb 2025 15:45:48 +0900
Subject: [PATCH] chore: remove oldest timestamp

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../concatenate_and_time_sync_node.cpp        | 37 ++++++++++++++++---
 1 file changed, 31 insertions(+), 6 deletions(-)

diff --git a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_node.cpp b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_node.cpp
index 8c4150a754bdd..e6b5d62a724d3 100644
--- a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_node.cpp
+++ b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_node.cpp
@@ -22,6 +22,7 @@
 #include <pcl_conversions/pcl_conversions.h>
 
 #include <iomanip>
+#include <limits>
 #include <list>
 #include <memory>
 #include <optional>
@@ -369,12 +370,36 @@ void PointCloudConcatenateDataSynchronizerComponent::manage_collector_list()
     }
   }
 
-  if (cloud_collectors_.size() > collectors_threshold) {
-    RCLCPP_WARN_STREAM_THROTTLE(
-      get_logger(), *get_clock(), 1000,
-      "The number of cloud collectors (" << cloud_collectors_.size() << ") exceeds the threshold ("
-                                         << collectors_threshold
-                                         << "), be careful if it keeps increasing.");
+  if (cloud_collectors_.size() >= collectors_threshold) {
+    auto min_it = cloud_collectors_.end();
+    double min_timestamp = std::numeric_limits<double>::max();
+
+    for (auto it = cloud_collectors_.begin(); it != cloud_collectors_.end(); ++it) {
+      auto info = (*it)->get_info();
+      double timestamp = std::numeric_limits<double>::max();
+      if (auto naive_info = std::dynamic_pointer_cast<NaiveCollectorInfo>(info)) {
+        timestamp = naive_info->timestamp;
+      } else if (auto advanced_info = std::dynamic_pointer_cast<AdvancedCollectorInfo>(info)) {
+        timestamp = advanced_info->timestamp;
+      } else {
+        continue;
+      }
+
+      // Update the iterator pointing to the collector with the smallest timestamp
+      if (timestamp < min_timestamp) {
+        min_timestamp = timestamp;
+        min_it = it;
+      }
+    }
+
+    // Reset the collector with the oldest timestamp if found
+    if (min_it != cloud_collectors_.end()) {
+      RCLCPP_WARN_STREAM(
+        get_logger(),
+        "Reset the oldest collector because the number of collectors is larger than the limit:  %d"
+          << collectors_threshold);
+      (*min_it)->reset();
+    }
   }
 }