feat(autoware_pointcloud_preprocessor): redesign concatenate and time sync node

[vividf]

Description

This PR solved the issue #6832.
Previous designs have some issues concatenating the pointcloud correctly, therefore, this PR redesigns the logic of the concatenate node in order to handle the edge cases like pointcloud delay or pointcloud drop.

Changes

• new algorithm
• diagnostic message
• parameter file, launch file, schema
• unit test: test the logic of the cloud_collector and combine_cloud_handler
• component test: testing edge cases (pointcloud dropped, delayed)

A more detailed description of the algorithm is on the Readme page.https://github.com/vividf/autoware.universe/blob/feature/redesign_concatenate_and_time_sync_node/sensing/autoware_pointcloud_preprocessor/docs/concatenate-data.md

Related links

Parent Issue:

• Link

How was this PR tested?

Unit test and Component test for concatenate node

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

Tested with sample rosbag

Please change the sample_sensor_kit_launch: autowarefoundation/sample_sensor_kit_launch#108

# Terminal 1
ros2 launch autoware_launch logging_simulator.launch.xml map_path:=$HOME/autoware_map/sample-map-rosbag vehicle_model:=sample_vehicle sensor_model:=sample_sensor_kit

# Terminal 2
ros2 bag play sample-rosbag_0.db3

The result provided by @mojomex below #8300 (comment)

Tested with vehicle 1

data: TIER4_INTERNAL_LINK

modify the config file concatenate_and_time_sync_node.param.yaml

/**:
  ros__parameters:
    debug_mode: false
    has_static_tf_only: false
    rosbag_length: 20.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/left/pointcloud_before_sync",
                    "/sensing/lidar/right/pointcloud_before_sync",
                    "/sensing/lidar/top/pointcloud_before_sync"
                ]
    output_frame: base_link
    matching_strategy:
      type: advanced
      lidar_timestamp_offsets: [0.0, 0.035, 0.073]
      lidar_timestamp_noise_window: [0.01, 0.01, 0.01]

# Terminal 1
ros2 launch autoware_pointcloud_preprocessor concatenate_and_time_sync_node.launch.xml

# Terminal 2
ros2 bag play rosbag2_2024_07_11-17_54_04_0.db3

Note

• This bag can test the scenario when the pointclouds are dropped.
• Set the rosbag_replay parameter to True, if user is replaying the rosbag without relaunching the node.

Tested with vehicle 2

data: TIER4_INTERNAL_LINK
modify the config file concatenate_and_time_sync_node.param.yaml

/**:
  ros__parameters:
      debug_mode: false
      has_static_tf_only: false
      rosbag_length: 60.0
      maximum_queue_size: 5
      timeout_sec: 0.2
      is_motion_compensated: false
      publish_synchronized_pointcloud: false
      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/left_lower/pointcloud",
                    "/sensing/lidar/left_upper/pointcloud",
                    "/sensing/lidar/front_lower/pointcloud",
                    "/sensing/lidar/front_upper/pointcloud",
                    "/sensing/lidar/right_upper/pointcloud",
                    "/sensing/lidar/right_lower/pointcloud",
                    "/sensing/lidar/rear_lower/pointcloud",
                    "/sensing/lidar/rear_upper/pointcloud"
                  ]
      output_frame: base_link
      matching_strategy:
        type: advanced
        lidar_timestamp_offsets: [0.0, 0.0, 0.025, 0.028, 0.026, 0.05, 0.075, 0.076]
        lidar_timestamp_noise_window: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01]

# Terminal 1
ros2 launch autoware_pointcloud_preprocessor concatenate_and_time_sync_node.launch.xml

# Terminal 2
ros2 bag play rosbag2_2024_10_23-12_21_48_0.db3

Result

TIER4_INTERNAL_LINK

Time comparison (vehicle 1 bag)

From last arrived pointcloud to publish concatenate pointcloud (include publishing)

Before (move the toc to the beginning of the cloudcallback function)

	Minimum	Maximum	Average
Time	6.82	90.986	13.70

Note that the huge latency is because a pointcloud is dropped.

After

	Minimum	Maximum	Average
Time	0.82	73.004	11.25

By setting is_motion_compensated to false

	Minimum	Maximum	Average
Time	0.69	70.614	9.16

Notes for reviewers

locking logic (mutex) might be an important part of double-checking :)

Interface changes

None.

Effects on system behavior

None.

[github-actions]

Thank you for contributing to the Autoware project!

🚧 If your pull request is in progress, switch it to draft mode.

Please ensure:

• You've checked our contribution guidelines.
• Your PR follows our pull request guidelines.
• All required CI checks pass before marking the PR ready for review.

[codecov]

Codecov Report

Attention: Patch coverage is 71.11111% with 130 lines in your changes missing coverage. Please review.

Project coverage is 30.10%. Comparing base (7f4ed3d) to head (137766f).
Report is 21 commits behind head on main.

Files with missing lines	Patch %	Lines
...oncatenate_data/concatenate_and_time_sync_node.cpp	73.26%	28 Missing and 26 partials ⚠️
...processor/src/concatenate_data/cloud_collector.cpp	51.78%	23 Missing and 4 partials ⚠️
...c/concatenate_data/collector_matching_strategy.cpp	43.47%	21 Missing and 5 partials ⚠️
...sor/src/concatenate_data/combine_cloud_handler.cpp	83.68%	11 Missing and 12 partials ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #8300      +/-   ##
==========================================
+ Coverage   29.69%   30.10%   +0.40%     
==========================================
  Files        1452     1454       +2     
  Lines      108962   107888    -1074     
  Branches    42584    42179     -405     
==========================================
+ Hits        32353    32476     +123     
+ Misses      73496    72204    -1292     
- Partials     3113     3208      +95     

Flag	Coverage Δ		*Carryforward flag
differential	26.50% <70.66%> (?)
differential-cuda	26.52% <71.11%> (?)
total	29.92% <ø> (+0.23%)	⬆️	Carriedforward from ccd5aab

*This pull request uses carry forward flags. Click here to find out more.

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

[vividf]

@kminoda
This PR includes the unit test and component test for the concatenate node.
Would be nice if you could check the component test (especially for the empty pointcloud case that caused the problem before)

[kminoda]

@vividf Thanks. Let me do that later

[github-actions]

Documentation URL: https://autowarefoundation.github.io/autoware.universe/pr-8300/
Modified URLs:

• https://autowarefoundation.github.io/autoware.universe/pr-8300/sensing/autoware_pointcloud_preprocessor/concatenate-data/

[mojomex]

Thanks for the changes!

There's still a bit of unused state in the cloud_collector, but refactoring so it references the chosen strategy should be quite quick.

Also, I left some small fixes for documentation rendering. see docs here or run locally via:

cd src/universe/autoware.universe
mkdocs serve

For running locally, you might also need to do this setup first.

[mojomex]

@vividf Sorry to keep you waiting after the last review 🙇

🟢 Code LGTM!
🟢 Tests LGTM!
🟢 Docs LGTM!
🟢 Evaluation on sample sensor kit LGTM!

Evaluation of newly added naive / advanced mode

The videos below show the top (green), left (orange), right (blue) pointcloud_before_sync and concatenated (white) pointclouds.

The bottom plot shows the header.stamp.nsec of each pointcloud, and the middle right plot shows for each input pointcloud, whether is has been concatenated (green) or not (purple).
Diagnostics output by the concat node are shown in the top right panel.

Evaluation of naive mode:

naive-small.mp4

Evaluation of advanced mode:

advanced-small.mp4

[mojomex]

@YoshiRi @yukkysaito @drwnz @badai-nguyen My review has concluded, could one of you code owners take a look and if OK approve as well? Thank you 🙇