Pose initialization error

[meliketanrikulu]

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

There was an issue about this problem before, but the package structure has changed quite a bit.You can reach the previously studied issue here. Before this issue was resolved, there was a part we added, the pose_initializer package was sending the first pose to ekf for 1 second. When this issue was resolved in autoware, we tested it on our own computers and the problem seemed to be resolved. However, we did not remove this small solution that we applied on the vehicle. When we updated everything in accordance with the structure in autoware, we encountered this problem again.

We are getting the exact same error. gnss_poser is sending the correct pose to the pose_initializer package. NDT also takes this right pose and searches at the right spot and creates the first pose correctly here. This pose is sent to EKF as the first pose via the pose_initilizer package. However, EKF produces the wrong pose. The pose produced by EKF is either formed at the zero point of the map or somewhere between the zero point and the correct point obtained from gnss_poser.

Expected behavior

We expect EKF to localize correctly when autoware is started.

Actual behavior

When Autoware starts, gnss_poser sends the correct initial location and NDT calculates a correct initial location using that location. Then EKF takes this pose as the first pose via pose_initilizer, but cannot calculate its position correctly. The pose produced by EKF is either formed at the zero point of the map or somewhere between the zero point and the correct point obtained from gnss_poser.

video1
video2

Steps to reproduce

If you want to reproduce:

1. bag files
2. sensor launch
3. vehicle launch
4. map files

Versions

No response

Possible causes

No response

Additional context

No response

[kminoda]

Thank you for the report! Let me confirm several points:

1. So do you still apply your previous solution (= the pose_initializer package was sending the first pose to ekf for 1 second)?
2. Which version of Autoware do you use? Is it the latest one?

We will also investigate the cause of this problem when we have time. But also feel free to post the cause or any solution here, if you find any (it will definitely be a big contribution for this OSS 😄 ) Here are some debug tips for this type of problem:

• The main characters here are mainly ndt_scan_matcher and ekf_localizer, which both communicate mutually. Inspecting the topics between two nodes may be the first step (e.g. checking when origin points are published by using ros2 topic echo).
• Checking activation/deactivation timing of pose_initializer may help, which is recently introduced with this PR.

[meliketanrikulu]

Thank you for the report! Let me confirm several points:

1. So do you still apply your previous solution (= the pose_initializer package was sending the first pose to ekf for 1 second)?
2. Which version of Autoware do you use? Is it the latest one?

We will also investigate the cause of this problem when we have time. But also feel free to post the cause or any solution here, if you find any (it will definitely be a big contribution for this OSS smile ) Here are some debug tips for this type of problem:

• The main characters here are mainly ndt_scan_matcher and ekf_localizer, which both communicate mutually. Inspecting the topics between two nodes may be the first step (e.g. checking when origin points are published by using ros2 topic echo).
• Checking activation/deactivation timing of pose_initializer may help, which is recently introduced with this PR.

Thank you for your comment @kminoda . I cheched all pipeline. There was a problem with ndt scan matcher package. I just changed this line. ndt old position is zero at the beginning, so for the beginning, the condition that this ndt old pose is empty is checked. However, here it is checked whether the new pose ndt is empty. I just replaced this with ndt old pose. This was causing the poses to return zero at the beginning from within the interpolator function here. Problem solved I will create PR.

[kminoda]

@meliketanrikulu Thank you very much! I will check the PR.

[meliketanrikulu]

Related PR: #2660

[kminoda]

This issue is solved, right? @meliketanrikulu
Please close this issue if so.