Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(autoware_pointcloud_preprocessor): redesign concatenate and time sync node #8300

Open
wants to merge 107 commits into
base: main
Choose a base branch
from

Conversation

vividf
Copy link
Contributor

@vividf vividf commented Aug 1, 2024

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_replay: 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
    lidar_timestamp_offsets: [0.0, 0.04, 0.08]
    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_replay: true
      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
      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

image-20241023-030032-20241023-055053

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

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

After

Minimum Maximum Average
Time 0.82 73.004 11.25

after

By setting is_motion_compensated to false

Minimum Maximum Average
Time 0.69 70.614 9.16

image-20240730-070949

Notes for reviewers

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

Interface changes

None.

Effects on system behavior

None.

@vividf vividf self-assigned this Aug 1, 2024
@github-actions github-actions bot added type:documentation Creating or refining documentation. (auto-assigned) component:sensing Data acquisition from sensors, drivers, preprocessing. (auto-assigned) labels Aug 1, 2024
Copy link

github-actions bot commented Aug 1, 2024

Thank you for contributing to the Autoware project!

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

Please ensure:

@vividf vividf changed the title Feature/redesign concatenate and time sync node feature(autoware_pointcloud_preprocessor): redesign concatenate and time sync node Aug 1, 2024
@vividf vividf added the tag:run-build-and-test-differential Mark to enable build-and-test-differential workflow. (used-by-ci) label Aug 1, 2024
@vividf vividf changed the title feature(autoware_pointcloud_preprocessor): redesign concatenate and time sync node feat(autoware_pointcloud_preprocessor): redesign concatenate and time sync node Aug 1, 2024
Copy link

codecov bot commented Aug 1, 2024

Codecov Report

Attention: Patch coverage is 75.57252% with 96 lines in your changes missing coverage. Please review.

Project coverage is 29.67%. Comparing base (41c5956) to head (972758f).
Report is 2 commits behind head on main.

Files with missing lines Patch % Lines
...oncatenate_data/concatenate_and_time_sync_node.cpp 71.92% 31 Missing and 26 partials ⚠️
...sor/src/concatenate_data/combine_cloud_handler.cpp 83.91% 11 Missing and 12 partials ⚠️
...processor/src/concatenate_data/cloud_collector.cpp 64.44% 14 Missing and 2 partials ⚠️
Additional details and impacted files
@@            Coverage Diff             @@
##             main    #8300      +/-   ##
==========================================
+ Coverage   29.49%   29.67%   +0.18%     
==========================================
  Files        1443     1449       +6     
  Lines      108648   109065     +417     
  Branches    41537    41754     +217     
==========================================
+ Hits        32044    32368     +324     
- Misses      73484    73535      +51     
- Partials     3120     3162      +42     
Flag Coverage Δ *Carryforward flag
differential 27.21% <75.57%> (?)
total 29.49% <ø> (-0.01%) ⬇️ Carriedforward from 7b20619

*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
Copy link
Contributor Author

vividf commented Aug 2, 2024

@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
Copy link
Contributor

kminoda commented Aug 8, 2024

@vividf Thanks. Let me do that later

@drwnz
Copy link
Contributor

drwnz commented Oct 28, 2024

@vividf about the sample rosbag timestamps, @mojomex found an issue in Nebula that might have been causing that.
Can you please try it again? @mojomex this one, right?
tier4/nebula#201

@mojomex
Copy link
Contributor

mojomex commented Oct 29, 2024

@drwnz @vividf
I tried again with the current Autoware repo (Nebula v0.2.1) and modified src/sensor_kit/sample_sensor_kit_launch/sample_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py to use the following parameters:

"input_topics": [
    "/sensing/lidar/right/pointcloud_before_sync",
    "/sensing/lidar/top/pointcloud_before_sync",
    "/sensing/lidar/left/pointcloud_before_sync",
],
"output_frame": LaunchConfiguration("base_frame"),
"input_twist_topic_type": "twist",
"publish_synchronized_pointcloud": True,
"debug_mode": False,
"has_static_tf_only": False,
"rosbag_replay": False,
"rosbag_length": 20.0,
"maximum_queue_size": 5,
"timeout_sec": 0.2,
"is_motion_compensated": True,
"keep_input_frame_in_synchronized_pointcloud": True,
"publish_previous_but_late_pointcloud": False,
"synchronized_pointcloud_postfix": "pointcloud",
"lidar_timestamp_offsets": [0.0, 0.015, 0.016],
"lidar_timestamp_noise_window": [0.01, 0.01, 0.01],

Here's the result, pointclouds of the same time span are concatted correctly and no concatenation was missed from what I can tell:

concat.mp4

@vividf
Copy link
Contributor Author

vividf commented Oct 29, 2024

@vividf
Copy link
Contributor Author

vividf commented Nov 8, 2024

@YoshiRi @technolojin @badai-nguyen
This PR modifies the algorithm and parameters and removes the default parameters.

I noticed that ground_segmentation.launch.py uses the concatenate algorithm within the ground segmentation process. If the perception needs to use concatenation after ground segmentation, please understand this PR and specify the parameters for the cloud you wish to concatenate.

Based on our previous discussion, the parameter configuration will likely look something like this:

/**:
  ros__parameters:
      debug_mode: false
      has_static_tf_only: false
      rosbag_replay: 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: [
                      "/concatenated_pointcloud",
                      "/left_lower/synchronized_pointcloud",
                  ]
      output_frame: base_link
      lidar_timestamp_offsets: [0.0, 0.0]
      lidar_timestamp_noise_window: [0.01, 0.01]

@vividf
Copy link
Contributor Author

vividf commented Nov 11, 2024

Issue that needs to be solved after this PR is merged.

@vividf vividf mentioned this pull request Nov 13, 2024
3 tasks
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
component:sensing Data acquisition from sensors, drivers, preprocessing. (auto-assigned) tag:require-cuda-build-and-test tag:run-build-and-test-differential Mark to enable build-and-test-differential workflow. (used-by-ci) type:documentation Creating or refining documentation. (auto-assigned)
Projects
Status: No status
Development

Successfully merging this pull request may close these issues.

5 participants