feat(ndt_scan_matcher): added a function to check maximum distance of sensor points

[SakodaShintaro]

Description

Context

The current ndt_scan_matcher does not check the normality of the input point cloud, and performs scan matching even when an abnormal point cloud comes in, leading to incorrect position estimation.

In particular, the pull requests below have been merged to make the default input point cloud topic /sensing/lidar/concatenated/pointcloud, which rarely outputs point clouds concatenated with short-range LiDAR only.

• feat(tier4_localization_launch): change the default input pointcloud of localization into the concatenated pointcloud #6528

ndt_scan_matcher should check the validity of the input point cloud.

Changes

As a first step in checking the input point cloud, this PR checks to see if there are points are more than a specified distance apart.

Note

This PR must be merged with

• feat(ndt_scan_matcher): added a parameter of sensor points autoware_launch#908

Tests performed

It has been confirmed that the logging_simulator runs with the same accuracy as before on AWSIM data with GT.

sample rosbag

The probability of incorrect initial position estimation in Sample-rosbag performed under poor computational resources has been reduced from 3/7 to nearly 0/7.

Effects on system behavior

The possibility of ndt being incorrect is slightly reduced.

Pre-review checklist for the PR author

The PR author must check the checkboxes below when creating the PR.

• I've confirmed the contribution guidelines.
• The PR follows the pull request guidelines.

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

• The PR follows the pull request guidelines.

Post-review checklist for the PR author

The PR author must check the checkboxes below before merging.

• There are no open discussions or they are tracked via tickets.

After all checkboxes are checked, anyone who has write access can merge the PR.

[codecov]

Codecov Report

Attention: Patch coverage is 0% with 9 lines in your changes are missing coverage. Please review.

Project coverage is 14.78%. Comparing base (e1a3139) to head (f0a974c).
Report is 16 commits behind head on main.

Files	Patch %	Lines
...ion/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp	0.00%	7 Missing ⚠️
...cher/include/ndt_scan_matcher/hyper_parameters.hpp	0.00%	0 Missing and 2 partials ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #6559      +/-   ##
==========================================
- Coverage   14.78%   14.78%   -0.01%     
==========================================
  Files        1917     1917              
  Lines      132040   132076      +36     
  Branches    39238    39264      +26     
==========================================
  Hits        19524    19524              
- Misses      90721    90754      +33     
- Partials    21795    21798       +3     

Flag	Coverage Δ		*Carryforward flag
differential	3.60% <0.00%> (?)
total	14.78% <ø> (ø)		Carriedforward from 998e11c

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

[TaikiYamada4]

@SakodaShintaro
Almost perfect to me!
I made a situation that the concatenation doesn't work well by making timeout_sec to 0.01 [s] from 0.1 [s] in the concatenate_data node, and the implementation in this PR worked well.

Apart for that, could you add the unit [m] to the warning message for clarity?

[anhnv3991]

@SakodaShintaro It is just my opinion but should we create a new node to perform this check on input clouds; or can we move this function (cloud validation) to a node that performs pre-processing steps on clouds? Some reasons for this:

• First, we can run scan checking and scan matching concurrently (align a scan while checking some others)
• Second, we maintain the modularity of the system; i.e. ndt_scan_matching only performs scan matching (not validate input clouds), pre-processing node only validates clouds (not doing anything else)
• Third, if there are other nodes that use these clouds, they can subscribe to this new node (that do the check and publish valid clouds) to get the validated clouds, and do not have to re-check the clouds.

[SakodaShintaro]

@anhnv3991
Thank you for your comment :)

Although this is an important point, it seems like it will take some time to fix it, so I would like to try merging this PR as is.

The current autoware.universe is degraded, which initial position estimation fails with sample-rosbag with a probability of about 50%, so I would like to fix this as soon as possible.

However, since good design is of course important, I will address your points in detail.

(1) Concurrency
You are correct, but in this code, the computational complexity of checking sensor points is O(N). In NDT, N is usually around 1500, so it is not significant. Even in the future, the checking code may not become a heavy process.

(2) Modularity
I agree with you. How about creating a new class in the future, such as SensorPointsValidator and having NDTScanMatcher hold it as a member variable? Just as NDT's align processing is essentially implemented with ndt_omp, it may be desirable to create a dedicated class for checking input point clouds. I think NDTScanMatcher's responsibility is communication in ROS2. In any case, I agree that core logic should be written as little as possible in NDTScanMatcher.

(3) Reusability
I think the validation of input is different for each process. For example, points only from a short-range LiDAR may not be suitable for NDT but could be useful for other processes. In general, the requirements for inputs should be defined for each process.

[TaikiYamada4]

LGTM