# How do I get the angle in radians (0 to 2pi) from a quaternion?

I'm trying to get the angle of a position model in Stage in radians from the Odometry message published to the "odom" topic.

This is how I've done it:

theta = 2 * asin(msg.pose.pose.orientation.z);


This sort of works in getting the angle in radians, but whenever the angle is below the x axis it gives me a negative angle.

Is there anyway of getting the angle in radians going from 0 to 2pi, starting from the positive x axis going anti-clockwise?

Thanks.

edit retag close merge delete

Sort by » oldest newest most voted

Look in the angles package. There is a header with functions that normalize the radian. I think there is also one for [0, 2 Pi).

For getting yaw of a quaternion, there should be a function in tf that does this for you.

more

3
( 2012-08-14 08:30:11 -0500 )edit

A formula that give an angle from 0 to 2pi is:

f(x,y)=pi()-pi()/2(1+sign(x)) (1-sign(y^2))-pi()/4(2+sign(x))sign(y)

      -sign(x*y)*atan((abs(x)-abs(y))/(abs(x)+abs(y)))


There is x=x2-x1 and y=y2-y1,and the formula is working

for any value of x and y.For x=y=0 the result is undefined.

more