feat(ndt_scan_matcher): warn if the lidar scan has gone out of the range of the loaded map

[anhnv3991]

Description

• Display a warning message when the initial pose of NDT has gone out of the map range. Basically, warn users when the map should be updated but it has not been updated yet.

Tests performed

• XX1 Odaiba PCD map and rosbag
• In launcher/autoware_launch/config/localization/ndt_scan_matcher/ndt_scan_matcher.param.yaml, lower the dynamic map loading radius to closer to lidar_radius (about 2 meters bigger)
• Launch logging simulator
• The terminal of the logging simulator should display something like this
  ... [ndt_scan_matcher_node-39] [WARN] [1718934187.199986673] [localization.pose_estimator.ndt_scan_matcher]: Lidar has gone out of the map range ...

Effects on system behavior

None

Interface changes

None

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

Project coverage is 28.94%. Comparing base (a22e9c4) to head (b81fd72).
Report is 4 commits behind head on main.

Files	Patch %	Lines
...ization/ndt_scan_matcher/src/map_update_module.cpp	16.66%	10 Missing ⚠️
...ion/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp	0.00%	6 Missing ⚠️

Additional details and impacted files

@@           Coverage Diff           @@
##             main    #7612   +/-   ##
=======================================
  Coverage   28.93%   28.94%           
=======================================
  Files        1605     1606    +1     
  Lines      117491   117464   -27     
  Branches    50490    50463   -27     
=======================================
- Hits        34000    33995    -5     
- Misses      74282    74295   +13     
+ Partials     9209     9174   -35     

Flag	Coverage Δ		*Carryforward flag
differential	22.72% <11.11%> (?)
total	28.93% <ø> (-0.01%)	⬇️	Carriedforward from c79a96f

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

[KYabuuchi]

@anhnv3991
The return value of should_update_map indicates "whether the map should be updated", not "whether the vehicle has exited the map boundary". Therefore, it is incorrect to output the WARN message when the function returns true.

The check for whether the vehicle is outside the map boundary is already performed internally within should_update_map, and an ERROR diagnostic is issued if a violation occurs. Additionally, since this function is called periodically in the timer callback, I think there is no need to check this in the sensor points callback as well.

The potential issue arises when waiting for a service response from map_loader. This waiting occurs within the timer_callback of MapUpdateModule, during which the conventional should_update_map is not called.

So, could you modify the implementation to check that the vehicle does not go out of the map boundary while waiting for the service response ?

[anhnv3991]

The potential issue arises when waiting for a service response from map_loader. This waiting occurs within the timer_callback of MapUpdateModule, during which the conventional should_update_map is not called.

@KYabuuchi Why don't we just call a check in the main callback sensor point (of ndt_scan_matcher_core.cpp)? Basically the check by callback timer is performed periodically (e.g. every 200ms if ros2 bag play --clock 200). Hence, within that period (200ms) there is no check, and the vehicle could move outside of the map range if it travels fast enough.

Checking from map_update_module while waiting for service is quite complicated, since map_update_module does not have access to the current position of NDT. Creating a shared pointer for the poses in such cases is unnecessary imo.

[KYabuuchi]

@anhnv3991 --clock 200 does not mean that the timer callback is called every 200ms. It specifies the frequency at which the /clock topic is published from the rosbag, so --clock 200 means /clock is published at 200Hz.
And, the MapUpdateModule's timer callback runs at 1Hz. source code link

From my understanding, NDT runs should_update_map in a separate thread from callback_sensor_points_main because:

• When updating the map, it needs to wait for the map_loader service in a separate thread anyway.
• Checking at a frequency of 1Hz seems to be sufficient. (On the other hand, there is no upper limit to the delay of the map_loader response.)
• Concentrating MapUpdateModule on map management makes the code easier to understand.

---

Checking from map_update_module while waiting for service is quite complicated, since map_update_module does not have access to the current position of NDT.

Indeed, what you're saying is correct. I didn't anticipate this difficulty... 🤔

[anhnv3991]

@KYabuuchi I see. I just checked the should_update_map again. You're right, should_update_map returns true does not mean the lidar has gone out of the map range. The if clause inside that function is what we need for checking.

So how about this? We can define a method of map_update_module to check the map range, and just let the callback_sensor_points call that method on NDT poses.

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

[anhnv3991]

@KYabuuchi Just a question: To change parameters of ndt_scan_matcher, I found 2 files

src/launcher/autoware_launch/config/localization/ndt_scan_matcher/ndt_scan_matcher.param.yaml 
src/universe/autoware.universe/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml

which have the same content. Which file do I have to edit to change parameters of ndt_scan_matcher? I thought colcon --symlink-install should link those files but it seems like it does not.

[KYabuuchi]

@anhnv3991 When using ros2 launch autoware_launch, the parameter files loaded are in src/launch/autoware_launch/*.
If you want to experiment with your implementation, you should edit the files in src/launch/autoware_launch/*.

These files contain the same content, and you can consider the parameter files in autoware.universe/* as sample parameter files.

[KYabuuchi]

Sorry for the delayed response regarding this.
Basically, I think this PR will provide the desired functionality. 👍 But, there is a risk of data races between the sensor points callback and the timer callback, so we need to be careful.

[SakodaShintaro]

I'm going to check this pull request now, but have all of Yabuuchi-san's comments been resolved yet?

[anhnv3991]

I'm going to check this pull request now, but have all of Yabuuchi-san's comments been resolved yet?

@SakodaShintaro Yes, I think so.

[SakodaShintaro]

Looks Good To Me
@KYabuuchi
Are your all comments already resolved?
I think it would also be good to have your approval before merging.

[KYabuuchi]

Sorry for the delayed review. All of my concerns have been addressed.
Looks Good To Me 🙆‍♂️