ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange

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[0]^2 + q[4]^2)
q[0] /= mag
q[1] /= 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[0][0] = q[0]^2 + q[1]^2 - q[2]^2 - q[3]^2
R[1][2] = 2*(q[1]*q[2] - q[0]*q[3])
R[1][3] = 2*(q[1]*q[3] + q[0]*q[2])
R[2][1] = 2*(q[1]*q[2] + q[0]*q[3])
R[2][2] = q[0]^2 - q[1]^2 + q[2]^2 - q[3]^2
R[2][3] = 2*(q[2]*q[3] - q[0]*q[1])
R[3][1] = 2*(q[1]*q[3] - q[0]*q[2])
R[3][2] = 2*(q[2]*q[3] + q[0]*q[1])
R[3][3] = q[0]^2 - q[1]^2 - q[2]^2 + q[3]^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