Mavros: Reiterate over frame conversions between NED and ENU

Maybe someone can comment on the current state? I lack the time to sift through the code currently, but I'd be happy to review the transformations.

@LorenzMeier See PR #208.

@LorenzMeier Actually, this is the current convention we use :

body-fixed NED → ROS ENU: (x y z)→(x -y -z) or (w x y z)→(x -y -z w)
local NED → ROS ENU: (x y z)→(y x -z) or (w x y z)→(y x -z w)

I cannot verify the correctness of this diagram, but this is what I think we're doing now.

Yes that's my scheme. I can confirm it, and I used and explained it on my thesis.

Note: This conversion is what we agreed some months ago and I was able to confirm it on Rviz.

The issue isn't the actual conversion but rather when a certain conversion is used.

I'm looking at this:
http://wiki.ros.org/geometry/CoordinateFrameConventions and
http://www.ros.org/reps/rep-0103.html

There are a number of issues with the graphic (and if it represents the orientation, the code):

• ROS is always right-handed. The ENU graphic (bottom row) is left handed, which is not correct.
• ROS is using ENU. ENU means east-north-up, so the X axis is pointing EAST and the zero degree heading is along the Y axis.

Consequently there is no difference between the body frame and the world frame at zero rotation, and the conversion between NED and ENU is the same for both frames, using global / local reference, respectively.

I agree mostly with whats @LorenzMeier has said. However, unless I'm very much mistaken every coordinate frame in the above graphic except the bottom right one is a left handed coordinate system. Maybe you mislabeled x and y axes in the graphics's legend?

Apart from that I agree that converting fron ENU to NED is the same whether its for the ground frame or body frame. On the body frame the name ENU or NED does not make much sense, but still converting from the coordinate frame used in ROS to FCU should always be the same I think.

I haven't yet checked the method for conversion that @mhkabir posted above. Think we need to get thw graphic right first before doing the math

@thedinuka Would you mind whipping up a new one in Paint or something, according to what seems correct to you . ;)

I have the original file, can chnage it then ans send a new one (no need to use paint @mhkabir). The thing about right and left hand seems right. But I still have doubts relative to having two different converions, and when I did that image that time, I was having them too. So probably we need to check the code, cause from what I remember, most of the implementations use the above conversions.

While waiting for the modified version of the graphic by @TSC21 , I have given this a bit more thought and it seems more complicated than I initiall thought. I kind of agree with @TSC21. There are two different pairs of coordinate transformations that we need to think of.

1) Transformation from ROS convention of body-fixed frame to Pixhawk convention of body-fixed frame.
2) Transform from ROS convention of Earth-fixed frame to Pixhawk convention of earth fixed frame.

While there is documentation both on ROS side and Pixhawk side about the convention used for the body-fixed coordinate frame, I couldn't find specific information about the convention used on the Pixhawk side about the Inertial (or Earth Fixed) coordinate frame. If I had to make a guess then I'd say that on the Pixhawk side, in the absence of GPS, the Earth Fixed coordinate frame aligns with the initial orientation of the body-fixed frame, but I'm not 100% sure whether this is the case. So to prevent any mistakes and confusion, I'm going to, for now, only consider the body fixed frame.

Relaying on the documentation [1] for ROS and [2] for Pixhawk, I have made the following diagram.

Once we have the coordinate frame diagrams for both (1) and (2) above verified, confirmed and agreed, then the math to do the actual transformations on mavros side is very straight forward.

[1] http://www.ros.org/reps/rep-0103.html
[2] https://pixhawk.org/dev/know-how/frames_of_reference

Please also look at #148 (covariation conversion).

This may help ros-infrastructure/rep#95

Did we end getting somewhere with this? Are the current conversions from ENU/NED, body/world frames correct? I'm asking this cause we need to iterate the possibility that @blackcoder is getting a yaw rotation on the FCU when sending the vision data because we are doing things wrong.
@vooon, @mhkabir, @tonybaltovski, @thedinuka ping!

@thedinuka Looks good to me.

If I had to make a guess then I'd say that on the Pixhawk side, in the absence of > GPS, the Earth Fixed coordinate frame aligns with the initial orientation of the > body-fixed frame

Actually don't need GPS. We have a global yaw reference from the mag already. That should mean EF frame rotated using by this yaw.

Hi,
I remap the topic from mocap_optitrack to vision_pose/pose in mavros. however the mav can not fly correctly, so I change following code
vision_position_estimate(stamp.toNSec() / 1000,
position.y(), position.x(), -position.z(),
roll, -pitch, -yaw); // ??? please check!
to
vision_position_estimate(stamp.toNSec() / 1000,
position.y(), position.x(), -position.z(),
roll, -pitch, -yaw + 1.57);
i.e change -yaw to -yaw + 1.57.
when conversing body frame from ENU to NED, mavros now use
(x y z)-->(x,-y,-z)
(w,x,y,z)-->(x,-y,-z,w), so we get
ENU ---> NED
roll ---->roll
pitch ----->-pitch
yaw ----->-yaw.
however I think we mistake that the yaw is based on the x axis in world frame(when x in body frame is along with x in world frame , yaw is 0), and have 90 degree rotation. so we should make -yaw + 1.57. i.e
ENU ---> NED
roll ---->roll
pitch ----->-pitch
yaw ----->-yaw+1.57.
after i changed it , the mav can fly correctly. so I think other plugin is also like this.

@congleetea that seems legit. Can you please apply the same transformations to setpoint plugins and see what's the result? also, does the data coming from local_position topic comes with the right orientation without you applying the same conversion?

Please also look at #148 (covariation conversion).

We also must take this into account.

PR #317 change conversions.

I have test this conversing

NED(Pixhawk)->ENU(ROS)
(X,Y,Z)->(X,-Y,-Z)

ENU(ROS)->NED(Pixhawk)
(X,Y,Z)->(X,-Y,-Z)

That works well both in rviz and real fling.

@supermice that confirms #317 so thanks! :)