Revision history [back]

Hi Tomm,

I had a similar problem, to convert quaternions to RPY. In my case, I was working with turtlebot, but it's also a ground robot and I've simplified the problem to a 3 DoF robot (planar scenarios).

Basically, the magic happens here:

tf::Quaternion q(
    msg->pose.pose.orientation.x,
    msg->pose.pose.orientation.y,
    msg->pose.pose.orientation.z,
    msg->pose.pose.orientation.w);
tf::Matrix3x3 m(q);
double roll, pitch, yaw;
m.getRPY(roll, pitch, yaw);
pose2d.theta = yaw;

There's a practical post about quaternion to yaw conversion. I think it can help you. http://www.theconstructsim.com/treating-quaternions-2d-navigation/

I've also created a Gist, because I always forget how to do it :) https://gist.github.com/marcoarruda/f931232fe3490b7fa20dbb38da1195ac

Cheers!