chore: fix min max

Signed-off-by: vividf <yihsiang.fang@tier4.jp>

Author: vividf

sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_node.hpp

@@ -113,6 +113,9 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node
     std::vector<double> lidar_timestamp_noise_window;
   } params_;
 
+  // remove this later
+  rclcpp::Clock::SharedPtr debug_clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
+
   std::set<std::string> missed_cloud_;
   std::shared_ptr<CombineCloudHandler> combine_cloud_handler_;
   std::shared_ptr<CloudCollector> cloud_collector_;

sensing/pointcloud_preprocessor/src/concatenate_data/cloud_collector.cpp

@@ -84,6 +84,11 @@ void CloudCollector::setReferenceTimeStamp(double timestamp, double noise_window
 {
   reference_timestamp_max_ = timestamp + noise_window;
   reference_timestamp_min_ = timestamp - noise_window;
+
+  std::cout << "timestamp: " << timestamp << std::endl;
+  std::cout << "noise_window: " << noise_window << std::endl;
+  std::cout << "reference_timestamp_max_: " << reference_timestamp_max_ << std::endl;
+  std::cout << "reference_timestamp_min_: " << reference_timestamp_min_ << std::endl;
 }
 
 std::tuple<double, double> CloudCollector::getReferenceTimeStampBoundary()

sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_node.cpp

@@ -223,7 +223,11 @@ std::string PointCloudConcatenateDataSynchronizerComponent::replaceSyncTopicName
 void PointCloudConcatenateDataSynchronizerComponent::cloud_callback(
   const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, const std::string & topic_name)
 {
-  std::cout << "topic name pointcloud arrive: " << topic_name << " " << std::fixed
+  // Get the current time
+  rclcpp::Time now = debug_clock->now();
+  std::cout << "Current time: " << now.seconds() << " seconds" << std::endl;
+
+  std::cout << "pointcloud name and timestamp: " << topic_name << " " << std::fixed
             << std::setprecision(9) << rclcpp::Time(input_ptr->header.stamp).seconds() << std::endl;
   sensor_msgs::msg::PointCloud2::SharedPtr xyzi_input_ptr(new sensor_msgs::msg::PointCloud2());
   auto input = std::make_shared<sensor_msgs::msg::PointCloud2>(*input_ptr);
@@ -242,12 +246,22 @@ void PointCloudConcatenateDataSynchronizerComponent::cloud_callback(
   // std::cout << "topic_to_offset_map_[topic_name]:  " << topic_to_offset_map_[topic_name]  <<
   // std::endl;
   if (!cloud_collectors_.empty()) {
-    // std::cout << "Searching collect in size:  " << cloud_collectors_.size() << std::endl;
+    std::cout << "Searching collect in size:  " << cloud_collectors_.size() << std::endl;
     for (const auto & cloud_collector : cloud_collectors_) {
       // std::cout << "collector timestamp:  " <<  std::fixed << std::setprecision(9) <<
       // cloud_collector->getTimeStamp()  << std::endl; cloud_collector->printTimer();
-      auto [reference_timestamp_max, reference_timestamp_min] =
+      auto [reference_timestamp_min, reference_timestamp_max] =
         cloud_collector->getReferenceTimeStampBoundary();
+
+      std::cout
+        << "rclcpp::Time(input_ptr->header.stamp).seconds() - topic_to_offset_map_[topic_name]: "
+        << rclcpp::Time(input_ptr->header.stamp).seconds() - topic_to_offset_map_[topic_name]
+        << std::endl;
+      std::cout << "reference_timestamp_max + topic_to_noise_window_map[topic_name]: "
+                << reference_timestamp_max + topic_to_noise_window_map[topic_name] << std::endl;
+      std::cout << "reference_timestamp_min - topic_to_noise_window_map[topic_name]: "
+                << reference_timestamp_min - topic_to_noise_window_map[topic_name] << std::endl;
+
       if (
         rclcpp::Time(input_ptr->header.stamp).seconds() - topic_to_offset_map_[topic_name] <
           reference_timestamp_max + topic_to_noise_window_map[topic_name] &&
@@ -263,7 +277,7 @@ void PointCloudConcatenateDataSynchronizerComponent::cloud_callback(
   }
   // if collecotrs is empty or didn't find matched collector.
   if (!collector_found) {
-    // std::cout << "create new collector " << std::endl;
+    std::cout << "create new collector " << std::endl;
     auto new_cloud_collector = std::make_shared<CloudCollector>(
       std::dynamic_pointer_cast<PointCloudConcatenateDataSynchronizerComponent>(shared_from_this()),
       cloud_collectors_, combine_cloud_handler_, params_.input_topics.size(), params_.timeout_sec);