feat: add nosie window for timestamp

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

Author: vividf

sensing/pointcloud_preprocessor/config/concatenate_and_time_sync.param.yaml

@@ -1,17 +1,18 @@
 /**:
   ros__parameters:
     maximum_queue_size: 5
-    timeout_sec: 0.5
-    offset_tolerance: 0.03
+    timeout_sec: 0.1
+    #offset_tolerance: 0.03
     is_motion_compensated: true
     publish_synchronized_pointcloud: true
     keep_input_frame_in_synchronized_pointcloud: true
     synchronized_pointcloud_postfix: pointcloud
     input_twist_topic_type: twist
     input_topics: [
-                    "/sensing/lidar/right/pointcloud_before_sync",
                     "/sensing/lidar/left/pointcloud_before_sync",
+                    "/sensing/lidar/right/pointcloud_before_sync",
                     "/sensing/lidar/top/pointcloud_before_sync"
                 ]
     output_frame: base_link
-    lidar_timestamp_offsets: [0.0, 0.03, 0.04]
+    lidar_timestamp_offsets: [0.0, 0.04, 0.08]
+    lidar_timestamp_noise_window: [0.01, 0.01, 0.01]

sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/cloud_collector.hpp

@@ -54,11 +54,12 @@
 
 #include "combine_cloud_handler.hpp"
 
+#include <list>
 #include <memory>
 #include <string>
+#include <tuple>
 #include <unordered_map>
 #include <vector>
-#include <list>
 
 namespace pointcloud_preprocessor
 {
@@ -74,13 +75,13 @@ class CloudCollector
     std::list<std::shared_ptr<CloudCollector>> & collectors,
     std::shared_ptr<CombineCloudHandler> combine_cloud_handler, int num_of_clouds, double time);
 
-  void setTimeStamp(double timestamp);
-  double getTimeStamp();
+  void setReferenceTimeStamp(double timestamp, double noise_window);
+  std::tuple<double, double> getReferenceTimeStampBoundary();
   void processCloud(std::string topic_name, sensor_msgs::msg::PointCloud2::SharedPtr cloud);
   void combineClouds();
   void deleteCollector();
   // for debugging
-  void printTimer(); 
+  void printTimer();
 
 private:
   std::shared_ptr<PointCloudConcatenateDataSynchronizerComponent> concatenate_node_;
@@ -90,7 +91,8 @@ class CloudCollector
   std::unordered_map<std::string, sensor_msgs::msg::PointCloud2::SharedPtr> topic_cloud_map_;
   uint64_t num_of_clouds_;
   double timeout_sec_;
-  double timestamp_;
+  double reference_timestamp_min_;
+  double reference_timestamp_max_;
   std::mutex mutex_;
 };
 

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

@@ -101,7 +101,7 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node
   {
     int maximum_queue_size;
     double timeout_sec;
-    double offset_tolerance;
+    double offset_tolerance;  // TODO(vivid): remove this
     bool is_motion_compensated;
     bool publish_synchronized_pointcloud;
     bool keep_input_frame_in_synchronized_pointcloud;
@@ -110,6 +110,7 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node
     std::vector<std::string> input_topics;
     std::string output_frame;
     std::vector<double> lidar_timestamp_offsets;
+    std::vector<double> lidar_timestamp_noise_window;
   } params_;
 
   std::set<std::string> missed_cloud_;
@@ -118,6 +119,7 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node
   std::list<std::shared_ptr<CloudCollector>> cloud_collectors_;
   std::mutex mutex_;
   std::unordered_map<std::string, double> topic_to_offset_map_;
+  std::unordered_map<std::string, double> topic_to_noise_window_map;
 
   // subscribers
   std::vector<rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr> pointcloud_subs;

sensing/pointcloud_preprocessor/src/concatenate_data/cloud_collector.cpp

@@ -56,6 +56,8 @@
 
 #include <rclcpp/rclcpp.hpp>
 
+#include <tuple>
+
 namespace pointcloud_preprocessor
 {
 
@@ -69,7 +71,7 @@ CloudCollector::CloudCollector(
   num_of_clouds_(num_of_clouds),
   timeout_sec_(timeout_sec)
 {
-  timestamp_ = 0.0;
+  // timestamp_ = 0.0;
   const auto period_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
     std::chrono::duration<double>(timeout_sec_));
 
@@ -78,14 +80,15 @@ CloudCollector::CloudCollector(
     std::bind(&CloudCollector::combineClouds, this));
 }
 
-void CloudCollector::setTimeStamp(double timestamp)
+void CloudCollector::setReferenceTimeStamp(double timestamp, double noise_window)
 {
-  timestamp_ = timestamp;
+  reference_timestamp_max_ = timestamp + noise_window;
+  reference_timestamp_min_ = timestamp - noise_window;
 }
 
-double CloudCollector::getTimeStamp()
+std::tuple<double, double> CloudCollector::getReferenceTimeStampBoundary()
 {
-  return timestamp_;
+  return std::make_tuple(reference_timestamp_min_, reference_timestamp_max_);
 }
 
 void CloudCollector::processCloud(

sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_node.cpp

@@ -96,6 +96,8 @@ PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchro
   params_.output_frame = declare_parameter<std::string>("output_frame");
   params_.lidar_timestamp_offsets =
     declare_parameter<std::vector<double>>("lidar_timestamp_offsets");
+  params_.lidar_timestamp_noise_window =
+    declare_parameter<std::vector<double>>("lidar_timestamp_noise_window");
 
   if (params_.input_topics.empty()) {
     RCLCPP_ERROR(get_logger(), "Need a 'input_topics' parameter to be set before continuing!");
@@ -110,12 +112,19 @@ PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchro
     return;
   }
   if (params_.lidar_timestamp_offsets.size() != params_.input_topics.size()) {
-    RCLCPP_ERROR(get_logger(), "The number of topics does not match the number of offsets");
+    RCLCPP_ERROR(
+      get_logger(), "The number of topics does not match the number of timestamp offsets");
+    return;
+  }
+  if (params_.lidar_timestamp_noise_window.size() != params_.input_topics.size()) {
+    RCLCPP_ERROR(
+      get_logger(), "The number of topics does not match the number of timestamp noise windwo");
     return;
   }
 
   for (size_t i = 0; i < params_.input_topics.size(); i++) {
     topic_to_offset_map_[params_.input_topics[i]] = params_.lidar_timestamp_offsets[i];
+    topic_to_noise_window_map[params_.input_topics[i]] = params_.lidar_timestamp_noise_window[i];
   }
 
   // Publishers
@@ -237,14 +246,17 @@ void PointCloudConcatenateDataSynchronizerComponent::cloud_callback(
     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] =
+        cloud_collector->getReferenceTimeStampBoundary();
       if (
-        std::abs(
-          rclcpp::Time(input_ptr->header.stamp).seconds() - topic_to_offset_map_[topic_name] -
-          cloud_collector->getTimeStamp()) < params_.offset_tolerance) {
+        rclcpp::Time(input_ptr->header.stamp).seconds() - topic_to_offset_map_[topic_name] <
+          reference_timestamp_max + topic_to_noise_window_map[topic_name] &&
+        rclcpp::Time(input_ptr->header.stamp).seconds() - topic_to_offset_map_[topic_name] >
+          reference_timestamp_min - topic_to_noise_window_map[topic_name]) {
         lock.unlock();
         cloud_collector->processCloud(topic_name, input_ptr);
         collector_found = true;
-        // std::cout << "find collector " << cloud_collector << std::endl;
+        std::cout << "find collector " << cloud_collector << std::endl;
         break;
       }
     }
@@ -258,8 +270,9 @@ void PointCloudConcatenateDataSynchronizerComponent::cloud_callback(
 
     cloud_collectors_.push_back(new_cloud_collector);
     lock.unlock();
-    new_cloud_collector->setTimeStamp(
-      rclcpp::Time(input_ptr->header.stamp).seconds() - topic_to_offset_map_[topic_name]);
+    new_cloud_collector->setReferenceTimeStamp(
+      rclcpp::Time(input_ptr->header.stamp).seconds() - topic_to_offset_map_[topic_name],
+      topic_to_noise_window_map[topic_name]);
     new_cloud_collector->processCloud(topic_name, input_ptr);
   }
 }