perf(map_height_fitter): more efficient query to find closest lanelet

[maxime-clem]

Description

Improve performance of the map_height_fitter when getting the ground height.
Currently, it checks ALL lanelets to find the closest one. With this PR, the query is more efficient (thanks to the R-tree used in the lanelet layer).

Tests performed

Not applicable.

Effects on system behavior

Not applicable.

Interface changes

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.

[SakodaShintaro]

@maxime-clem
Thank you very much!
Sorry, I implemented it inefficiently due to my ignorance of lanelet modules.

I would like to ask an additional question that is not related to the pull request.
Is it reasonable to use centerline() from a retrieved item to get the z-coordinate?
I don't know what a centerline is, and I'm thinking that if this could be a very long lanelet element along the way, then there might be a slight probability that it is far from the query point.

[maxime-clem]

Is it reasonable to use centerline() from a retrieved item to get the z-coordinate?

Thank you for checking the PR. It sounds like it would be better to retrieve the closest point. Which is also possible. I can use nearest(lanelet::BasicPoint2d{x, y}, 1); on the vector_map_->pointLayer instead of the vector_map_->laneletLayer. If that sounds okay I will update the PR.

[SakodaShintaro]

I think that change is fine. 👍
I apologize for the inconvenience, but please make the change.

[maxime-clem]

@SakodaShintaro I committed the change. Are you able to test the new version and confirm it works as expected ?

[SakodaShintaro]

I conducted the following three types of tests.

• planning_simulator (initializing pose by vectormap)
• logging_simulator (initializing pose by both pcd and vectormap)
• AWSIM (initializing pose by both pcd and vectormap)

Everything worked fine. Thank you very much.