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

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 72.17195% with 123 lines in your changes missing coverage. Please review.

Project coverage is 29.80%. Comparing base (86ba21f) to head (3a24c26).
Report is 1 commits behind head on main.

Files with missing lines	Patch %	Lines
...oncatenate_data/concatenate_and_time_sync_node.cpp	73.00%	28 Missing and 26 partials ⚠️
...processor/src/concatenate_data/cloud_collector.cpp	55.35%	21 Missing and 4 partials ⚠️
...sor/src/concatenate_data/combine_cloud_handler.cpp	83.80%	11 Missing and 12 partials ⚠️
...c/concatenate_data/collector_matching_strategy.cpp	48.78%	17 Missing and 4 partials ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #8300      +/-   ##
==========================================
- Coverage   29.88%   29.80%   -0.08%     
==========================================
  Files        1440     1448       +8     
  Lines      108347   108822     +475     
  Branches    42467    41624     -843     
==========================================
+ Hits        32376    32437      +61     
- Misses      72730    73227     +497     
+ Partials     3241     3158      -83     

Flag	Coverage Δ		*Carryforward flag
differential	27.28% <72.17%> (?)
total	29.61% <ø> (-0.27%)	⬇️	Carriedforward from 5a10484

*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

[vividf]

@drwnz @mojomex
The naive approach is implemented in commit aa89a1f.

Result of unsynchronized LiDARs

[mojomex]

As the naive and "strict" approach are mutually exclusive, the code should be structured that only the parameters/variables of the enabled one exist.

Doing this leads to:

• no unused parameters in the config --> users will make less config mistakes
• less documentation needed
• no god object --> easier to debug, less chance of bugs, more maintainable

Instead of having all parameters in a flat hierarchy and documenting which of them are ignored when, you can make parameters tree-structured instead:

In the case of the naive approach:

# common parameters
debug_mode: false
[...]
approach:
  type: naive
  # this approach has no other parameters

In the case of the strict approach:

# common parameters
debug_mode: false
[...]
approach:
  type: strict
  lidar_timestamp_offsets: [...]
  lidar_timestamp_noise_window: [...]

In ROS2, each hierarchy level becomes a . in the parameter name --> approach.type and so on.

When parsing these, you can use polymorphism

class SchemaError : public std::exception {
  const char * what() const noexcept override {
    return "JSON schema violation";
  }
};

class Approach {
  virtual std::optional<CloudCollector> match_cloud_to_collector(...) = 0;
};

struct ApproachNaive : public Approach {
  std::optional<CloudCollector> match_cloud_to_collector(...) override;
};

struct ApproachStrict : public Approach {
  std::vector<double> lidar_timestamp_offsets;
  std::vector<double> lidar_timestamp_noise_window;

  std::optional<CloudCollector> match_cloud_to_collector(...) override;
};

std::shared_ptr<Approach> parse_approach(rclcpp::Node & node) {
  auto type = node.declare_parameter<std::string>("approach.type");
  if (type == "naive") {
    return std::make_shared<ApproachNaive>();
  }
  if (type == "strict") {
    return std::make_shared<ApproachStrict>(
        node.declare_parameter<std::vector<double>>("approach.lidar_timestamp_offsets"),
        node.declare_parameter<std::vector<double>>("approach.lidar_timestamp_noise_window")
    );
  }

  throw SchemaError{};
}

to declare only the needed parameters. This is only demonstration code, I'm sure there are nicer ways to write this.

From then, instead of if/else in multiple places, you can use polymorphism so that the calling code can just call apporach.my_method without having to know which approach is used.

[vividf]

@mojomex
Thanks for your advice and detailed explanation!
They are fixed in fb9bebc