Incorrect transform direction used when calculating gnss_base_link in package gnss_poser

[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

In gnss poser, gnss_base_link is found by using the gnss data and transform between gnss to base-link. The following transforms are expected to be multiplied, respectively. gnss_pose * gnss_to_base_link. However, in the package, the multiplication is done as gnss_pose * base_link_to_gnss.

Here the function to make transforms is called. And this line contains transform from base_link to gnss using lookup transform. But I think the exact source and target frame should be swapped. Source frame should be gnss target frame base_link

Expected behavior

When we give the orientation and position of the gnss to the sensor_kit, we expect the gnss_base_link to appear in the correct rotation

Actual behavior

We were able to obtain the correct result when we gave inverse while giving the rotation of gnss according to sensor_kit_base_link in sensor_kit.

Steps to reproduce

Since roll pitch yaw values for gnss in sample sensor kit are 0 0 0, we do not encounter this error in autoware demos.

Versions

No response

Possible causes

No response

Additional context

No response

[YamatoAndo]

@meliketanrikulu Thank you for the report!
First of all, the comments and the code are not matching, which makes it difficult to understand.
Would you please make the necessary corrections?

[VRichardJP]

I guess they are refering to this snippet:

autoware.universe/sensing/gnss_poser/src/gnss_poser_core.cpp

Lines 116 to 124 in f945c01

	auto tf_gnss_antenna2base_link_msg_ptr = std::make_shared<geometry_msgs::msg::TransformStamped>();
	getStaticTransform(
	gnss_frame_, base_frame_, tf_gnss_antenna2base_link_msg_ptr, nav_sat_fix_msg_ptr->header.stamp);
	tf2::Transform tf_gnss_antenna2base_link{};
	tf2::fromMsg(tf_gnss_antenna2base_link_msg_ptr->transform, tf_gnss_antenna2base_link);
	// transform pose from gnss_antenna(in map frame) to base_link(in map frame)
	tf2::Transform tf_map2base_link{};
	tf_map2base_link = tf_map2gnss_antenna * tf_gnss_antenna2base_link;

tf_gnss_antenna2base_link_msg_ptr names implies "gnss_link -> base_link" transform and is used that way, but getStaticTransform(gnss_frame_, base_frame_, ... returns the transform "base_link -> gnss_link"?

[meliketanrikulu]

I guess they are refering to this snippet:

autoware.universe/sensing/gnss_poser/src/gnss_poser_core.cpp

Lines 116 to 124 in f945c01

	auto tf_gnss_antenna2base_link_msg_ptr = std::make_shared<geometry_msgs::msg::TransformStamped>();
	getStaticTransform(
	gnss_frame_, base_frame_, tf_gnss_antenna2base_link_msg_ptr, nav_sat_fix_msg_ptr->header.stamp);
	tf2::Transform tf_gnss_antenna2base_link{};
	tf2::fromMsg(tf_gnss_antenna2base_link_msg_ptr->transform, tf_gnss_antenna2base_link);
	// transform pose from gnss_antenna(in map frame) to base_link(in map frame)
	tf2::Transform tf_map2base_link{};
	tf_map2base_link = tf_map2gnss_antenna * tf_gnss_antenna2base_link;

tf_gnss_antenna2base_link_msg_ptr names implies "gnss_link -> base_link" transform and is used that way, but getStaticTransform(gnss_frame_, base_frame_, ... returns the transform "base_link -> gnss_link"?

Yes . Thank you for explaining @VRichardJP . The function returns base_link -> gnss_link transform. But I think it should be the other way ( gnss -> base_link ) around

[meliketanrikulu]

@YamatoAndo could you check this issue

[stale]

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

[YamatoAndo]

@meliketanrikulu May I close this issue?

[meliketanrikulu]

Of course. Sorry i forgot to close issue

[FranzAlbers]

Hello @YamatoAndo, @kminoda, @meliketanrikulu,

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

We noticed this bug when our RTK-DGNSS-equipped vehicle recently started having a height offset of around 80 cm between the gnss_base_link from the gnss_poser and our point cloud and lanelet2 maps, while both the precision of the RTK-DGNSS and our maps should be at most a few centimeters. We noted the same height offset and a longitudinal offset of around 60 cm towards the NDT localization pose (ndt_base_link). Before #5033, the RTK-DGNSS pose perfectly fitted the map and approximately matched the NDT/EKF pose.

This can be seen in the image below. The map->base_link transformation from the gnss_poser is used. Our Navsatfix is given in the adma/imu_link frame and transformed in the wrong direction with PR #5033, resulting in a floating vehicle.

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. (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. This was the case before PR # 5033, so I propose to revert it.

[idorobotics]

Reopened after a request from @FranzAlbers in #6392.

[YamatoAndo]

Thank you for the bug report!

I have tested with the autoware tutorial's rosbag.

With #5033

header:
  stamp:
    sec: 1585897264
    nanosec: 30981
  frame_id: gnss_link
status:
  status: 0
  service: 15
latitude: 35.6183154
longitude: 139.7806291
altitude: 40.513
position_covariance:
- 3.575881
- 0.0
- 0.0
- 0.0
- 3.575881
- 0.0
- 0.0
- 0.0
- 10.439361
position_covariance_type: 2
---

$ ros2 topic echo /sensing/gnss/pose
header:
  stamp:
    sec: 1585897264
    nanosec: 30981
  frame_id: map
pose:
  position:
    x: 89571.96773586844
    y: 42299.807555724685
    z: 42.3145225433175
  orientation:
    x: -0.0003634119436112312
    y: 0.007507785887620079
    z: -0.01819473179251916
    w: 0.9998062076311705
---

$ ros2 run tf2_ros tf2_echo gnss_link base_link
At time 0.0
- Translation: [-0.769, -0.031, -1.813]
- Rotation: in Quaternion [0.000, -0.008, 0.018, 1.000]
- Rotation: in RPY (radian) [0.000, -0.015, 0.036]
- Rotation: in RPY (degree) [0.026, -0.861, 2.085]
- Matrix:
  0.999 -0.036 -0.015 -0.769
  0.036  0.999 -0.001 -0.031
  0.015  0.000  1.000 -1.813
  0.000  0.000  0.000  1.000

Without #5033

$ ros2 topic echo /sensing/gnss/ublox/nav_sat_fix 
header:
  stamp:
    sec: 1585897268
    nanosec: 28543
  frame_id: gnss_link
status:
  status: 0
  service: 15
latitude: 35.6183152
longitude: 139.7806335
altitude: 40.874
position_covariance:
- 3.485689
- 0.0
- 0.0
- 0.0
- 3.485689
- 0.0
- 0.0
- 0.0
- 10.208025
position_covariance_type: 2
---

$ ros2 topic echo /sensing/gnss/pose
header:
  stamp:
    sec: 1585897268
    nanosec: 28543
  frame_id: map
pose:
  position:
    x: 89570.79957770585
    y: 42299.74614522535
    z: 39.06070269805477
  orientation:
    x: 0.0003634119436112312
    y: -0.007507785887620079
    z: 0.01819473179251916
    w: 0.9998062076311705
---

$ ros2 run tf2_ros tf2_echo gnss_link base_link
At time 0.0
- Translation: [-0.769, -0.031, -1.813]
- Rotation: in Quaternion [0.000, -0.008, 0.018, 1.000]
- Rotation: in RPY (radian) [0.000, -0.015, 0.036]
- Rotation: in RPY (degree) [0.026, -0.861, 2.085]
- Matrix:
  0.999 -0.036 -0.015 -0.769
  0.036  0.999 -0.001 -0.031
  0.015  0.000  1.000 -1.813
  0.000  0.000  0.000  1.000

If we focus on the height values, the gnss_link should be 1.813 lower than the base_link,
so it can be said that the transformation without this PR is correct.

[meliketanrikulu]

Hello, I've been out of the office for a while. I'm sorry for the late reply . When I checked as you said, I saw that there was a change that was not correct. Since GNSS/INS sensor data was received on base_link in our vehicle setup, I could not observe this error before. Thank you for reporting the bug. @FranzAlbers . Thanks for the PR and correction. @YamatoAndo . I think we can close this issue.