Localization issue while driving uphill

Kim-mins commented 3 months ago

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

Hi team,

I'm currently running Autoware with Carla using bridge implementation, and I found a situation that the localization gets bad while driving uphill, and it eventually makes the ego vehicle stop.

Here's the rviz video and launch.log file: [video] [launch.log]

At the start of the video, the poincloud matches with the given .pcd file, but it gets worse as the vehicle goes up, and the vehicle eventually stops and could not reach to the goal point. Also, the trace(skyblue line) in rviz shows that there are oscillation on the baselink.

And according to the log file, there are logs that sayingNVTL gets worse.

...
1721137556.6314461 [ndt_scan_matcher_node-42] [WARN] [1721137556.630878637] [localization.pose_estimator.ndt_scan_matcher]: Score is below the threshold. Score: 2.23286, Threshold: 2.3
...
1721137561.2945189 [ndt_scan_matcher_node-42] [WARN] [1721137561.293876881] [localization.pose_estimator.ndt_scan_matcher]: Score is below the threshold. Score: 1.28923, Threshold: 2.3
...

Expected behavior

I hope the ego vehicle to drive to the destination well.

Actual behavior

But it could not reach to the given goal point.

Steps to reproduce

Here's the ros2bag file for the reproduction: [ros2bag]

Versions

autoware.universe: 7877192cb5700c1db15109dcd959785fc381951d
Carla: 0.9.15

Possible causes

No response

Additional context

No response

KYabuuchi commented 3 months ago

I checked your rosbag in my environment. I found your point cloud map is missing some buildings, which seems to be causing the scan matching to not work correctly.

Therefore, I believe this issue will be resolved if you fix the point cloud map.

KYabuuchi commented 3 months ago

On the other hand, I have successfully prevented the scan matcher from failing on hills by adjusting the following parameters, without updating the map.

autoware_launch/config/localization/ekf_localizer.param.yaml
z_filter_proc_dev: 1.0
z_filter_proc_dev: 3.0 roll_filter_proc_dev: 0.01
pitch_filter_proc_dev: 0.01
pitch_filter_proc_dev: 0.1


* `autoware_launch/config/localization/ndt_scan_matcher/pointcloud_preprocessor/crop_box_filter_measurement_range.param.yaml` 
```diff
-    min_x: -60.0
+    min_x: -30.0
     max_x: 60.0
-    min_y: -60.0
-    max_y: 60.0
+    min_y: -30.0
+    max_y: 30.0

This change is intended to cut off the point cloud and prevent the virtual building and hill from being observed simultaneously.

https://github.com/user-attachments/assets/0a4994ae-bf3c-4140-b386-375726f416a5

It is still unstable but I think we can improve it more by adjusting the parameters.

Kim-mins commented 3 months ago

Thank you for your detailed investigation @KYabuuchi!

I did not notice that my .pcd file misses some pointclouds. Thank you for pointing out.

I agree to your modification works well, but I got one small question for this issue.

Question: I think using the default parameters for ndt scan matcher is encouraged, and seems I should change the parameters if I want to resolve the issue. If I don't change the parameters, should I fill the missed part of the .pcd file?

I really appreciate to your help! Thanks!

KYabuuchi commented 2 months ago

@Kim-mins I don't think using the default parameters of the NDT scan matcher is necessarily encouraged. It's expected that parameters will be adjusted according to the environment.

If you don't change the parameters, you'll need to fix the pcd file. Alternatively, avoiding unmapped areas should prevent the issue from occurring.

Kim-mins commented 2 months ago

Thank you for the clear answers @KYabuuchi!

Then it seems better for me to change the parameters.. Thanks a lot!

The issue seems gone now, so can I close this issue?

KYabuuchi commented 2 months ago

@Kim-mins Yes. Please close this issue :ok_hand:

autowarefoundation / autoware.universe