diff --git a/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt b/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt
index 56dab521d7dff..19c698b11bc5f 100644
--- a/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt
+++ b/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt
@@ -64,7 +64,10 @@ ament_target_dependencies(faster_voxel_grid_downsample_filter
)
ament_auto_add_library(pointcloud_preprocessor_filter SHARED
- src/concatenate_data/concatenate_and_time_sync_nodelet.cpp
+ 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
@@ -111,7 +114,7 @@ rclcpp_components_register_node(pointcloud_preprocessor_filter
# ========== Concatenate and Sync data ==========
rclcpp_components_register_node(pointcloud_preprocessor_filter
PLUGIN "autoware::pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent"
- EXECUTABLE concatenate_data_node)
+ EXECUTABLE concatenate_and_time_sync_node)
# ========== CropBox ==========
rclcpp_components_register_node(pointcloud_preprocessor_filter
@@ -243,8 +246,17 @@ if(BUILD_TESTING)
test/test_distortion_corrector_node.cpp
)
+ ament_add_gtest(test_concatenate_node_unit
+ test/test_concatenate_node_unit.cpp
+ )
+
target_link_libraries(test_utilities pointcloud_preprocessor_filter)
target_link_libraries(test_distortion_corrector_node pointcloud_preprocessor_filter)
+ target_link_libraries(test_concatenate_node_unit pointcloud_preprocessor_filter)
+ add_ros_test(
+ test/test_concatenate_node_component.py
+ TIMEOUT "50"
+ )
endif()
diff --git a/sensing/autoware_pointcloud_preprocessor/config/concatenate_and_time_sync_node.param.yaml b/sensing/autoware_pointcloud_preprocessor/config/concatenate_and_time_sync_node.param.yaml
new file mode 100644
index 0000000000000..56fe6643b9973
--- /dev/null
+++ b/sensing/autoware_pointcloud_preprocessor/config/concatenate_and_time_sync_node.param.yaml
@@ -0,0 +1,21 @@
+/**:
+ ros__parameters:
+ debug_mode: false
+ has_static_tf_only: false
+ rosbag_length: 10.0
+ maximum_queue_size: 5
+ timeout_sec: 0.2
+ is_motion_compensated: true
+ publish_synchronized_pointcloud: true
+ keep_input_frame_in_synchronized_pointcloud: true
+ publish_previous_but_late_pointcloud: false
+ synchronized_pointcloud_postfix: pointcloud
+ input_twist_topic_type: twist
+ input_topics: [
+ "/sensing/lidar/right/pointcloud_before_sync",
+ "/sensing/lidar/top/pointcloud_before_sync",
+ "/sensing/lidar/left/pointcloud_before_sync",
+ ]
+ output_frame: base_link
+ matching_strategy:
+ type: naive
diff --git a/sensing/autoware_pointcloud_preprocessor/docs/concatenate-data.md b/sensing/autoware_pointcloud_preprocessor/docs/concatenate-data.md
index 08f7b92f88975..f640608cf91ec 100644
--- a/sensing/autoware_pointcloud_preprocessor/docs/concatenate-data.md
+++ b/sensing/autoware_pointcloud_preprocessor/docs/concatenate-data.md
@@ -1,75 +1,224 @@
-# concatenate_data
+# concatenate_and_time_synchronize_node
## Purpose
-Many self-driving cars combine multiple LiDARs to expand the sensing range. Therefore, a function to combine a plurality of point clouds is required.
+The `concatenate_and_time_synchronize_node` is a 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.
-To combine multiple sensor data with a similar timestamp, the [message_filters](https://github.com/ros2/message_filters) is often used in the ROS-based system, but this requires the assumption that all inputs can be received. Since safety must be strongly considered in autonomous driving, the point clouds concatenate node must be designed so that even if one sensor fails, the remaining sensor information can be output.
+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:
-## Inner-workings / Algorithms
+| Left | Top | Right |
+| :-----------------------------------------------: | :---------------------------------------------: | :-------------------------------------------------: |
+|  |  |  |
-The figure below represents the reception time of each sensor data and how it is combined in the case.
+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:
-
+
+
+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
+
+
+
+### Step 1: Match and Create Collector
+
+When a point cloud arrives, its timestamp is checked, and an offset is subtracted to get the reference timestamp. The node then checks if there is an existing collector with the same reference timestamp. If such a collector exists, the point cloud is added to it. If no such collector exists, a new collector is created with the reference timestamp.
+
+### Step 2: Trigger the Timer
+
+Once a collector is created, a timer for that collector starts counting down (this value is defined by `timeout_sec`). The collector begins to concatenate the point clouds either when all point clouds defined in `input_topics` have been collected or when the timer counts down to zero.
+
+### Step 3: Concatenate the Point Clouds
+
+The concatenation process involves merging multiple point clouds into a single, concatenated point cloud. The timestamp of the concatenated point cloud will be the earliest timestamp from the input point clouds. By setting the parameter `is_motion_compensated` to `true`, the node will consider the timestamps of the input point clouds and utilize the `twist` information from `geometry_msgs::msg::TwistWithCovarianceStamped` to compensate for motion, aligning the point cloud to the selected (earliest) timestamp.
+
+### Step 4: Publish the Point Cloud
+
+After concatenation, the concatenated point cloud is published, and the collector is deleted to free up resources.
## Inputs / Outputs
### Input
-| Name | Type | Description |
-| --------------- | ------------------------------------------------ | ----------------------------------------------------------------------------- |
-| `~/input/twist` | `geometry_msgs::msg::TwistWithCovarianceStamped` | The vehicle odometry is used to interpolate the timestamp of each sensor data |
+| Name | Type | Description |
+| --------------- | ------------------------------------------------ | -------------------------------------------------------------------------------------------------------------------------------------------------------- |
+| `~/input/twist` | `geometry_msgs::msg::TwistWithCovarianceStamped` | Twist information adjusts the point cloud scans based on vehicle motion, allowing LiDARs with different timestamps to be synchronized for concatenation. |
+| `~/input/odom` | `nav_msgs::msg::Odometry` | Vehicle odometry adjusts the point cloud scans based on vehicle motion, allowing LiDARs with different timestamps to be synchronized for concatenation. |
+
+By setting the `input_twist_topic_type` parameter to `twist` or `odom`, the subscriber will subscribe to either `~/input/twist` or `~/input/odom`. If the user doesn't want to use the twist information or vehicle odometry to compensate for motion, set `is_motion_compensated` to `false`.
### Output
| Name | Type | Description |
| ----------------- | ------------------------------- | ------------------------- |
-| `~/output/points` | `sensor_msgs::msg::Pointcloud2` | concatenated point clouds |
+| `~/output/points` | `sensor_msgs::msg::Pointcloud2` | Concatenated point clouds |
+
+### Core Parameters
-## Parameters
+{{ json_to_markdown("sensing/autoware_pointcloud_preprocessor/schema/concatenate_and_time_sync_node.schema.json") }}
-| Name | Type | Default Value | Description |
-| -------------------- | ---------------- | ------------- | ------------------------------------------------------------------- |
-| `input/points` | vector of string | [] | input topic names that type must be `sensor_msgs::msg::Pointcloud2` |
-| `input_frame` | string | "" | input frame id |
-| `output_frame` | string | "" | output frame id |
-| `has_static_tf_only` | bool | false | flag to listen TF only once |
-| `max_queue_size` | int | 5 | max queue size of input/output topics |
+### Parameter Settings
-### Core Parameters
+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 (point cloud drops or delays) effectively.
+
+#### timeout_sec
+
+When network issues occur or when point clouds experience delays in the previous processing pipeline, some point clouds may be delayed or dropped. To address this, the `timeout_sec` parameter is used. Once the timer is created, it will start counting down from `timeout_sec`. If the timer reaches zero, the collector will not wait for delayed or dropped point clouds but will concatenate the remaining point clouds in the collector directly. The figure below demonstrates how `timeout_sec` works with `concatenate_and_time_sync_node` when `timeout_sec` is set to `0.12` (120 ms).
+
+
+
+#### lidar_timestamp_offsets
+
+Since different vehicles have varied designs for LiDAR scanning, the timestamps of each LiDAR may differ. Users need to know the offsets between each LiDAR and set the values in `lidar_timestamp_offsets`.
+
+To monitor the timestamps of each LiDAR, run the following command:
+
+```bash
+ros2 topic echo "pointcloud_topic" --field header
+```
+
+The timestamps should increase steadily by approximately 100 ms, as per the Autoware default. You should see output like this:
+
+```bash
+nanosec: 156260951
+nanosec: 257009560
+nanosec: 355444581
+```
+
+This pattern indicates a LiDAR timestamp of 0.05.
+
+If there are three LiDARs (left, right, top), and the timestamps for the left, right, and top point clouds are `0.01`, `0.05`, and `0.09` seconds respectively, the parameters should be set as [0.0, 0.04, 0.08]. This reflects the timestamp differences between the current point cloud and the point cloud with the earliest timestamp. Note that the order of the `lidar_timestamp_offsets` corresponds to the order of the `input_topics`.
+
+The figure below demonstrates how `lidar_timestamp_offsets` works with `concatenate_and_time_sync_node`.
+
+
+
+#### lidar_timestamp_noise_window
+
+Additionally, due to the mechanical design of LiDARs, there may be some jitter in the timestamps of each scan, as shown in the image below. For example, if the scan frequency is set to 10 Hz (scanning every 100 ms), the timestamps between each scan might not be exactly 100 ms apart. To handle this noise, the `lidar_timestamp_noise_window` parameter is provided.
+
+Users can use [this tool](https://github.com/tier4/timestamp_analyzer) to visualize the noise between each scan.
+
+
+
+From the example above, the noise ranges from 0 to 8 ms, so the user should set `lidar_timestamp_noise_window` to `0.008`.
-| Name | Type | Default Value | Description |
-| --------------------------------- | ---------------- | ------------- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
-| `timeout_sec` | double | 0.1 | tolerance of time to publish next pointcloud [s] When this time limit is exceeded, the filter concatenates and publishes pointcloud, even if not all the point clouds are subscribed. |
-| `input_offset` | vector of double | [] | This parameter can control waiting time for each input sensor pointcloud [s]. You must to set the same length of offsets with input pointclouds numbers. For its tuning, please see [actual usage page](#how-to-tuning-timeout_sec-and-input_offset). |
-| `publish_synchronized_pointcloud` | bool | false | If true, publish the time synchronized pointclouds. All input pointclouds are transformed and then re-published as message named `_synchronized`. |
-| `input_twist_topic_type` | std::string | twist | Topic type for twist. Currently support `twist` or `odom`. |
+The figure below demonstrates how `lidar_timestamp_noise_window` works with the `concatenate_and_time_sync_node`. If the green `X` is within the range of the red triangles, it indicates that the point cloud matches the reference timestamp of the collector.
-## Actual Usage
+
-For the example of actual usage of this node, please refer to the [preprocessor.launch.py](../launch/preprocessor.launch.py) file.
+## Launch
+
+```bash
+# The launch file will read the parameters from the concatenate_and_time_sync_node.param.yaml
+ros2 launch autoware_pointcloud_preprocessor concatenate_and_time_sync_node.launch.xml
+```
-### How to tuning timeout_sec and input_offset
+## Test
-The values in `timeout_sec` and `input_offset` are used in the timer_callback to control concatenation timings.
+```bash
+# build autoware_pointcloud_preprocessor
+colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-up-to autoware_pointcloud_preprocessor
-- Assumptions
- - when the timer runs out, we concatenate the pointclouds in the buffer
- - when the first pointcloud comes to buffer, we reset the timer to `timeout_sec`
- - when the second and later pointclouds comes to buffer, we reset the timer to `timeout_sec` - `input_offset`
- - we assume all lidar has same frequency
+# test autoware_pointcloud_preprocessor
+colcon test --packages-select autoware_pointcloud_preprocessor --event-handlers console_cohesion+
+```
-| Name | Description | How to tune |
-| -------------- | ---------------------------------------------------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
-| `timeout_sec` | timeout sec for default timer | To avoid mis-concatenation, at least this value must be shorter than sampling time. |
-| `input_offset` | timeout extension when a pointcloud comes to buffer. | The amount of waiting time will be `timeout_sec` - `input_offset`. So, you will need to set larger value for the last-coming pointcloud and smaller for fore-coming. |
+## Debug and Diagnostics
-### Node separation options for future
+To verify whether the node has successfully concatenated the point clouds, the user can examine rqt or the `/diagnostics` topic using the following command:
-Since the pointcloud concatenation has two process, "time synchronization" and "pointcloud concatenation", it is possible to separate these processes.
+```bash
+ros2 topic echo /diagnostics
+```
-In the future, Nodes will be completely separated in order to achieve node loosely coupled nature, but currently both nodes can be selected for backward compatibility ([See this PR](https://github.com/autowarefoundation/autoware.universe/pull/3312)).
+Below is an example output when the point clouds are concatenated successfully:
-## Assumptions / Known limits
+- Each point cloud has a value of `True`.
+- The `cloud_concatenation_success` is `True`.
+- The `level` value is `\0`. (diagnostic_msgs::msg::DiagnosticStatus::OK)
-It is necessary to assume that the vehicle odometry value exists, the sensor data and odometry timestamp are correct, and the TF from `base_link` to `sensor_frame` is also correct.
+```bash
+header:
+ stamp:
+ sec: 1722492015
+ nanosec: 848508777
+ frame_id: ''
+status:
+- level: "\0"
+ name: 'concatenate_and_time_sync_node: concat_status'
+ message: Concatenated pointcloud is published and include all topics
+ hardware_id: concatenate_data_checker
+ values:
+ - key: concatenated_cloud_timestamp
+ value: '1718260240.159229994'
+ - key: reference_timestamp_min
+ value: '1718260240.149230003'
+ - key: reference_timestamp_max
+ value: '1718260240.169229984'
+ - key: /sensing/lidar/left/pointcloud_before_sync/timestamp
+ value: '1718260240.159229994'
+ - 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/is_concatenated
+ value: 'True'
+ - key: /sensing/lidar/top/pointcloud_before_sync/timestamp
+ value: '1718260240.234578133'
+ - 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 `False`.
+- The `cloud_concatenation_success` is `False`.
+- The `level` value is `\x02`. (diagnostic_msgs::msg::DiagnosticStatus::ERROR)
+
+```bash
+header:
+ stamp:
+ sec: 1722492663
+ nanosec: 344942959
+ frame_id: ''
+status:
+- level: "\x02"
+ name: 'concatenate_and_time_sync_node: concat_status'
+ message: Concatenated pointcloud is published but miss some topics
+ hardware_id: concatenate_data_checker
+ values:
+ - key: concatenated_cloud_timestamp
+ value: '1718260240.859827995'
+ - key: reference_timestamp_min
+ value: '1718260240.849828005'
+ - key: reference_timestamp_max
+ value: '1718260240.869827986'
+ - key: /sensing/lidar/left/pointcloud_before_sync/timestamp
+ value: '1718260240.859827995'
+ - 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/is_concatenated
+ value: 'True'
+ - key: /sensing/lidar/top/pointcloud_before_sync/is_concatenated
+ value: 'False'
+ - key: cloud_concatenation_success
+ value: 'False'
+```
+
+## Node separation options
+
+There is also an option to separate the concatenate_and_time_sync_node into two nodes: one for `time synchronization` and another for `concatenate pointclouds` ([See this PR](https://github.com/autowarefoundation/autoware.universe/pull/3312)).
+
+Note that the `concatenate_pointclouds` and `time_synchronizer_nodelet` are using the [old design](https://github.com/autowarefoundation/autoware.universe/blob/9bb228fe5b7fa4c6edb47e4713c73489a02366e1/sensing/autoware_pointcloud_preprocessor/docs/concatenate-data.md) of the concatenate node.
+
+## 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 `matching_strategy.type` to `naive`.
diff --git a/sensing/autoware_pointcloud_preprocessor/docs/image/concatenate_algorithm.drawio.svg b/sensing/autoware_pointcloud_preprocessor/docs/image/concatenate_algorithm.drawio.svg
new file mode 100644
index 0000000000000..0ca825f5acaa6
--- /dev/null
+++ b/sensing/autoware_pointcloud_preprocessor/docs/image/concatenate_algorithm.drawio.svg
@@ -0,0 +1,810 @@
+
+
+
+
diff --git a/sensing/autoware_pointcloud_preprocessor/docs/image/concatenate_all.png b/sensing/autoware_pointcloud_preprocessor/docs/image/concatenate_all.png
new file mode 100644
index 0000000000000..2410df95a74c7
Binary files /dev/null and b/sensing/autoware_pointcloud_preprocessor/docs/image/concatenate_all.png differ
diff --git a/sensing/autoware_pointcloud_preprocessor/docs/image/concatenate_data.drawio.svg b/sensing/autoware_pointcloud_preprocessor/docs/image/concatenate_data.drawio.svg
deleted file mode 100644
index 786fee4c22ed7..0000000000000
--- a/sensing/autoware_pointcloud_preprocessor/docs/image/concatenate_data.drawio.svg
+++ /dev/null
@@ -1,298 +0,0 @@
-
-
-
-
diff --git a/sensing/autoware_pointcloud_preprocessor/docs/image/concatenate_edge_case.drawio.svg b/sensing/autoware_pointcloud_preprocessor/docs/image/concatenate_edge_case.drawio.svg
new file mode 100644
index 0000000000000..33835b6396e51
--- /dev/null
+++ b/sensing/autoware_pointcloud_preprocessor/docs/image/concatenate_edge_case.drawio.svg
@@ -0,0 +1,2361 @@
+
+
+
+
diff --git a/sensing/autoware_pointcloud_preprocessor/docs/image/concatenate_left.png b/sensing/autoware_pointcloud_preprocessor/docs/image/concatenate_left.png
new file mode 100644
index 0000000000000..87e78ab91fed6
Binary files /dev/null and b/sensing/autoware_pointcloud_preprocessor/docs/image/concatenate_left.png differ
diff --git a/sensing/autoware_pointcloud_preprocessor/docs/image/concatenate_right.png b/sensing/autoware_pointcloud_preprocessor/docs/image/concatenate_right.png
new file mode 100644
index 0000000000000..cd6d64eabd49a
Binary files /dev/null and b/sensing/autoware_pointcloud_preprocessor/docs/image/concatenate_right.png differ
diff --git a/sensing/autoware_pointcloud_preprocessor/docs/image/concatenate_top.png b/sensing/autoware_pointcloud_preprocessor/docs/image/concatenate_top.png
new file mode 100644
index 0000000000000..c317642bd9799
Binary files /dev/null and b/sensing/autoware_pointcloud_preprocessor/docs/image/concatenate_top.png differ
diff --git a/sensing/autoware_pointcloud_preprocessor/docs/image/ideal_timestamp_offset.drawio.svg b/sensing/autoware_pointcloud_preprocessor/docs/image/ideal_timestamp_offset.drawio.svg
new file mode 100644
index 0000000000000..0b8eca9dd75aa
--- /dev/null
+++ b/sensing/autoware_pointcloud_preprocessor/docs/image/ideal_timestamp_offset.drawio.svg
@@ -0,0 +1,784 @@
+
+
+
+
diff --git a/sensing/autoware_pointcloud_preprocessor/docs/image/jitter.png b/sensing/autoware_pointcloud_preprocessor/docs/image/jitter.png
new file mode 100644
index 0000000000000..c984e940171a6
Binary files /dev/null and b/sensing/autoware_pointcloud_preprocessor/docs/image/jitter.png differ
diff --git a/sensing/autoware_pointcloud_preprocessor/docs/image/noise_timestamp_offset.drawio.svg b/sensing/autoware_pointcloud_preprocessor/docs/image/noise_timestamp_offset.drawio.svg
new file mode 100644
index 0000000000000..bf5d604d2fb94
--- /dev/null
+++ b/sensing/autoware_pointcloud_preprocessor/docs/image/noise_timestamp_offset.drawio.svg
@@ -0,0 +1,2023 @@
+
+
+
+
diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/cloud_collector.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/cloud_collector.hpp
new file mode 100644
index 0000000000000..13604569df9a8
--- /dev/null
+++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/cloud_collector.hpp
@@ -0,0 +1,89 @@
+// 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 "combine_cloud_handler.hpp"
+
+#include
+#include
+#include
+
+namespace autoware::pointcloud_preprocessor
+{
+
+class PointCloudConcatenateDataSynchronizerComponent;
+class CombineCloudHandler;
+
+struct CollectorInfoBase
+{
+ virtual ~CollectorInfoBase() = default;
+};
+
+struct NaiveCollectorInfo : public CollectorInfoBase
+{
+ double timestamp;
+
+ explicit NaiveCollectorInfo(double timestamp = 0.0) : timestamp(timestamp) {}
+};
+
+struct AdvancedCollectorInfo : public CollectorInfoBase
+{
+ double timestamp;
+ double noise_window;
+
+ explicit AdvancedCollectorInfo(double timestamp = 0.0, double noise_window = 0.0)
+ : timestamp(timestamp), noise_window(noise_window)
+ {
+ }
+};
+
+class CloudCollector
+{
+public:
+ CloudCollector(
+ std::shared_ptr && ros2_parent_node,
+ std::shared_ptr & combine_cloud_handler, int num_of_clouds,
+ double timeout_sec, bool debug_mode);
+ bool topic_exists(const std::string & topic_name);
+ bool process_pointcloud(
+ const std::string & topic_name, sensor_msgs::msg::PointCloud2::SharedPtr cloud);
+ void concatenate_callback();
+
+ ConcatenatedCloudResult concatenate_pointclouds(
+ std::unordered_map topic_to_cloud_map);
+
+ std::unordered_map
+ get_topic_to_cloud_map();
+
+ [[nodiscard]] bool concatenate_finished() const;
+
+ void set_info(std::shared_ptr collector_info);
+ [[nodiscard]] std::shared_ptr get_info() const;
+ void show_debug_message();
+
+private:
+ std::shared_ptr ros2_parent_node_;
+ std::shared_ptr combine_cloud_handler_;
+ rclcpp::TimerBase::SharedPtr timer_;
+ std::unordered_map topic_to_cloud_map_;
+ uint64_t num_of_clouds_;
+ double timeout_sec_;
+ bool debug_mode_;
+ bool concatenate_finished_{false};
+ std::mutex concatenate_mutex_;
+ std::shared_ptr collector_info_;
+};
+
+} // namespace autoware::pointcloud_preprocessor
diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/collector_matching_strategy.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/collector_matching_strategy.hpp
new file mode 100644
index 0000000000000..c314b24a07921
--- /dev/null
+++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/collector_matching_strategy.hpp
@@ -0,0 +1,81 @@
+// 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
+
+#include
+
+#include
+#include
+#include
+#include
+#include
+#include
+
+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> match_cloud_to_collector(
+ const std::list> & cloud_collectors,
+ const MatchingParams & params) const = 0;
+ virtual void set_collector_info(
+ std::shared_ptr & collector, const MatchingParams & matching_params) = 0;
+};
+
+class NaiveMatchingStrategy : public CollectorMatchingStrategy
+{
+public:
+ explicit NaiveMatchingStrategy(rclcpp::Node & node);
+ [[nodiscard]] std::optional> match_cloud_to_collector(
+ const std::list> & cloud_collectors,
+ const MatchingParams & params) const override;
+ void set_collector_info(
+ std::shared_ptr & collector, const MatchingParams & matching_params) override;
+};
+
+class AdvancedMatchingStrategy : public CollectorMatchingStrategy
+{
+public:
+ explicit AdvancedMatchingStrategy(rclcpp::Node & node, std::vector input_topics);
+
+ [[nodiscard]] std::optional> match_cloud_to_collector(
+ const std::list> & cloud_collectors,
+ const MatchingParams & params) const override;
+ void set_collector_info(
+ std::shared_ptr & collector, const MatchingParams & matching_params) override;
+
+private:
+ std::unordered_map topic_to_offset_map_;
+ std::unordered_map topic_to_noise_window_map_;
+};
+
+std::shared_ptr parse_matching_strategy(rclcpp::Node & node);
+
+} // namespace autoware::pointcloud_preprocessor
diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/combine_cloud_handler.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/combine_cloud_handler.hpp
new file mode 100644
index 0000000000000..fa8c58aa74c11
--- /dev/null
+++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/combine_cloud_handler.hpp
@@ -0,0 +1,110 @@
+// 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
+#include
+#include
+#include
+#include
+#include
+
+// ROS includes
+#include "autoware/point_types/types.hpp"
+
+#include
+#include
+#include
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include
+#include
+#include
+#include
+#include
+
+namespace autoware::pointcloud_preprocessor
+{
+using autoware::point_types::PointXYZIRC;
+using point_cloud_msg_wrapper::PointCloud2Modifier;
+
+struct ConcatenatedCloudResult
+{
+ sensor_msgs::msg::PointCloud2::SharedPtr concatenate_cloud_ptr{nullptr};
+ std::optional>
+ topic_to_transformed_cloud_map;
+ std::unordered_map topic_to_original_stamp_map;
+};
+
+class CombineCloudHandler
+{
+private:
+ rclcpp::Node & node_;
+
+ std::string output_frame_;
+ bool is_motion_compensated_;
+ bool publish_synchronized_pointcloud_;
+ bool keep_input_frame_in_synchronized_pointcloud_;
+ std::unique_ptr managed_tf_buffer_{nullptr};
+
+ std::deque twist_queue_;
+
+ /// @brief RclcppTimeHash structure defines a custom hash function for the rclcpp::Time type by
+ /// using its nanoseconds representation as the hash value.
+ struct RclcppTimeHash
+ {
+ std::size_t operator()(const rclcpp::Time & t) const
+ {
+ return std::hash()(t.nanoseconds());
+ }
+ };
+
+ static void convert_to_xyzirc_cloud(
+ const sensor_msgs::msg::PointCloud2::SharedPtr & input_cloud,
+ sensor_msgs::msg::PointCloud2::SharedPtr & xyzirc_cloud);
+
+ void correct_pointcloud_motion(
+ const std::shared_ptr & transformed_cloud_ptr,
+ const std::vector & pc_stamps,
+ std::unordered_map & transform_memo,
+ std::shared_ptr transformed_delay_compensated_cloud_ptr);
+
+public:
+ CombineCloudHandler(
+ rclcpp::Node & node, std::string output_frame, bool is_motion_compensated,
+ bool publish_synchronized_pointcloud, bool keep_input_frame_in_synchronized_pointcloud,
+ bool has_static_tf_only);
+ void process_twist(
+ const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr & twist_msg);
+ void process_odometry(const nav_msgs::msg::Odometry::ConstSharedPtr & odometry_msg);
+
+ ConcatenatedCloudResult combine_pointclouds(
+ std::unordered_map & topic_to_cloud_map);
+
+ Eigen::Matrix4f compute_transform_to_adjust_for_old_timestamp(
+ const rclcpp::Time & old_stamp, const rclcpp::Time & new_stamp);
+
+ std::deque get_twist_queue();
+};
+
+} // namespace autoware::pointcloud_preprocessor
diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_node.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_node.hpp
new file mode 100644
index 0000000000000..654593e317ad9
--- /dev/null
+++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_node.hpp
@@ -0,0 +1,124 @@
+// Copyright 2023 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
+#include
+#include
+#include
+#include
+#include
+
+// ROS includes
+#include "cloud_collector.hpp"
+#include "collector_matching_strategy.hpp"
+#include "combine_cloud_handler.hpp"
+
+#include
+#include
+#include
+#include
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include
+#include
+#include
+#include
+#include
+
+namespace autoware::pointcloud_preprocessor
+{
+class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node
+{
+public:
+ explicit PointCloudConcatenateDataSynchronizerComponent(const rclcpp::NodeOptions & node_options);
+ ~PointCloudConcatenateDataSynchronizerComponent() override = default;
+ void publish_clouds(
+ ConcatenatedCloudResult && concatenated_cloud_result,
+ std::shared_ptr collector_info);
+ void manage_collector_list();
+ std::list> get_cloud_collectors();
+ void add_cloud_collector(const std::shared_ptr & collector);
+
+private:
+ struct Parameters
+ {
+ bool use_naive_approach;
+ bool debug_mode;
+ bool has_static_tf_only;
+ double rosbag_length;
+ int maximum_queue_size;
+ double timeout_sec;
+ bool is_motion_compensated;
+ bool publish_synchronized_pointcloud;
+ bool keep_input_frame_in_synchronized_pointcloud;
+ bool publish_previous_but_late_pointcloud;
+ std::string synchronized_pointcloud_postfix;
+ std::string input_twist_topic_type;
+ std::vector input_topics;
+ std::string output_frame;
+ std::string matching_strategy;
+ } params_;
+
+ double current_concatenate_cloud_timestamp_{0.0};
+ double latest_concatenate_cloud_timestamp_{0.0};
+ bool drop_previous_but_late_pointcloud_{false};
+ bool publish_pointcloud_{false};
+ bool is_concatenated_cloud_empty_{false};
+ std::shared_ptr diagnostic_collector_info_;
+ std::unordered_map diagnostic_topic_to_original_stamp_map_;
+
+ std::shared_ptr combine_cloud_handler_;
+ std::list> cloud_collectors_;
+ std::unique_ptr collector_matching_strategy_;
+ std::mutex cloud_collectors_mutex_;
+
+ // default postfix name for synchronized pointcloud
+ static constexpr const char * default_sync_topic_postfix = "_synchronized";
+
+ // subscribers
+ std::vector::SharedPtr> pointcloud_subs_;
+ rclcpp::Subscription::SharedPtr twist_sub_;
+ rclcpp::Subscription::SharedPtr odom_sub_;
+
+ // publishers
+ rclcpp::Publisher::SharedPtr concatenated_cloud_publisher_;
+ std::unordered_map::SharedPtr>
+ topic_to_transformed_cloud_publisher_map_;
+ std::unique_ptr debug_publisher_;
+
+ std::unique_ptr> stop_watch_ptr_;
+ diagnostic_updater::Updater diagnostic_updater_{this};
+
+ void cloud_callback(
+ const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, const std::string & topic_name);
+ void twist_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr input);
+ void odom_callback(const nav_msgs::msg::Odometry::ConstSharedPtr input);
+
+ static std::string format_timestamp(double timestamp);
+ void check_concat_status(diagnostic_updater::DiagnosticStatusWrapper & stat);
+ std::string replace_sync_topic_name_postfix(
+ const std::string & original_topic_name, const std::string & postfix);
+};
+
+} // namespace autoware::pointcloud_preprocessor
diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp
deleted file mode 100644
index 6164e85c35717..0000000000000
--- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp
+++ /dev/null
@@ -1,197 +0,0 @@
-// Copyright 2023 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.
-
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2009, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Willow Garage, Inc. nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- * $Id: concatenate_data.cpp 35231 2011-01-14 05:33:20Z rusu $
- *
- */
-
-#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_AND_TIME_SYNC_NODELET_HPP_
-#define AUTOWARE__POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_AND_TIME_SYNC_NODELET_HPP_
-
-#include
-#include