We are trying to get the heading of our turtlebot. We can get the Quaternion data about the robot's pose but want to convert to a simple euler angle. Is there C++ ROS code that is already available that supports this?

edit retag close merge delete

Sort by » oldest newest most voted Yes, there is. Using the standard tf libraries:

tf:Quaternion q(x,y,z,w);
double yaw,pitch,roll;
btMatrix3x3(q).getEulerYPR(yaw,pitch,roll);

Yaw is what you want.

more It's funny you should ask this question. I have been looking into something similar for a quadrotor. Specifically I want to extract the yaw rotation from a quaternion for some simple visual servoing. At this point I believe this can be done by the following procedure:

1) Set the x and y components of the quaternion to 0 2) Normalize the quaternion. In pseudo-code

mag = sqrt(q^2 + q^2)
q /= mag
q /= mag

3) Convert the quaternion to a rotation matrix. Again in pseudo-code (as per the Rotation matrices section of Conversion between quaternions and Euler angles -- Wikipedia

R = q^2 + q^2 - q^2 - q^2
R = 2*(q*q - q*q)
R = 2*(q*q + q*q)
R = 2*(q*q + q*q)
R = q^2 - q^2 + q^2 - q^2
R = 2*(q*q - q*q)
R = 2*(q*q - q*q)
R = 2*(q*q + q*q)
R = q^2 - q^2 - q^2 + q^2

If you actually want all the Euler angles then the Quaternion section of the Rotation Matrix -- Wikipedia article may be helpful. However beware that the angles you get out may not be accurate due to the shortcomings of Euler angles. For example, if you create a rotation matrix R = RotZ(pi/2)*RotY(pi/2), convert it to a quaternion and convert it back you will get RotY(pi/2). I.e. the process doesn't return pi/2 for the rotation about z.

more

It dawns on me that the conversion to a rotation matrix can be simplified since q = q = 0.