How to convert uorb topic "vehicle_odometry" into nav_msgs/msg/odometry

I am trying to convert uorb topic “vehicle_odometry” into “nav_msgs/msg/odometry” in order to make transformation of the robot and feed the /odom ros topic to the slam_toolbox.

I found that “vehicle_odometry” is using NED (North, East, Down) for localization and (Forward, Right, Down) for FRD body frame standard.

However, “nav_msgs/msg/odometry” is using ENU (East, North, Up) for localization and (Forward, Left, Up) for body standard.

It makes the convertion complicated, especially for the conversion of
quaternion rotation.

In mavlink, it said the quaternion rotation of “vehicle_odometry” is x, y, z and w.
For “nav_msgs/msg/odometry”, quaternion is “orientation x”, “orientation y”, “orientation z” and w (rotation).

However, the meaning of orientation y and z are difference between 2 messages. Even I can make y and z negative to do the conversion, I think it will still fail because the w (rotation) will change if the definition of “orientation y” and “orientation z” are different. and I have no idea how to make a correct w (rotation) by mysefl according to the orientation x, y and z I have.

Did someone get the “nav_msgs/msg/odometry” message from pixhawk before? How can we publish a correct “nav_msgs/msg/odometry” messages using the data provided by pixhawk?

I am using holybro pixhawk 6c and subscribe to the uorb topic using micrortps and ros2 on the jetson Xavier NX.

All I need is only to publish /tf and /odom for slam_toolbox.

You can use px4_ros_com/frame_transforms.h at main · PX4/px4_ros_com · GitHub
It contains a lot of functions to perform frame conversions.

1 Like

I have read frame_transforms.cpp.
I found that all the cases for “NED_TO_ENU” are empty.
I remember that “vehicle_odometry” is using NED and “nav_msgs/msg/Odometry” is using ENU.

Is it normal for “NED_TO_ENU” not to do anything? Are the code for frame_transforms.h and frame_transforms.cpp completed?

They are not empty. Or to be more precise, they do not have the “break” statement. This means that the case after them is executed, which is ENU_TO_NED.
And it is correct, as ENU_TO_NED and NED_TO_ENU are represented with the same mathematical transformation.

1 Like

Thank you so much. It really helps a lot.

I’m currently in the process of trying to use the functions of “frame_transforms.cpp” but even then, the orientations don’t seem to match up.

Testing with a unit quaternion, which corresponds to the origin of my ROS2 odometry message:

auto q_enu = Eigen::Quaterniond(1, 0, 0, 0);
auto q_ned = transform_orientation(q_enu, StaticTF::ENU_TO_NED);

Gives me the orientation:

0, 0.707107, 0.707107, 0

However, this doesn’t match the orientation of the vehicle_odometry message from uORB:

q: [0.6956, 0.0056, -0.0068, 0.7184]

I’ve been at this for days now with no answer, and help would be much appreciated, thanks.