Pose initialization has become slow with dynamic map loading

[VRichardJP]

Checklist

• [X] I've read the contribution guidelines.
• [X] I've searched other issues and no duplicate issues were found.
• [X] I'm convinced that this is not my fault but a bug.

Description

When I use the planning simulator with default parameters (i.e. with dynamic map loading) and a 250MB PCD map, pose initialization with Rviz takes around 6-7 seconds:

Screencast from 01-31-2024 01_27_45 PM.webm

When I use the same setup but with the dynamic map loading disabled (see below), pose initialization is done instantly:

Screencast from 01-31-2024 01_30_36 PM.webm

• autoware_launch/config/map/pointcloud_map_loader.param.yaml:
  • enable_partial_load: false
• autoware_launch/config/perception/object_recognition/detection/pointcloud_map_filter.param.yaml:
  • use_dynamic_map_loading: False

I guess what happens is the PCD map is reloaded every single time the pose is initialized, despite being the exact same map. AFAIK, this only impacts pose initialization so it is not a huge issue at the moment, but I think current approach may impact other modules as well.

One simple way to improve this would be to split the map loading into 2 queries:

1. a query to get the chunk ID at given position, or all chunk IDs within given radius. This one should be fairly fast
2. a query to retrieve chunks by ID.

In such case, nodes using dynamic map loading feature would simply need to track which chunk ID is currently stored in their own cache.

Expected behavior

No delay on pose initialization

Actual behavior

4-5 seconds latency

Steps to reproduce

planning simulator with "big" PCD map

Versions

No response

Possible causes

No response

Additional context

No response

[isamu-takagi]

This is because the whole map is transferred when correcting the Z coordinate. The pose from rviz is not accurate in height, so the initial_pose_adaptor uses map_height_fitter to fix it.

Since dynamic_map_loading feature sends point clouds in file units, it is necessary to split the point cloud file in advance when using this function. Or I don't know if it's possible, but I think it can be solved by implementing a function in the map_loader that automatically splits a single file.

[stale[bot]]

This pull request has been automatically marked as stale because it has not had recent activity.

[Motsu-san]

@VRichardJP Can this issue be closed? If not, please reply to @isamu-takagi 's comment.

[VRichardJP]

@isamu-takagi @Motsu-san If the point cloud is split into chunks, would map height fitter only fetch the most relevant chunk?

The PCD I used happened to be very dense. Down sampling the map reduced the service time significantly, but it still remained slower than the previous design.

[VRichardJP]

Thinking back at my cache idea: if both the map loader and height fitting nodes are deployed in the some container, I think it should be possible to write the code in a way the map could be accessed by the height fitter node using intra process (so no copy or in the worse case 1 copy when the message is created, which shouldn't take long)

[isamu-takagi]

If the point cloud is split into chunks, would map height fitter only fetch the most relevant chunk?

Yes. When the map is splitted according to this prerequisites, only the nearest chunks will be requested.

Thinking back at my cache idea: if both the map loader and height fitting nodes are deployed in the some container, I think it should be possible to write the code in a way the map could be accessed by the height fitter node using intra process (so no copy or in the worse case 1 copy when the message is created, which shouldn't take long)

Since the service does not seem to support intra-process communication, I don't think the processing time will change if dynamic loading is enabled. The most efficient way is for the map_loader to have the function of the map_height_fitter, but I'm not sure if it's a good idea from the perspective of dividing packages.

[stale[bot]]

This pull request has been automatically marked as stale because it has not had recent activity.