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

1

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.

2

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
4

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?

5

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
6

Thank you so much. It really helps a lot.

7

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.

8

Here is my conversion.

      Eigen::Quaterniond q = px4_ros_com::frame_transforms::utils::quaternion::array_to_eigen_quat(msg->q);
      Eigen::Quaterniond enu_q = px4_ros_com::frame_transforms::ned_to_enu_orientation(q);
      Eigen::Vector3d position = Eigen::Vector3d(msg->x, msg->y, msg->z);
      Eigen::Vector3d enu_position = px4_ros_com::frame_transforms::ned_to_enu_local_frame(position);
      Eigen::Vector3d velocity = Eigen::Vector3d(msg->vx, msg->vy, msg->vz);
      Eigen::Vector3d enu_velocity = px4_ros_com::frame_transforms::ned_to_enu_local_frame(velocity);
      publish_data.pose.pose.position.x = enu_position.x();
      publish_data.pose.pose.position.y = enu_position.y();
      publish_data.pose.pose.position.z = enu_position.z();
      publish_data.pose.pose.orientation.x = (float)enu_q.x();
      publish_data.pose.pose.orientation.y = (float)enu_q.y();
      publish_data.pose.pose.orientation.z = (float)enu_q.z();
      publish_data.pose.pose.orientation.w = (float)enu_q.w();
      publish_data.twist.twist.linear.x = enu_velocity.x();
      publish_data.twist.twist.linear.y = enu_velocity.y();
      publish_data.twist.twist.linear.z = enu_velocity.z();
      publish_data.twist.twist.angular.x = msg->rollspeed;
      publish_data.twist.twist.angular.y = -msg->pitchspeed;
      publish_data.twist.twist.angular.z = -msg->yawspeed;

I used the “ned_to_enu_local_frame” for position and linear velocity. It is actually swapping x and y, and using negative z value.

I have totally no idea how to do conversion for w in orientation. Therefore, I used “ned_to_enu_orientation” for the orientation.

For the angular speed, I only use negative vaule for y and z according to the definition of FRD body frame, FLU body frame and right hand rule. When converting from FRD to FLU, the direction of y rotation and z rotation will be reverse. That’s why I used the negative pitchspeed and negative yawspeed as the value of y and z.

For the angular speed, I can only do the conversion according to the definition because there is no function which is suitable in “frame_transforms.cpp”.

However, the final result shown in rviz is incorrect. When the fixed frame is odom, the bottom of the robot is pointing to the sky but all the rotation and translation can follow the hardware pixhawk.

When the fixed frame is map, the translation is working properly but the pitch and yaw are in reverse direction. I have tried to use both negative and positive for pitchspeed and yawspeed. No matter positive or negative, the pitch and yaw direction are reverse. Hence, I think there is something wrong with the conversion of orientation but I am using the function from “frame_transforms.cpp” and the transformation is indeed from NED to ENU.

Therefore, I am still trying and I have no idea how to solve this problem currently.

Untitled
Untitled1994×1460 454 KB

9

@ripdk12 , @JohnnyLiu2886
You are both correctly converting from NED to ENU, however you are missing the conversion between aircraft frame in px4 notation (FRD or, referring to the frame_transforms.h file, aircraft) and aircraft frame in ros notation (FLU or, referring to the frame_transforms.h file, baselink).
To do that you need to apply aircraft_to_baselink_orientation or baselink_to_aircraft_orientation.

This is all needed because the px4 odometry quaternion gives the rotation of an aircraft frame w.r.t. a NED frame, you want a baselink w.r.t. an ENU. Applying only NED_to_ENU gives you an aicraft w.r.t. a ENU frame, which is not enough.

For example, this script converts orientation form “aircraft → NED” to “base_link → ENU”

orientation = px4_ros_com::frame_transforms::ned_to_enu_orientation(
  px4_ros_com::frame_transforms::baselink_to_aircraft_orientation(
    orientation
  )
);

I never had to convert the velocity so I don’t have something ready for that.

1 Like
10

The script is for conerting aircraft into base_link but you are using “baselink_to_aircraft _orientation”. Is is typo? Why is it not “aircraft_to_baselink_orientation” instead?

11

As my good man Benja stated before, if you look at the transform_orientations function:

case StaticTF::AIRCRAFT_TO_BASELINK:
case StaticTF::BASELINK_TO_AIRCRAFT:
	out = q * AIRCRAFT_BASELINK_Q;
	break;

There is no break between the cases of AIRCRAFT_TO_BASELINK, and BASELINK_TO_AIRCRAFT. This means that it doesn’t matter if you do “aircraft_to_baselink_orientation” or “baselink_to_aircraft _orientation”.

And thanks so much Benja for providing the final piece to the puzzle!

1 Like
12

I see. That’s make sense.
I have solved the problem of orientation but the translation is still not working. It is keep trying to go back to the original point in rviz no matter how the robot is moving.
Are you able to do translation properly?

13

I’m also experiencing issues, the orientation still doesn’t match what I expect, I’m not sure how to test it in rviz since my goal is to send data to the px4 and not the other way around.
I’m starting to give up on this rtps bridge honestly, since it’s so hard to figure out on my own, and help is almost impossible to get since so few people actually use it.

14

I found that the “vehicle_odometry” is actually inaccurate. The value is always within 1 meter for position even I move the pixhawk to 22 meters away.

“vehicle_odometry” will only work well when there is gps module or realsense camera.

That’s why I experienced the awful translation inside the office. The robot will always stick to the odometry no matter how far did it move.