fix(ndt_scan_matcher): fixed a lock scope in update_ndt

[SakodaShintaro]

Description

In this pull request, the data race for ndt_ptr_ has been fixed.

Context

In the traditional code, within the update_ndt() of the map_update_module, a copy of the ndt_ptr_ was obtained and changes were applied to this copy then it was written back.

However, the class NormalDistributionsTransform, which was copied, internally contains a shared_ptr as a member variable. As a result, a simple copy resulted in a shallow copy.

Consequently, when the map update of NDT and the pose estimation by NDT occurred simultaneously, it led to data race issues.

Changes

Because the shared_ptr is deeply used in the NormalDistributionsTransform, implementing a deep copy is difficult.

So the lock scope has been expanded. As a result, it is no longer necessary to make a copy of ndt_ptr_, and updates are now applied directly to the original object. Converting the type of the additional points is a heavy process, so this is now pre-calculated outside of the lock scope.

Tests performed

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

Process time

Since the lock scope is larger than previously thought, the time to stop pose estimation by ndt will also be longer.

The time taken to update the map was measured.

Environment : ThinkPad X1 Extreme Gen 4 (11th Gen Intel(R) Core(TM) i7-11800H @ 2.30GHz)

The first time is adding a new map, so it takes a lot of time to ndt_ptr_->addTarget.

From the second time, a differential update is done, and typically, createVoxelKdtree is the time-consuming part. If there are many points to add, the ndt_ptr_->addTarget also takes time.

In the future, it may be better to recreate NormalDistributionsTransform to enable deep copy.

endurance test

I ran logging_simulator 170 times for about 12 hours and confirmed that ndt_scan_matcher did not crash even once.

Effects on system behavior

The issue of the node potentially crashing during dynamic map loading is resolved.

However, pose estimation by ndt has a slight delay when the map is differential updated.

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: 9 lines in your changes are missing coverage. Please review.

Comparison is base (83d316c) 15.28% compared to head (1c16bfd) 15.28%.

Files	Patch %	Lines
...ization/ndt_scan_matcher/src/map_update_module.cpp	0.00%	9 Missing ⚠️

Additional details and impacted files

@@           Coverage Diff           @@
##             main    #5951   +/-   ##
=======================================
  Coverage   15.28%   15.28%           
=======================================
  Files        1750     1750           
  Lines      120435   120435           
  Branches    36731    36731           
=======================================
  Hits        18405    18405           
  Misses      81378    81378           
  Partials    20652    20652           

Flag	Coverage Δ		*Carryforward flag
differential	3.07% <0.00%> (?)
total	15.28% <ø> (+<0.01%)	⬆️	Carriedforward from 83d316c

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

[SakodaShintaro]

I ran logging_simulator 170 times for about 12 hours and confirmed that ndt_scan_matcher did not crash even once.

@YamatoAndo
Could you review this pull request?