feat: refactor naive method and fix the multithreading issue

Signed-off-by: vividf <[email protected]>

sensing/autoware_pointcloud_preprocessor/CMakeLists.txt

@@ -67,6 +67,7 @@ ament_auto_add_library(pointcloud_preprocessor_filter SHARED
   src/concatenate_data/concatenate_and_time_sync_node.cpp
   src/concatenate_data/combine_cloud_handler.cpp
   src/concatenate_data/cloud_collector.cpp
+  src/concatenate_data/collector_matching_strategy.cpp
   src/concatenate_data/concatenate_pointclouds.cpp
   src/crop_box_filter/crop_box_filter_node.cpp
   src/time_synchronizer/time_synchronizer_node.cpp

sensing/autoware_pointcloud_preprocessor/config/concatenate_and_time_sync_node.param.yaml

@@ -1,7 +1,5 @@
 /**:
   ros__parameters:
-    # ignore the lidar_timestamp_offsets and lidar_timestamp_noise_window if 'use_naive_approach' is true
-    use_naive_approach: true
     debug_mode: false
     has_static_tf_only: false
     rosbag_length: 10.0
@@ -19,5 +17,7 @@
                     "/sensing/lidar/left/pointcloud_before_sync",
                 ]
     output_frame: base_link
-    lidar_timestamp_offsets: [0.0, 0.015, 0.016]
-    lidar_timestamp_noise_window: [0.01, 0.01, 0.01]
+    matching_strategy:
+      type: advanced
+      lidar_timestamp_offsets: [0.0, 0.015, 0.016]
+      lidar_timestamp_noise_window: [0.01, 0.01, 0.01]

sensing/autoware_pointcloud_preprocessor/docs/concatenate-data.md

@@ -2,7 +2,32 @@
 
 ## Purpose
 
-The `concatenate_and_time_synchronize_node` is a ros2 node that combines and synchronizes multiple point clouds into a single concatenated point cloud. This enhances the sensing range for autonomous driving vehicles by integrating data from multiple LiDARs.
+The `concatenate_and_time_synchronize_node` is a ROS2 node designed to combine and synchronize multiple point clouds into a single, unified point cloud. By integrating data from multiple LiDARs, this node significantly enhances the sensing range and coverage of autonomous vehicles, enabling more accurate perception of the surrounding environment. Synchronization ensures that point clouds are aligned temporally, reducing errors caused by mismatched timestamps.
+
+For example, consider a vehicle equipped with three LiDAR sensors mounted on the left, right, and top positions. Each LiDAR captures data from its respective field of view, as shown below:
+
+<div align="center">
+  <table>
+    <tr>
+      <td><img src="./image/concatenate_left.png" alt="Concatenate Left" width="300"></td>
+      <td><img src="./image/concatenate_top.png" alt="Concatenate Top" width="300"></td>
+      <td><img src="./image/concatenate_right.png" alt="Concatenate Right" width="300"></td>
+    </tr>
+    <tr>
+      <td align="center">Left</td>
+      <td align="center">Right</td>
+      <td align="center">Top</td>
+    </tr>
+  </table>
+</div>
+
+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 align="center">
+  <img src="./image/concatenate_all.png" alt="Full Scene View" width="500">
+</div>
+
+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.
 
 ## Inner Workings / Algorithms
 
@@ -47,9 +72,9 @@ By setting the `input_twist_topic_type` parameter to `twist` or `odom`, the subs
 
 ### Parameter Settings
 
-If your LiDAR sensor does not support synchronization, set `use_naive_approach` to `true` to use the naive approach, which bypasses point cloud timestamps and directly concatenates the point clouds. On the other hand, if your LiDAR sensors are synchronized, set `use_naive_approach` to `false` and adjust the `lidar_timestamp_offsets` and `lidar_timestamp_noise_window` parameters accordingly.
+If you didn't synchronize your LiDAR sensors, set the `type` parameter of `matching_strategy` to `naive` to concatenate the point clouds directly. However, if your LiDAR sensors are synchronized, set type to `advanced` and adjust the `lidar_timestamp_offsets` and `lidar_timestamp_noise_window` parameters accordingly.
 
-The three parameters, `timeout_sec`, `lidar_timestamp_offsets`, and `lidar_timestamp_noise_window`, are essential for efficiently collecting point clouds in the same collector and managing edge cases effectively.
+The three parameters, `timeout_sec`, `lidar_timestamp_offsets`, and `lidar_timestamp_noise_window`, are essential for efficiently collecting point clouds in the same collector and managing edge cases (point cloud drops or delays) effectively.
 
 #### timeout_sec
 
@@ -209,4 +234,4 @@ Note that the `concatenate_pointclouds` and `time_synchronizer_nodelet` are usin
 ## Assumptions / Known Limits
 
 - If `is_motion_compensated` is set to `false`, the `concatenate_and_time_sync_node` will directly concatenate the point clouds without applying for motion compensation. This can save several milliseconds depending on the number of LiDARs being concatenated. Therefore, if the timestamp differences between point clouds are negligible, the user can set `is_motion_compensated` to `false` and omit the need for twist or odometry input for the node.
-- As mentioned above, the user should clearly understand how their LiDAR's point cloud timestamps are managed to set the parameters correctly. If the user does not synchronize the point clouds, please set `use_naive_approach` to `true`.
+- As mentioned above, the user should clearly understand how their LiDAR's point cloud timestamps are managed to set the parameters correctly. If the user does not synchronize the point clouds, please set `matching_strategy.type` to `naive`.

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

@@ -42,7 +42,7 @@ class CloudCollector
   void set_reference_timestamp(double timestamp, double noise_window);
   std::tuple<double, double> get_reference_timestamp_boundary();
   bool topic_exists(const std::string & topic_name);
-  void process_pointcloud(
+  bool process_pointcloud(
     const std::string & topic_name, sensor_msgs::msg::PointCloud2::SharedPtr cloud);
   void concatenate_callback();
 
@@ -52,6 +52,8 @@ class CloudCollector
   std::unordered_map<std::string, sensor_msgs::msg::PointCloud2::SharedPtr>
   get_topic_to_cloud_map();
 
+  bool concatenate_finished() const;
+
 private:
   std::shared_ptr<PointCloudConcatenateDataSynchronizerComponent> ros2_parent_node_;
   std::shared_ptr<CombineCloudHandler> combine_cloud_handler_;
@@ -63,6 +65,8 @@ class CloudCollector
   double reference_timestamp_max_{0.0};
   double arrival_timestamp_{0.0};  // This is used for the naive approach
   bool debug_mode_;
+  bool concatenate_finished_{false};
+  std::mutex concatenate_mutex_;
 };
 
 }  // namespace autoware::pointcloud_preprocessor

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

@@ -0,0 +1,82 @@
+// 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.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#pragma once
+
+#include "cloud_collector.hpp"
+
+#include <rclcpp/node.hpp>
+
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
+#include <list>
+#include <memory>
+#include <optional>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+namespace autoware::pointcloud_preprocessor
+{
+
+struct MatchingParams
+{
+  std::string topic_name;
+  double cloud_timestamp;
+  double cloud_arrival_time;
+};
+
+class CollectorMatchingStrategy
+{
+public:
+  virtual ~CollectorMatchingStrategy() = default;
+
+  [[nodiscard]] virtual std::optional<std::shared_ptr<CloudCollector>> match_cloud_to_collector(
+    const std::list<std::shared_ptr<CloudCollector>> & cloud_collectors,
+    const MatchingParams & params) const = 0;
+  virtual void set_collector_timestamp(
+    std::shared_ptr<CloudCollector> & collector, const MatchingParams & matching_params) = 0;
+};
+
+class NaiveMatchingStrategy : public CollectorMatchingStrategy
+{
+public:
+  explicit NaiveMatchingStrategy(rclcpp::Node & node);
+  [[nodiscard]] std::optional<std::shared_ptr<CloudCollector>> match_cloud_to_collector(
+    const std::list<std::shared_ptr<CloudCollector>> & cloud_collectors,
+    const MatchingParams & params) const override;
+  void set_collector_timestamp(
+    std::shared_ptr<CloudCollector> & collector, const MatchingParams & matching_params) override;
+};
+
+class AdvancedMatchingStrategy : public CollectorMatchingStrategy
+{
+public:
+  explicit AdvancedMatchingStrategy(rclcpp::Node & node, std::vector<std::string> input_topics);
+
+  [[nodiscard]] std::optional<std::shared_ptr<CloudCollector>> match_cloud_to_collector(
+    const std::list<std::shared_ptr<CloudCollector>> & cloud_collectors,
+    const MatchingParams & params) const override;
+  void set_collector_timestamp(
+    std::shared_ptr<CloudCollector> & collector, const MatchingParams & matching_params) override;
+
+private:
+  std::vector<std::string> input_topics_;
+  std::unordered_map<std::string, double> topic_to_offset_map_;
+  std::unordered_map<std::string, double> topic_to_noise_window_map_;
+};
+
+std::shared_ptr<CollectorMatchingStrategy> parse_matching_strategy(rclcpp::Node & node);
+
+}  // namespace autoware::pointcloud_preprocessor

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

@@ -23,6 +23,7 @@
 
 // ROS includes
 #include "cloud_collector.hpp"
+#include "collector_matching_strategy.hpp"
 #include "combine_cloud_handler.hpp"
 
 #include <autoware/universe_utils/ros/debug_publisher.hpp>
@@ -55,7 +56,7 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node
   void publish_clouds(
     ConcatenatedCloudResult && concatenated_cloud_result, double reference_timestamp_min,
     double reference_timestamp_max, double arrival_timestamp);
-  void delete_collector(CloudCollector & cloud_collector);
+  void manage_collector_list();
   std::list<std::shared_ptr<CloudCollector>> get_cloud_collectors();
   void add_cloud_collector(const std::shared_ptr<CloudCollector> & collector);
 
@@ -76,8 +77,6 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node
     std::string input_twist_topic_type;
     std::vector<std::string> input_topics;
     std::string output_frame;
-    std::vector<double> lidar_timestamp_offsets;
-    std::vector<double> lidar_timestamp_noise_window;
   } params_;
 
   double current_concatenate_cloud_timestamp_{0.0};
@@ -92,9 +91,8 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node
 
   std::shared_ptr<CombineCloudHandler> combine_cloud_handler_;
   std::list<std::shared_ptr<CloudCollector>> cloud_collectors_;
+  std::unique_ptr<CollectorMatchingStrategy> collector_matching_strategy_;
   std::mutex cloud_collectors_mutex_;
-  std::unordered_map<std::string, double> topic_to_offset_map_;
-  std::unordered_map<std::string, double> topic_to_noise_window_map_;
 
   // default postfix name for synchronized pointcloud
   static constexpr const char * default_sync_topic_postfix = "_synchronized";

sensing/autoware_pointcloud_preprocessor/schema/concatenate_and_time_sync_node.schema.json

@@ -6,11 +6,6 @@
     "concatenate_and_time_sync_node": {
       "type": "object",
       "properties": {
-        "use_naive_approach": {
-          "type": "boolean",
-          "default": false,
-          "description": "Flag to enable a naive approach for concatenating point clouds without considering their timestamps. Set this to `false` and adjust `lidar_timestamp_offsets` and `lidar_timestamp_noise_window` if your LiDAR sensor supports synchronization."
-        },
         "debug_mode": {
           "type": "boolean",
           "default": false,
@@ -86,29 +81,56 @@
           "minLength": 1,
           "description": "Output frame."
         },
-        "lidar_timestamp_offsets": {
-          "type": "array",
-          "items": {
-            "type": "number",
-            "minimum": 0.0
+        "matching_strategy": {
+          "type": "object",
+          "properties": {
+            "type": {
+              "type": "string",
+              "enum": ["naive", "advanced"],
+              "default": "advanced",
+              "description": "Set it to `advanced` if you can synchronize your LiDAR sensor; otherwise, set it to `naive`."
+            },
+            "lidar_timestamp_offsets": {
+              "type": "array",
+              "items": {
+                "type": "number",
+                "minimum": 0.0
+              },
+              "default": [0.0, 0.0, 0.0],
+              "minItems": 2,
+              "description": "List of LiDAR timestamp offsets in seconds (relative to the earliest LiDAR timestamp). The offsets should be provided in the same order as the input topics."
+            },
+            "lidar_timestamp_noise_window": {
+              "type": "array",
+              "items": {
+                "type": "number",
+                "minimum": 0.0
+              },
+              "default": [0.01, 0.01, 0.01],
+              "minItems": 2,
+              "description": "List of LiDAR timestamp noise windows in seconds. The noise values should be specified in the same order as the input_topics."
+            }
           },
-          "default": [0.0, 0.0, 0.0],
-          "minItems": 2,
-          "description": "List of LiDAR timestamp offsets in seconds (relative to the earliest LiDAR timestamp). The offsets should be provided in the same order as the input topics."
-        },
-        "lidar_timestamp_noise_window": {
-          "type": "array",
-          "items": {
-            "type": "number",
-            "minimum": 0.0
-          },
-          "default": [0.01, 0.01, 0.01],
-          "minItems": 2,
-          "description": "List of LiDAR timestamp noise windows in seconds. The noise values should be specified in the same order as the input_topics."
+          "required": ["type"],
+          "dependencies": {
+            "type": {
+              "oneOf": [
+                {
+                  "properties": { "type": { "const": "naive" } },
+                  "not": {
+                    "required": ["lidar_timestamp_offsets", "lidar_timestamp_noise_window"]
+                  }
+                },
+                {
+                  "properties": { "type": { "const": "advanced" } },
+                  "required": ["lidar_timestamp_offsets", "lidar_timestamp_noise_window"]
+                }
+              ]
+            }
+          }
         }
       },
       "required": [
-        "use_naive_approach",
         "debug_mode",
         "has_static_tf_only",
         "rosbag_length",
@@ -122,8 +144,7 @@
         "input_twist_topic_type",
         "input_topics",
         "output_frame",
-        "lidar_timestamp_offsets",
-        "lidar_timestamp_noise_window"
+        "matching_strategy"
       ]
     }
   },

sensing/autoware_pointcloud_preprocessor/src/concatenate_data/cloud_collector.cpp

@@ -41,9 +41,12 @@ CloudCollector::CloudCollector(
   const auto period_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
     std::chrono::duration<double>(timeout_sec_));
 
-  timer_ = rclcpp::create_timer(
-    ros2_parent_node_, ros2_parent_node_->get_clock(), period_ns,
-    std::bind(&CloudCollector::concatenate_callback, this));
+  timer_ =
+    rclcpp::create_timer(ros2_parent_node_, ros2_parent_node_->get_clock(), period_ns, [this]() {
+      std::lock_guard<std::mutex> concatenate_lock(concatenate_mutex_);
+      if (concatenate_finished_) return;
+      concatenate_callback();
+    });
 }
 
 void CloudCollector::set_arrival_timestamp(double timestamp)
@@ -72,9 +75,12 @@ bool CloudCollector::topic_exists(const std::string & topic_name)
   return topic_to_cloud_map_.find(topic_name) != topic_to_cloud_map_.end();
 }
 
-void CloudCollector::process_pointcloud(
+bool CloudCollector::process_pointcloud(
   const std::string & topic_name, sensor_msgs::msg::PointCloud2::SharedPtr cloud)
 {
+  std::lock_guard<std::mutex> concatenate_lock(concatenate_mutex_);
+  if (concatenate_finished_) return false;
+
   // 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()) {
@@ -88,6 +94,13 @@ void CloudCollector::process_pointcloud(
   if (topic_to_cloud_map_.size() == num_of_clouds_) {
     concatenate_callback();
   }
+
+  return true;
+}
+
+bool CloudCollector::concatenate_finished() const
+{
+  return concatenate_finished_;
 }
 
 void CloudCollector::concatenate_callback()
@@ -127,7 +140,7 @@ void CloudCollector::concatenate_callback()
     std::move(concatenated_cloud_result), reference_timestamp_min_, reference_timestamp_max_,
     arrival_timestamp_);
 
-  ros2_parent_node_->delete_collector(*this);
+  concatenate_finished_ = true;
 }
 
 ConcatenatedCloudResult CloudCollector::concatenate_pointclouds(