Ask Your Question
1

Getting Turtlebot Heading

asked 2012-03-31 09:56:35 -0500

zlalanne gravatar image

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 flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
7

answered 2012-04-01 06:01:06 -0500

Ryan gravatar image

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.

edit flag offensive delete link more
0

answered 2012-03-31 10:55:18 -0500

Dustin gravatar image

updated 2012-03-31 11:14:02 -0500

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.

edit flag offensive delete link more

Comments

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

Dustin gravatar imageDustin ( 2012-03-31 11:12:17 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2012-03-31 09:56:35 -0500

Seen: 2,561 times

Last updated: Apr 01 '12