CODEWITHSUNDEEP
orientationrosquaternions
Nicooost
Nicooost

Reputation: 76

Quaternion to Yaw to Heading

I have quaternion values q available.

These are correct, because they align the object correctly in the ROS RVIZ tool.

I now want to get a heading value from these values.

My idea was to calculate the Yaw value first and then do a Radian to Degree conversion to get the heading. However, this works only conditionally, as soon as the object moves in the direction of NE everything is correct and the correct heading value is output. The same applies for the direction SW. However, when the object moves in the NW direction, I get a heading value of 135 degrees, exactly the opposite direction. Exactly the same applies to the direction SE.

My conversions look as follows:

double heading = atan2(2.0*(q.y*q.z + q.w*q.x), -1.0 +2.0 * (q.w*q.w + q.x*q.x)) * 180 / M_PI;
if(heading < 0){
   heading +=360;
}

I got the formula from: https://math.stackexchange.com/questions/3496329/map-quaternion-to-heading-pitch-roll-using-different-axis

My Question is: What I'm doing wrong.. Why are NW and SE swapped?

Upvotes: 0

Views: 2044

Answers (3)

Nicooost
Nicooost

Reputation: 76

My Problem was, that the Yaw value was reflected and rotated by 90 Degrees.

My RVIZ tool does not use N E S W System, so in this case it was displayed correct.

The solution was:

double heading = (atan2(2.0*(q.y*q.z + q.w*q.x), -1.0 +2.0 * (q.w*q.w + q.x*q.x)) * 180 / M_PI) - 90;
if(heading < 0){
   heading +=360;
}
heading = -heading + 360

Thanks, your solutions are also correct, if the value is not rotated.

Upvotes: 0

BTables
BTables

Reputation: 4843

Since this is inside ROS it would typically be recommended to use the tf library that already exists which includes a getRPY() function for quaternions. Take the following example:

tf2::Quaternion tf_quat;
tf2::fromMsg(quat_msg, tf_quat); //Assume quat_msg is a quaternion ros msg

tf2::Matrix3x3 m(tf_quat);
double roll, pitch, yaw;
m.getRPY(roll, pitch, yaw);

Upvotes: 1

jonasmike
jonasmike

Reputation: 393

Really don't know ROS RVIZ. But for Bullet3d I use:

siny = +2.0 * (q.w * q.z + q.y * q.x);
cosy = +1.0 - 2.0 * (q.x * q.x + q.z * q.z);
heading = atan2(siny, cosy);      // in radians

Upvotes: 1

Related Questions