concatenate_and_time_synchronize_node

Purpose

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.

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:

Left	Top	Right

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	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

Core Parameters

Name	Type	Description	Default	Range
debug_mode	boolean	Flag to enables debug mode to display additional logging information.	False	N/A
has_static_tf_only	boolean	Flag to indicate if only static TF is used.	False	N/A
rosbag_length	float	This value determine if the rosbag has started from the beginning again. The value should be set smaller than the actual length of the bag.	10.0	≥0.0
maximum_queue_size	integer	Maximum size of the queue for the Keep Last policy in QoS history.	5	≥1
timeout_sec	float	Timer's timeout duration in seconds when collectors are created. Collectors will concatenate the point clouds when timeout_sec reaches zero.	0.1	≥0.001
is_motion_compensated	boolean	Flag to indicate if motion compensation is enabled.	True	N/A
publish_synchronized_pointcloud	boolean	Flag to indicate if synchronized point cloud should be published.	True	N/A
keep_input_frame_in_synchronized_pointcloud	boolean	Flag to indicate if input frame should be kept in synchronized point cloud.	True	N/A
publish_previous_but_late_pointcloud	boolean	Flag to indicate if a concatenated point cloud should be published if its timestamp is earlier than the previous published concatenated point cloud.	False	N/A
synchronized_pointcloud_postfix	string	Postfix for the topic name of the synchronized point cloud.	pointcloud	N/A
input_twist_topic_type	string	Type of the input twist topic.	twist	['twist', 'odom']
input_topics	array	List of input point cloud topics.	['cloud_topic1', 'cloud_topic2', 'cloud_topic3']	N/A
output_frame	string	Output frame.	base_link	N/A
matching_strategy.type	string	Set it to advanced if you can synchronize your LiDAR sensor; otherwise, set it to naive.	advanced	['naive', 'advanced']
matching_strategy.lidar_timestamp_offsets	array	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.	[0.0, 0.0, 0.0]	N/A
matching_strategy.lidar_timestamp_noise_window	array	List of LiDAR timestamp noise windows in seconds. The noise values should be specified in the same order as the input_topics.	[0.01, 0.01, 0.01]	N/A

Parameter Settings

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:

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:

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 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.

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.

Launch

# 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

Test

# build autoware_pointcloud_preprocessor
colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-up-to autoware_pointcloud_preprocessor

# test autoware_pointcloud_preprocessor
colcon test --packages-select autoware_pointcloud_preprocessor --event-handlers console_cohesion+

Debug and Diagnostics

To verify whether the node has successfully concatenated the point clouds, the user can examine rqt or the /diagnostics topic using the following command:

ros2 topic echo /diagnostics

Below is an example output when the point clouds are concatenated successfully:

• Each point cloud has a value of True.
• The cloud concatenation success is True.
• The level value is \0. (diagnostic_msgs::msg::DiagnosticStatus::OK)

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)

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).

Note that the concatenate_pointclouds and time_synchronizer_nodelet are using the old design 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.