feat(autoware_pointcloud_preprocessor): redesign concatenate and time sync node

sensing/autoware_pointcloud_preprocessor/docs/concatenate-data.md

@@ -12,9 +12,7 @@ For example, consider a vehicle equipped with three LiDAR sensors mounted on the
 
 After processing the data through the `concatenate_and_time_synchronize_node`, the outputs from all LiDARs are combined into a single comprehensive point cloud that provides a complete view of the environment:
 
-<div style="margin: 0 auto; max-width: 500px" markdown="1">
-  ![Full Scene View](./image/concatenate_all.png)
-</div>
+![Full Scene View](./image/concatenate_all.png)
 
 This resulting point cloud allows autonomous systems to detect obstacles, map the environment, and navigate more effectively, leveraging the complementary fields of view from multiple LiDAR sensors.
 
@@ -130,16 +128,16 @@ colcon test --packages-select autoware_pointcloud_preprocessor --event-handlers
 
 ## Debug and Diagnostics
 
-To verify whether the node has successfully concatenated the point clouds, the user can examine the `/diagnostics` topic using the following command:
+To verify whether the node has successfully concatenated the point clouds, the user can examine rqt or the `/diagnostics` topic using the following command:
 
 ```bash
 ros2 topic echo /diagnostics
 ```
 
 Below is an example output when the point clouds are concatenated successfully:
 
-- Each point cloud has a value of `1`.
-- The `concatenate status` is `1`.
+- Each point cloud has a value of `True`.
+- The `cloud concatenation success` is `True`.
 - The `level` value is `\0`. (diagnostic_msgs::msg::DiagnosticStatus::OK)
 
 ```bash
@@ -162,24 +160,24 @@ status:
     value: '1718260240.169229984'
   - key: /sensing/lidar/left/pointcloud_before_sync timestamp
     value: '1718260240.159229994'
-  - key: /sensing/lidar/left/pointcloud_before_sync
-    value: '1'
+  - key: /sensing/lidar/left/pointcloud_before_sync is concatenated
+    value: 'True'
   - key: /sensing/lidar/right/pointcloud_before_sync timestamp
     value: '1718260240.194104910'
-  - key: /sensing/lidar/right/pointcloud_before_sync
-    value: '1'
+  - key: /sensing/lidar/right/pointcloud_before_sync is concatenated
+    value: 'True'
   - key: /sensing/lidar/top/pointcloud_before_sync timestamp
     value: '1718260240.234578133'
-  - key: /sensing/lidar/top/pointcloud_before_sync
-    value: '1'
-  - key: concatenate status
-    value: '1'
+  - key: /sensing/lidar/top/pointcloud_before_sync is concatenated
+    value: 'True'
+  - key: cloud concatenation success
+    value: 'True'
 ```
 
 Below is an example when point clouds fail to concatenate successfully.
 
-- Some point clouds might have values of `0`.
-- The `concatenate status` is `0`.
+- Some point clouds might have values of `False`.
+- The `cloud concatenation success` is `False`.
 - The `level` value is `\x02`. (diagnostic_msgs::msg::DiagnosticStatus::ERROR)
 
 ```bash
@@ -202,16 +200,16 @@ status:
     value: '1718260240.869827986'
   - key: /sensing/lidar/left/pointcloud_before_sync timestamp
     value: '1718260240.859827995'
-  - key: /sensing/lidar/left/pointcloud_before_sync
-    value: '1'
+  - key: /sensing/lidar/left/pointcloud_before_sync is concatenated
+    value: 'True'
   - key: /sensing/lidar/right/pointcloud_before_sync timestamp
     value: '1718260240.895193815'
-  - key: /sensing/lidar/right/pointcloud_before_sync
-    value: '1'
-  - key: /sensing/lidar/top/pointcloud_before_sync
-    value: '0'
-  - key: concatenate status
-    value: '0'
+  - key: /sensing/lidar/right/pointcloud_before_sync is concatenated
+    value: 'True'
+  - key: /sensing/lidar/top/pointcloud_before_sync is concatenated
+    value: 'False'
+  - key: cloud concatenation success
+    value: 'False'
 ```
 
 ## Node separation options

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

@@ -29,11 +29,31 @@
 
 enum class CollectorStrategyType { Naive, Advanced };
 
-struct CollectorInfo
+struct CollectorInfoBase
+{
+  virtual ~CollectorInfoBase() = default;
+  [[nodiscard]] virtual CollectorStrategyType getStrategyType() const = 0;
+};
+
+struct NaiveCollectorInfo : public CollectorInfoBase
+{
+  double timestamp{0.0};
+
+  [[nodiscard]] CollectorStrategyType getStrategyType() const override
+  {
+    return CollectorStrategyType::Naive;
+  }
+};
+
+struct AdvancedCollectorInfo : public CollectorInfoBase
 {
   double timestamp{0.0};
   double noise_window{0.0};
-  CollectorStrategyType strategy_type{CollectorStrategyType::Naive};
+
+  [[nodiscard]] CollectorStrategyType getStrategyType() const override
+  {
+    return CollectorStrategyType::Advanced;
+  }
 };
 
 class CloudCollector
@@ -54,10 +74,10 @@
   std::unordered_map<std::string, sensor_msgs::msg::PointCloud2::SharedPtr>
   get_topic_to_cloud_map();
 
-  bool concatenate_finished() const;
+  [[nodiscard]] bool concatenate_finished() const;
 
-  void set_info(CollectorInfo collector_info);
-  CollectorInfo get_info() const;
+  void set_info(std::shared_ptr<CollectorInfoBase> collector_info);
+  [[nodiscard]] std::shared_ptr<CollectorInfoBase> get_info() const;
   void show_debug_message();
 
 private:
@@ -70,7 +90,7 @@
   bool debug_mode_;
   bool concatenate_finished_{false};
   std::mutex concatenate_mutex_;
-  CollectorInfo collector_info_;
+  std::shared_ptr<CollectorInfoBase> collector_info_;
 };
 
 }  // namespace autoware::pointcloud_preprocessor

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

@@ -54,7 +54,8 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node
   explicit PointCloudConcatenateDataSynchronizerComponent(const rclcpp::NodeOptions & node_options);
   ~PointCloudConcatenateDataSynchronizerComponent() override = default;
   void publish_clouds(
-    ConcatenatedCloudResult && concatenated_cloud_result, CollectorInfo collector_info);
+    ConcatenatedCloudResult && concatenated_cloud_result,
+    std::shared_ptr<CollectorInfoBase> collector_info);
   void manage_collector_list();
   std::list<std::shared_ptr<CloudCollector>> get_cloud_collectors();
   void add_cloud_collector(const std::shared_ptr<CloudCollector> & collector);
@@ -84,7 +85,7 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node
   bool drop_previous_but_late_pointcloud_{false};
   bool publish_pointcloud_{false};
   bool is_concatenated_cloud_empty_{false};
-  CollectorInfo diagnostic_collector_info_;
+  std::shared_ptr<CollectorInfoBase> diagnostic_collector_info_;
   std::unordered_map<std::string, double> diagnostic_topic_to_original_stamp_map_;
 
   std::shared_ptr<CombineCloudHandler> combine_cloud_handler_;

sensing/autoware_pointcloud_preprocessor/src/concatenate_data/cloud_collector.cpp

@@ -48,19 +48,19 @@
     });
 }
 
-void CloudCollector::set_info(CollectorInfo collector_info)
+void CloudCollector::set_info(std::shared_ptr<CollectorInfoBase> collector_info)
 {
-  collector_info_ = collector_info;
+  collector_info_ = std::move(collector_info);
 }
 
-CollectorInfo CloudCollector::get_info() const
+std::shared_ptr<CollectorInfoBase> CloudCollector::get_info() const
 {
   return collector_info_;
 }
 
 bool CloudCollector::topic_exists(const std::string & topic_name)
 {
   return topic_to_cloud_map_.find(topic_name) != topic_to_cloud_map_.end();
 }
 
 bool CloudCollector::process_pointcloud(
@@ -72,7 +72,7 @@
   // Check if the map already contains an entry for the same topic. This shouldn't happen if the
   // parameter 'lidar_timestamp_noise_window' is set correctly.
   if (topic_to_cloud_map_.find(topic_name) != topic_to_cloud_map_.end()) {
     RCLCPP_WARN_STREAM_THROTTLE(
       ros2_parent_node_->get_logger(), *ros2_parent_node_->get_clock(),
       std::chrono::milliseconds(10000).count(),
       "Topic '" << topic_name
@@ -94,7 +94,7 @@
 void CloudCollector::concatenate_callback()
 {
   if (debug_mode_) {
     show_debug_message();
   }
 
   // All pointclouds are received or the timer has timed out, cancel the timer and concatenate the
@@ -120,22 +120,26 @@
   return topic_to_cloud_map_;
 }
 
 void CloudCollector::show_debug_message()
 {
   auto time_until_trigger = timer_->time_until_trigger();
   std::stringstream log_stream;
   log_stream << std::fixed << std::setprecision(6);
   log_stream << "Collector's concatenate callback time: "
              << ros2_parent_node_->get_clock()->now().seconds() << " seconds\n";
 
-  if (collector_info_.strategy_type == CollectorStrategyType::Advanced) {
-    log_stream << "Advanced strategy:\n Collector's reference time min: "
-               << collector_info_.timestamp - collector_info_.noise_window
-               << " to max: " << collector_info_.timestamp + collector_info_.noise_window
-               << " seconds\n";
-  } else if (collector_info_.strategy_type == CollectorStrategyType::Naive) {
-    log_stream << "Naive strategy:\n Collector's first cloud arrival time: "
-               << collector_info_.timestamp << " seconds\n";
+  if (collector_info_) {
+    if (collector_info_->getStrategyType() == CollectorStrategyType::Advanced) {
+      auto advanced_info = std::dynamic_pointer_cast<AdvancedCollectorInfo>(collector_info_);
+      log_stream << "Advanced strategy:\n Collector's reference time min: "
+                 << advanced_info->timestamp - advanced_info->noise_window
+                 << " to max: " << advanced_info->timestamp + advanced_info->noise_window
+                 << " seconds\n";
+    } else if (collector_info_->getStrategyType() == CollectorStrategyType::Naive) {
+      auto naive_info = std::dynamic_pointer_cast<NaiveCollectorInfo>(collector_info_);
+      log_stream << "Naive strategy:\n Collector's timestamp: " << naive_info->timestamp
+                 << " seconds\n";
+    }
   }
 
   log_stream << "Time until trigger: " << (time_until_trigger.count() / 1e9) << " seconds\n";
@@ -151,6 +155,6 @@
   log_stream << "]\n";
 
   RCLCPP_INFO(ros2_parent_node_->get_logger(), "%s", log_stream.str().c_str());
 }
 
 }  // namespace autoware::pointcloud_preprocessor

sensing/autoware_pointcloud_preprocessor/src/concatenate_data/collector_matching_strategy.cpp

@@ -26,13 +26,13 @@
 namespace autoware::pointcloud_preprocessor
 {
 
 NaiveMatchingStrategy::NaiveMatchingStrategy(rclcpp::Node & node)
 {
   strategy_type_ = CollectorStrategyType::Naive;
   RCLCPP_INFO(node.get_logger(), "Utilize naive matching strategy");
 }
 
 std::optional<std::shared_ptr<CloudCollector>> NaiveMatchingStrategy::match_cloud_to_collector(
   const std::list<std::shared_ptr<CloudCollector>> & cloud_collectors,
   const MatchingParams & params) const
 {
@@ -41,27 +41,27 @@
 
   for (const auto & cloud_collector : cloud_collectors) {
     if (!cloud_collector->topic_exists(params.topic_name)) {
-      CollectorInfo collector_info = cloud_collector->get_info();
-      double time_difference = std::abs(params.cloud_arrival_time - collector_info.timestamp);
-      if (!smallest_time_difference || time_difference < smallest_time_difference) {
-        smallest_time_difference = time_difference;
-        closest_collector = cloud_collector;
+      auto info = cloud_collector->get_info();
+      if (auto naive_info = std::dynamic_pointer_cast<NaiveCollectorInfo>(info)) {
+        double time_difference = std::abs(params.cloud_arrival_time - naive_info->timestamp);
+        if (!smallest_time_difference || time_difference < smallest_time_difference) {
+          smallest_time_difference = time_difference;
+          closest_collector = cloud_collector;
+        }
       }
     }
   }
 
   return closest_collector;
 }
 
 void NaiveMatchingStrategy::set_collector_info(
   std::shared_ptr<CloudCollector> & collector, const MatchingParams & matching_params)
 {
-  CollectorInfo collector_info;
-  ;
-  collector_info.timestamp = matching_params.cloud_arrival_time;
-  collector_info.strategy_type = strategy_type_;
-  collector->set_info(collector_info);
+  auto info = std::make_shared<NaiveCollectorInfo>();
+  info->timestamp = matching_params.cloud_arrival_time;
+  collector->set_info(info);
 }
 
 AdvancedMatchingStrategy::AdvancedMatchingStrategy(
   rclcpp::Node & node, std::vector<std::string> input_topics)
@@ -74,11 +74,11 @@
 
   if (lidar_timestamp_offsets.size() != input_topics.size()) {
     throw std::runtime_error(
       "The number of topics does not match the number of timestamp offsets.");
   }
   if (lidar_timestamp_noise_window.size() != input_topics.size()) {
     throw std::runtime_error(
       "The number of topics does not match the number of timestamp noise window.");
   }
 
   for (size_t i = 0; i < input_topics.size(); i++) {
@@ -96,14 +96,16 @@
   const MatchingParams & params) const
 {
   for (const auto & cloud_collector : cloud_collectors) {
-    CollectorInfo collector_info = cloud_collector->get_info();
-    auto reference_timestamp_min = collector_info.timestamp - collector_info.noise_window;
-    auto reference_timestamp_max = collector_info.timestamp + collector_info.noise_window;
-    double time = params.cloud_timestamp - topic_to_offset_map_.at(params.topic_name);
-    if (
-      time < reference_timestamp_max + topic_to_noise_window_map_.at(params.topic_name) &&
-      time > reference_timestamp_min - topic_to_noise_window_map_.at(params.topic_name)) {
-      return cloud_collector;
+    auto info = cloud_collector->get_info();
+    if (auto advanced_info = std::dynamic_pointer_cast<AdvancedCollectorInfo>(info)) {
+      auto reference_timestamp_min = advanced_info->timestamp - advanced_info->noise_window;
+      auto reference_timestamp_max = advanced_info->timestamp + advanced_info->noise_window;
+      double time = params.cloud_timestamp - topic_to_offset_map_.at(params.topic_name);
+      if (
+        time < reference_timestamp_max + topic_to_noise_window_map_.at(params.topic_name) &&
+        time > reference_timestamp_min - topic_to_noise_window_map_.at(params.topic_name)) {
+        return cloud_collector;
+      }
     }
   }
   return std::nullopt;
@@ -112,13 +114,11 @@
 void AdvancedMatchingStrategy::set_collector_info(
   std::shared_ptr<CloudCollector> & collector, const MatchingParams & matching_params)
 {
-  CollectorInfo collector_info;
-  ;
-  collector_info.timestamp =
+  auto info = std::make_shared<AdvancedCollectorInfo>();
+  info->timestamp =
     matching_params.cloud_timestamp - topic_to_offset_map_[matching_params.topic_name];
-  collector_info.noise_window = topic_to_noise_window_map_[matching_params.topic_name];
-  collector_info.strategy_type = strategy_type_;
-  collector->set_info(collector_info);
+  info->noise_window = topic_to_noise_window_map_[matching_params.topic_name];
+  collector->set_info(info);
 }
 
 }  // namespace autoware::pointcloud_preprocessor

sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_node.cpp

@@ -1,4 +1,4 @@
 // Copyright 2024 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -33,85 +33,85 @@
 namespace autoware::pointcloud_preprocessor
 {
 
 PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchronizerComponent(
   const rclcpp::NodeOptions & node_options)
 : Node("point_cloud_concatenator_component", node_options)
 {
   // initialize debug tool
   using autoware::universe_utils::DebugPublisher;
   using autoware::universe_utils::StopWatch;
   stop_watch_ptr_ = std::make_unique<StopWatch<std::chrono::milliseconds>>();
   debug_publisher_ = std::make_unique<DebugPublisher>(this, "concatenate_data_synchronizer");
   stop_watch_ptr_->tic("cyclic_time");
   stop_watch_ptr_->tic("processing_time");
 
   //  initialize parameters
   params_.debug_mode = declare_parameter<bool>("debug_mode");
   params_.has_static_tf_only = declare_parameter<bool>("has_static_tf_only");
   params_.rosbag_length = declare_parameter<double>("rosbag_length");
   params_.maximum_queue_size = static_cast<size_t>(declare_parameter<int>("maximum_queue_size"));
   params_.timeout_sec = declare_parameter<double>("timeout_sec");
   params_.is_motion_compensated = declare_parameter<bool>("is_motion_compensated");
   params_.publish_synchronized_pointcloud =
     declare_parameter<bool>("publish_synchronized_pointcloud");
   params_.keep_input_frame_in_synchronized_pointcloud =
     declare_parameter<bool>("keep_input_frame_in_synchronized_pointcloud");
   params_.publish_previous_but_late_pointcloud =
     declare_parameter<bool>("publish_previous_but_late_pointcloud");
   params_.synchronized_pointcloud_postfix =
     declare_parameter<std::string>("synchronized_pointcloud_postfix");
   params_.input_twist_topic_type = declare_parameter<std::string>("input_twist_topic_type");
   params_.input_topics = declare_parameter<std::vector<std::string>>("input_topics");
   params_.output_frame = declare_parameter<std::string>("output_frame");
 
   if (params_.input_topics.empty()) {
     throw std::runtime_error("Need a 'input_topics' parameter to be set before continuing.");
   }
   if (params_.input_topics.size() == 1) {
     throw std::runtime_error("Only one topic given. Need at least two topics to continue.");
   }
 
   if (params_.output_frame.empty()) {
     throw std::runtime_error("Need an 'output_frame' parameter to be set before continuing.");
   }
 
   params_.matching_strategy = declare_parameter<std::string>("matching_strategy.type");
   if (params_.matching_strategy == "naive") {
     collector_matching_strategy_ = std::make_unique<NaiveMatchingStrategy>(*this);
   } else if (params_.matching_strategy == "advanced") {
     collector_matching_strategy_ =
       std::make_unique<AdvancedMatchingStrategy>(*this, params_.input_topics);
   } else {
     throw std::runtime_error("Matching strategy must be 'advanced' or 'naive'");
   }
 
   // Publishers
   concatenated_cloud_publisher_ = this->create_publisher<sensor_msgs::msg::PointCloud2>(
     "output", rclcpp::SensorDataQoS().keep_last(params_.maximum_queue_size));
 
   // Transformed Raw PointCloud2 Publisher to publish the transformed pointcloud
   if (params_.publish_synchronized_pointcloud) {
     for (auto & topic : params_.input_topics) {
       std::string new_topic =
         replace_sync_topic_name_postfix(topic, params_.synchronized_pointcloud_postfix);
       auto publisher = this->create_publisher<sensor_msgs::msg::PointCloud2>(
         new_topic, rclcpp::SensorDataQoS().keep_last(params_.maximum_queue_size));
       topic_to_transformed_cloud_publisher_map_.insert({topic, publisher});
     }
   }
 
   // Subscribers
   if (params_.is_motion_compensated) {
     if (params_.input_twist_topic_type == "twist") {
       twist_sub_ = this->create_subscription<geometry_msgs::msg::TwistWithCovarianceStamped>(
         "~/input/twist", rclcpp::QoS{100},
         std::bind(
           &PointCloudConcatenateDataSynchronizerComponent::twist_callback, this,
           std::placeholders::_1));
     } else if (params_.input_twist_topic_type == "odom") {
       odom_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
         "~/input/odom", rclcpp::QoS{100},
         std::bind(
           &PointCloudConcatenateDataSynchronizerComponent::odom_callback, this,
           std::placeholders::_1));
     } else {
@@ -178,24 +178,24 @@
   return replaced_topic_name;
 }
 
 void PointCloudConcatenateDataSynchronizerComponent::cloud_callback(
   const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, const std::string & topic_name)
 {
   stop_watch_ptr_->toc("processing_time", true);
   double cloud_arrival_time = this->get_clock()->now().seconds();
   manage_collector_list();
 
   if (!utils::is_data_layout_compatible_with_point_xyzirc(*input_ptr)) {
     RCLCPP_ERROR(
       get_logger(), "The pointcloud layout is not compatible with PointXYZIRC. Aborting");
 
     if (utils::is_data_layout_compatible_with_point_xyzi(*input_ptr)) {
       RCLCPP_ERROR(
         get_logger(),
         "The pointcloud layout is compatible with PointXYZI. You may be using legacy code/data");
     }
 
     return;
   }
 
   if (params_.debug_mode) {
@@ -253,19 +253,20 @@
   combine_cloud_handler_->process_twist(input);
 }
 
 void PointCloudConcatenateDataSynchronizerComponent::odom_callback(
   const nav_msgs::msg::Odometry::ConstSharedPtr input)
 {
   combine_cloud_handler_->process_odometry(input);
 }
 
 void PointCloudConcatenateDataSynchronizerComponent::publish_clouds(
-  ConcatenatedCloudResult && concatenated_cloud_result, CollectorInfo collector_info)
+  ConcatenatedCloudResult && concatenated_cloud_result,
+  std::shared_ptr<CollectorInfoBase> collector_info)
 {
   // should never come to this state.
   if (concatenated_cloud_result.concatenate_cloud_ptr == nullptr) {
     RCLCPP_ERROR(this->get_logger(), "Concatenated cloud is a nullptr.");
     return;
   }
 
   if (concatenated_cloud_result.concatenate_cloud_ptr->data.empty()) {
@@ -280,12 +281,12 @@
     current_concatenate_cloud_timestamp_ < latest_concatenate_cloud_timestamp_ &&
     !params_.publish_previous_but_late_pointcloud) {
     // Publish the cloud if the rosbag replays in loop
     if (
       latest_concatenate_cloud_timestamp_ - current_concatenate_cloud_timestamp_ >
       params_.rosbag_length) {
       publish_pointcloud_ = true;  // Force publishing in this case
     } else {
       drop_previous_but_late_pointcloud_ = true;  // Otherwise, drop the late pointcloud
     }
   } else {
     // Publish pointcloud if timestamps are valid or the condition doesn't apply
@@ -362,82 +363,82 @@
   return oss.str();
 }
 
 void PointCloudConcatenateDataSynchronizerComponent::check_concat_status(
   diagnostic_updater::DiagnosticStatusWrapper & stat)
 {
   if (publish_pointcloud_ || drop_previous_but_late_pointcloud_) {
     stat.add(
       "concatenated cloud timestamp", format_timestamp(current_concatenate_cloud_timestamp_));
 
-    if (diagnostic_collector_info_.strategy_type == CollectorStrategyType::Naive) {
-      stat.add(
-        "first cloud's arrival timestamp", format_timestamp(diagnostic_collector_info_.timestamp));
-    } else if (diagnostic_collector_info_.strategy_type == CollectorStrategyType::Advanced) {
+    if (
+      auto naive_info = std::dynamic_pointer_cast<NaiveCollectorInfo>(diagnostic_collector_info_)) {
+      stat.add("first cloud's arrival timestamp", format_timestamp(naive_info->timestamp));
+    } else if (
+      auto advanced_info =
+        std::dynamic_pointer_cast<AdvancedCollectorInfo>(diagnostic_collector_info_)) {
       stat.add(
         "reference timestamp min",
-        format_timestamp(
-          diagnostic_collector_info_.timestamp - diagnostic_collector_info_.noise_window));
+        format_timestamp(advanced_info->timestamp - advanced_info->noise_window));
       stat.add(
         "reference timestamp max",
-        format_timestamp(
-          diagnostic_collector_info_.timestamp + diagnostic_collector_info_.noise_window));
+        format_timestamp(advanced_info->timestamp + advanced_info->noise_window));
     }
 
     bool topic_miss = false;
 
-    int concatenated_cloud_status = 1;  // Status of concatenated cloud, 1: success, 0: failure
+    bool concatenation_success = true;
     for (const auto & topic : params_.input_topics) {
-      int cloud_status = 1;  // Status of each lidar's pointcloud
+      bool input_cloud_concatenated = true;
       if (
         diagnostic_topic_to_original_stamp_map_.find(topic) !=
         diagnostic_topic_to_original_stamp_map_.end()) {
         stat.add(
           topic + " timestamp", format_timestamp(diagnostic_topic_to_original_stamp_map_[topic]));
       } else {
         topic_miss = true;
-        concatenated_cloud_status = 0;
-        cloud_status = 0;
+        concatenation_success = false;
+        input_cloud_concatenated = false;
       }
-      stat.add(topic, cloud_status);
+      stat.add(topic + " is concatenated", input_cloud_concatenated);
     }
 
-    stat.add("concatenate status", concatenated_cloud_status);
+    stat.add("cloud concatenation success", concatenation_success);
 
     int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK;
     std::string message = "Concatenated pointcloud is published and include all topics";
 
     if (drop_previous_but_late_pointcloud_) {
       if (topic_miss) {
         level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
         message =
           "Concatenated pointcloud misses some topics and is not published because it arrived "
           "too late";
       } else {
         level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
         message = "Concatenated pointcloud is not published as it is too late";
       }
     } else {
       if (is_concatenated_cloud_empty_) {
         level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
         message = "Concatenated pointcloud is empty";
       } else if (topic_miss) {
         level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
         message = "Concatenated pointcloud is published but misses some topics";
       }
     }
 
     stat.summary(level, message);
 
     publish_pointcloud_ = false;
     drop_previous_but_late_pointcloud_ = false;
     is_concatenated_cloud_empty_ = false;
   } else {
     const int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK;
     const std::string message =
       "Concatenate node launch successfully, but waiting for input pointcloud";
     stat.summary(level, message);
   }
 }
 
 std::list<std::shared_ptr<CloudCollector>>
 PointCloudConcatenateDataSynchronizerComponent::get_cloud_collectors()

sensing/autoware_pointcloud_preprocessor/test/test_concatenate_node_unit.cpp

@@ -1,4 +1,4 @@
 // Copyright 2024 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -89,24 +89,24 @@
     }
   }
 
   static geometry_msgs::msg::TransformStamped generate_transform_msg(
     const std::string & parent_frame, const std::string & child_frame, double x, double y, double z,
     double qx, double qy, double qz, double qw)
   {
     rclcpp::Time timestamp(timestamp_seconds, timestamp_nanoseconds, RCL_ROS_TIME);
     geometry_msgs::msg::TransformStamped tf_msg;
     tf_msg.header.stamp = timestamp;
     tf_msg.header.frame_id = parent_frame;
     tf_msg.child_frame_id = child_frame;
     tf_msg.transform.translation.x = x;
     tf_msg.transform.translation.y = y;
     tf_msg.transform.translation.z = z;
     tf_msg.transform.rotation.x = qx;
     tf_msg.transform.rotation.y = qy;
     tf_msg.transform.rotation.z = qz;
     tf_msg.transform.rotation.w = qw;
     return tf_msg;
   }
 
   static sensor_msgs::msg::PointCloud2 generate_pointcloud_msg(
     bool generate_points, bool is_lidar_frame, const std::string & topic_name,
@@ -187,19 +187,19 @@
 };
 
 //////////////////////////////// Test combine_cloud_handler ////////////////////////////////
 TEST_F(ConcatenateCloudTest, TestProcessTwist)
 {
   auto twist_msg = std::make_shared<geometry_msgs::msg::TwistWithCovarianceStamped>();
   twist_msg->header.stamp = rclcpp::Time(10, 0);
   twist_msg->twist.twist.linear.x = 1.0;
   twist_msg->twist.twist.angular.z = 0.1;
 
   combine_cloud_handler_->process_twist(twist_msg);
 
   ASSERT_FALSE(combine_cloud_handler_->get_twist_queue().empty());
   EXPECT_EQ(combine_cloud_handler_->get_twist_queue().front().twist.linear.x, 1.0);
   EXPECT_EQ(combine_cloud_handler_->get_twist_queue().front().twist.angular.z, 0.1);
 }
 
 TEST_F(ConcatenateCloudTest, TestProcessOdometry)
 {
@@ -291,165 +291,187 @@
 
 //////////////////////////////// Test cloud_collector ////////////////////////////////
 
-TEST_F(ConcatenateCloudTest, TestSetAndGetCollectorInfo)
+TEST_F(ConcatenateCloudTest, TestSetAndGetNaiveCollectorInfo)
 {
-  autoware::pointcloud_preprocessor::CollectorInfo collector_info;
-  collector_info.timestamp = 10.0;
-  collector_info.noise_window = 0.1;
-  collector_->set_info(collector_info);
+  auto naive_info = std::make_shared<autoware::pointcloud_preprocessor::NaiveCollectorInfo>();
+  naive_info->timestamp = 15.0;
+
+  collector_->set_info(naive_info);
+  auto collector_info_new = collector_->get_info();
+
+  auto naive_info_new =
+    std::dynamic_pointer_cast<autoware::pointcloud_preprocessor::NaiveCollectorInfo>(
+      collector_info_new);
+  ASSERT_NE(naive_info_new, nullptr) << "Collector info is not of type NaiveCollectorInfo";
+
+  EXPECT_DOUBLE_EQ(naive_info_new->timestamp, 15.0);
+}
+
+TEST_F(ConcatenateCloudTest, TestSetAndGetAdvancedCollectorInfo)
+{
+  auto advanced_info = std::make_shared<autoware::pointcloud_preprocessor::AdvancedCollectorInfo>();
+  advanced_info->timestamp = 10.0;
+  advanced_info->noise_window = 0.1;
+  collector_->set_info(advanced_info);
   auto collector_info_new = collector_->get_info();
-  auto min = collector_info_new.timestamp - collector_info_new.noise_window;
-  auto max = collector_info_new.timestamp + collector_info_new.noise_window;
+  auto advanced_info_new =
+    std::dynamic_pointer_cast<autoware::pointcloud_preprocessor::AdvancedCollectorInfo>(
+      collector_info_new);
+  ASSERT_NE(advanced_info_new, nullptr) << "Collector info is not of type AdvancedCollectorInfo";
+
+  // Validate the values
+  auto min = advanced_info_new->timestamp - advanced_info_new->noise_window;
+  auto max = advanced_info_new->timestamp + advanced_info_new->noise_window;
   EXPECT_DOUBLE_EQ(min, 9.9);
   EXPECT_DOUBLE_EQ(max, 10.1);
 }
 
 TEST_F(ConcatenateCloudTest, TestConcatenateClouds)
 {
   rclcpp::Time top_timestamp(timestamp_seconds, timestamp_nanoseconds, RCL_ROS_TIME);
   rclcpp::Time left_timestamp(timestamp_seconds, timestamp_nanoseconds + 40'000'000, RCL_ROS_TIME);
   rclcpp::Time right_timestamp(timestamp_seconds, timestamp_nanoseconds + 80'000'000, RCL_ROS_TIME);
   sensor_msgs::msg::PointCloud2 top_pointcloud =
     generate_pointcloud_msg(true, false, "lidar_top", top_timestamp);
   sensor_msgs::msg::PointCloud2 left_pointcloud =
     generate_pointcloud_msg(true, false, "lidar_left", left_timestamp);
   sensor_msgs::msg::PointCloud2 right_pointcloud =
     generate_pointcloud_msg(true, false, "lidar_right", right_timestamp);
 
   sensor_msgs::msg::PointCloud2::SharedPtr top_pointcloud_ptr =
     std::make_shared<sensor_msgs::msg::PointCloud2>(top_pointcloud);
   sensor_msgs::msg::PointCloud2::SharedPtr left_pointcloud_ptr =
     std::make_shared<sensor_msgs::msg::PointCloud2>(left_pointcloud);
   sensor_msgs::msg::PointCloud2::SharedPtr right_pointcloud_ptr =
     std::make_shared<sensor_msgs::msg::PointCloud2>(right_pointcloud);
 
   std::unordered_map<std::string, sensor_msgs::msg::PointCloud2::SharedPtr> topic_to_cloud_map;
   topic_to_cloud_map["lidar_top"] = top_pointcloud_ptr;
   topic_to_cloud_map["lidar_left"] = left_pointcloud_ptr;
   topic_to_cloud_map["lidar_right"] = right_pointcloud_ptr;
 
   auto [concatenate_cloud_ptr, topic_to_transformed_cloud_map, topic_to_original_stamp_map] =
     collector_->concatenate_pointclouds(topic_to_cloud_map);
 
   // test output concatenate cloud
   // No input twist, so it will not do the motion compensation
   std::array<Eigen::Vector3f, 10> expected_pointcloud = {
     {Eigen::Vector3f(10.0f, 0.0f, 0.0f), Eigen::Vector3f(0.0f, 10.0f, 0.0f),
      Eigen::Vector3f(0.0f, 0.0f, 10.0f), Eigen::Vector3f(10.0f, 0.0f, 0.0f),
      Eigen::Vector3f(0.0f, 10.0f, 0.0f), Eigen::Vector3f(0.0f, 0.0f, 10.0f),
      Eigen::Vector3f(10.0f, 0.0f, 0.0f), Eigen::Vector3f(0.0f, 10.0f, 0.0f),
      Eigen::Vector3f(0.0f, 0.0f, 10.0f)}};
 
   size_t i = 0;
   std::ostringstream oss;
   oss << "Concatenated pointcloud:\n";
 
   sensor_msgs::PointCloud2Iterator<float> iter_x(*concatenate_cloud_ptr, "x");
   sensor_msgs::PointCloud2Iterator<float> iter_y(*concatenate_cloud_ptr, "y");
   sensor_msgs::PointCloud2Iterator<float> iter_z(*concatenate_cloud_ptr, "z");
 
   for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++i) {
     oss << "Concatenated point " << i << ": (" << *iter_x << ", " << *iter_y << ", " << *iter_z
         << ")\n";
     EXPECT_FLOAT_EQ(*iter_x, expected_pointcloud[i].x());
     EXPECT_FLOAT_EQ(*iter_y, expected_pointcloud[i].y());
     EXPECT_FLOAT_EQ(*iter_z, expected_pointcloud[i].z());
   }
 
   if (debug_) {
     RCLCPP_INFO(concatenate_node_->get_logger(), "%s", oss.str().c_str());
   }
 
   // test concatenate cloud has the oldest pointcloud's timestamp
   EXPECT_FLOAT_EQ(
     top_timestamp.seconds(), rclcpp::Time(concatenate_cloud_ptr->header.stamp).seconds());
 
   // test separated transformed cloud
   std::array<Eigen::Vector3f, 10> expected_top_pointcloud = {
     {Eigen::Vector3f(10.0f, 0.0f, 0.0f), Eigen::Vector3f(0.0f, 10.0f, 0.0f),
      Eigen::Vector3f(0.0f, 0.0f, 10.0f)}};
   std::array<Eigen::Vector3f, 10> expected_left_pointcloud = {
     {Eigen::Vector3f(10.0f, 0.0f, 0.0f), Eigen::Vector3f(0.0f, 10.0f, 0.0f),
      Eigen::Vector3f(0.0f, 0.0f, 10.0f)}};
   std::array<Eigen::Vector3f, 10> expected_right_pointcloud = {
     {Eigen::Vector3f(10.0f, 0.0f, 0.0f), Eigen::Vector3f(0.0f, 10.0f, 0.0f),
      Eigen::Vector3f(0.0f, 0.0f, 10.0f)}};
 
   oss.clear();
   oss.str("");
   i = 0;
 
   sensor_msgs::PointCloud2Iterator<float> top_pc_iter_x(
     *(topic_to_transformed_cloud_map.value().at("lidar_top")), "x");
   sensor_msgs::PointCloud2Iterator<float> top_pc_iter_y(
     *(topic_to_transformed_cloud_map.value().at("lidar_top")), "y");
   sensor_msgs::PointCloud2Iterator<float> top_pc_iter_z(
     *(topic_to_transformed_cloud_map.value().at("lidar_top")), "z");
 
   for (; top_pc_iter_x != top_pc_iter_x.end();
        ++top_pc_iter_x, ++top_pc_iter_y, ++top_pc_iter_z, ++i) {
     oss << "Top point " << i << ": (" << *top_pc_iter_x << ", " << *top_pc_iter_y << ", "
         << *top_pc_iter_z << ")\n";
     EXPECT_FLOAT_EQ(*top_pc_iter_x, expected_top_pointcloud[i].x());
     EXPECT_FLOAT_EQ(*top_pc_iter_y, expected_top_pointcloud[i].y());
     EXPECT_FLOAT_EQ(*top_pc_iter_z, expected_top_pointcloud[i].z());
   }
 
   if (debug_) {
     RCLCPP_INFO(concatenate_node_->get_logger(), "%s", oss.str().c_str());
   }
 
   oss.clear();
   oss.str("");
   i = 0;
   sensor_msgs::PointCloud2Iterator<float> left_pc_iter_x(
     *(topic_to_transformed_cloud_map.value().at("lidar_left")), "x");
   sensor_msgs::PointCloud2Iterator<float> left_pc_iter_y(
     *(topic_to_transformed_cloud_map.value().at("lidar_left")), "y");
   sensor_msgs::PointCloud2Iterator<float> left_pc_iter_z(
     *(topic_to_transformed_cloud_map.value().at("lidar_left")), "z");
 
   for (; left_pc_iter_x != left_pc_iter_x.end();
        ++left_pc_iter_x, ++left_pc_iter_y, ++left_pc_iter_z, ++i) {
     oss << "Left point " << i << ": (" << *left_pc_iter_x << ", " << *left_pc_iter_y << ", "
         << *left_pc_iter_z << ")\n";
     EXPECT_FLOAT_EQ(*left_pc_iter_x, expected_left_pointcloud[i].x());
     EXPECT_FLOAT_EQ(*left_pc_iter_y, expected_left_pointcloud[i].y());
     EXPECT_FLOAT_EQ(*left_pc_iter_z, expected_left_pointcloud[i].z());
   }
 
   if (debug_) {
     RCLCPP_INFO(concatenate_node_->get_logger(), "%s", oss.str().c_str());
   }
 
   oss.clear();
   oss.str("");
   i = 0;
   sensor_msgs::PointCloud2Iterator<float> right_pc_iter_x(
     *(topic_to_transformed_cloud_map.value().at("lidar_right")), "x");
   sensor_msgs::PointCloud2Iterator<float> right_pc_iter_y(
     *(topic_to_transformed_cloud_map.value().at("lidar_right")), "y");
   sensor_msgs::PointCloud2Iterator<float> right_pc_iter_z(
     *(topic_to_transformed_cloud_map.value().at("lidar_right")), "z");
 
   for (; right_pc_iter_x != right_pc_iter_x.end();
        ++right_pc_iter_x, ++right_pc_iter_y, ++right_pc_iter_z, ++i) {
     oss << "Right point " << i << ": (" << *right_pc_iter_x << ", " << *right_pc_iter_y << ", "
         << *right_pc_iter_z << ")\n";
     EXPECT_FLOAT_EQ(*right_pc_iter_x, expected_right_pointcloud[i].x());
     EXPECT_FLOAT_EQ(*right_pc_iter_y, expected_right_pointcloud[i].y());
     EXPECT_FLOAT_EQ(*right_pc_iter_z, expected_right_pointcloud[i].z());
   }
 
   if (debug_) {
     RCLCPP_INFO(concatenate_node_->get_logger(), "%s", oss.str().c_str());
   }
 
   // test original cloud's timestamps
   EXPECT_FLOAT_EQ(top_timestamp.seconds(), topic_to_original_stamp_map["lidar_top"]);
   EXPECT_FLOAT_EQ(left_timestamp.seconds(), topic_to_original_stamp_map["lidar_left"]);
   EXPECT_FLOAT_EQ(right_timestamp.seconds(), topic_to_original_stamp_map["lidar_right"]);
 }
 
 TEST_F(ConcatenateCloudTest, TestProcessSingleCloud)
 {