gnss_poser: Incorrect transform direction between NavSatFix frame and gnss_base_link

[FranzAlbers]

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

Hello @YamatoAndo, @kminoda, @meliketanrikulu,
first of all, thanks for working on this awesome project!

I would like to reopen issue #3640 because I think the associated PR #5033 introduced the problem it claimed to solve. The direction of the transform between the NavSatFix Frame and gnss_base_link was correct before and incorrect after the PR.

We noticed this bug when our RTK-DGNSS-equipped vehicle recently started having a height and a longitudinal offset between the gnss_base_link published by the gnss_poser and our point cloud and lanelet2 maps, while both the RTK-DGNSS and our maps provide ground-truth quality with at most a few centimeters offset. Before PR #5033, the RTK-DGNSS pose perfectly fitted the map and approximately matched the NDT/EKF pose.

Expected behavior

Our NavSatFix is given in the adma/imu_link frame, NOT in the adma/gnss_link.

The image below shows the expected behavior (PR #5033 reverted): gnss_base_link is transformed from adma/imu_link in the correct direction and coincides with the base_link / ndt_base_link frame.

Actual behavior

The image below shows the current behavior with gnss_base_link being transformed from the adma/imu_link in the wrong direction exactly by the transformation from base_link to the adma/imu_link. Note, that map->base_link is estimated by the NDT/EKF localization here to show the issue more clearly (unlike in the left image in #3640 (comment) where map->base_link is estimated by the gnss_poser, resulting in a floating vehicle).

Steps to reproduce

I've tried to reproduce this with the autoware sample rosbag and the logging simulator, but the gnss_poser pose from the NavSatFix was located around 45 meters above the base_link, so I think this is not precise enough for seeing differences of less than 1m in the pose.

Versions

• OS: Ubuntu 22.04
• ROS2 Humble
• Autoware container: https://github.com/autowarefoundation/autoware/pkgs/container/autoware-universe/173838572?tag=humble-20240201-cuda-amd64

Possible causes

The source_frame and target_frame in lookupTransform() are often confused, because the transformation of data between the frames and the transformation of the frames themselves are inverse. If the same source_frame and target_frame are used with lookupTransform and tf2_echo, we obtain inverse transformations (ros/geometry#108 (comment)).

In this case, we intend to use the transform between the frames, so we should use the inverse of what lookupTransform gives us, since lookupTransform returns the transformation for the data, not the frame itself (as tf2_echo does). This was the case before PR #5033, so I propose to revert it (though I agree that the corrected comment in that PR was correct).

Additional context

No response

[idorobotics]

The issue #3640 is re-opened as requested. Feel free to link your PR after discussing your solution in the comments. To avoid duplicates, I'll close this issue.