Behavior Planner Time Consumption Due to side_shift Module

[StepTurtle]

Checklist

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

Description

When running Autoware with a large-scale map and providing a long route, there is a noticeable increase in time consumption for the side_shift module. This results in a gradual decrease in planner speed to 1 or 2 Hz as the vehicle progresses.

We have no idea whether the problem will also occur on small maps because we have not figured out how to use small maps to test this situation.

Expected behavior

The side_shift module should operate efficiently with large-scale maps and long routes, ensuring that the planner maintains a consistent speed without significant degradation over time.

Actual behavior

Currently, the side_shift module exhibits increased time consumption with large-scale maps and long routes, leading to a gradual decrease in planner speed as the vehicle progresses.

planning_low_hz_side_shift_module.webm

Steps to reproduce

1.Load a large-scale map into Autoware.
2.Plan a long route for the vehicle.
3.Run Autoware and observe the performance of the side_shift module over time.

Tip

• It could take couple of hours
• You can open the 'debug' display on Rviz to see planner manager status

Versions

• OS: 22.04
• ROS 2: Humble
• Autoware: main branch

Possible causes

No response

Additional context

No response

[kyoichi-sugahara]

@StepTurtle
Thank you so much for the report and we need to investigate the reason for this problem later.
As a tip, in case this is a critical issue and you need to address it, you can try setting the following parameter to false. This will prevent the side_shift module from launching.
https://github.com/autowarefoundation/autoware_launch/blob/efae4831cff8ee29e0007923871f49a54ccce0a3/autoware_launch/config/planning/preset/default_preset.yaml/#L33-L35

[maxime-clem]

I have not been able to reproduce the issue with my dummy large map.
@StepTurtle are you able to share your map ? I can then try to reproduce and fix the issue.

[StepTurtle]

@maxime-clem Here is the map I used: https://drive.google.com/drive/folders/1ZNJBis-fVaAaleumWh-BxEFdnJn2QpBH?usp=drive_link

Also here is the commands to declare initial pose and goal pose, if you don't want to wait lanelet2 loading:

ros2 topic pub /initialpose geometry_msgs/msg/PoseWithCovarianceStamped "{header: {stamp: {sec: 0, nanosec: 0}, frame_id: 'map'}, pose: {pose: {position: {x:  -24303.66918760375, y: -4395.511373508722, z: 1.0}, orientation: {x: 0.0, y: 0.0, z: 0.6087614, w: 0.7933533}}, covariance: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, ]}}"

ros2 topic pub /planning/mission_planning/goal geometry_msgs/msg/PoseStamped "{header: {stamp: {sec: 0, nanosec: 0}, frame_id: 'map'}, pose: {position: {x: 36105.062779498985 , y: 8357.336685524322, z: 1.0}, orientation: {x: 0.0, y: 0.0, z: 0.6087614, w: 0.7933533}}}"

[meliketanrikulu]

Hello @maxime-clem . You were able to test this. Are there any extra requirements to reproduce it?

[maxime-clem]

@meliketanrikulu I was able to reproduce this performance issue using the map shared above.

1. I start Autoware with the simple_planning_simulator using the test map.
2. I set the initial pose and goal as written in the above comment.
3. I then enable the Debug group in the Rviz displays to show the "planner manager status" text.
4. I then update the ego pose to be around the middle of the route, for example with the following command:

ros2 topic pub /initialpose geometry_msgs/msg/PoseWithCovarianceStamped "{header: {stamp: {sec: 0, nanosec: 0}, frame_id: 'map'}, pose: {pose: {position: {x:  6995.7236328125, y: 1872.29248046875, z: 1.0}, orientation: {x: -0.006263831242349847, y: 0.011237356867520937, z: 0.4868408788328398, w: 0.8733959267858235}}, covariance: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, ]}}" --once

5. Finally I observe the processing time of the side_shift module increasing to 350~450ms.