Ros2 Heading and Yaw Orientation Do Not Match Ros1

[JordanSlater]

Describe the bug Ros2 needs corrections from master branch for GPS heading and yaw orientation covariance.

To Reproduce Steps to reproduce the behavior:

1. Checkout master branch and run with ublox zed f9p hardware.
2. Checkout ros2 branch and run same setup.
3. You'll get different readings for yaw orientation covariance between master and ros2 branches.

Expected behavior I'd expect the same readings between the ros2 and ros1 drivers.

Desktop (please complete the following information):

• OS: Ubuntu 20.04
• ROS Version ros2 humble and ros1 noetic and foxy.

Additional context Looks like a couple corrections in the master branch have not made it into the ros2 branch. Heading: Ros1: https://github.com/KumarRobotics/ublox/blame/4f107f3b82135160a1aca3ef0689fd119199bbef/ublox_gps/src/node.cpp#L1803 Ros2: https://github.com/KumarRobotics/ublox/blame/ros2/ublox_gps/src/hp_pos_rec_product.cpp#L58

Yaw Orientation Covariance: Ros1: https://github.com/KumarRobotics/ublox/blame/4f107f3b82135160a1aca3ef0689fd119199bbef/ublox_gps/src/node.cpp#L1816 Ros2: https://github.com/KumarRobotics/ublox/blame/ros2/ublox_gps/src/hp_pos_rec_product.cpp#L70

[clalancette]

Would you mind opening a pull request with those changes? It would also be helpful to reference the PRs that put those changes into the ROS 1 branch for comparison. Thanks.

[tomlogan501]

For the heading, after some tests, I got this formula up and running : -((static_cast<double>(m.rel_pos_heading) * 1e-5 / 180.0 * M_PI) + M_PI_2); Can you confirm it ?

[JordanSlater]

I'm sorry but I am unable to confirm with the software/hardware any longer and I do not have experience with NED and ENU conversions.