win_ros Quaternion conversions

edit

Hi, i'm fairly new to ROS and i got stuck on the following problem. I have a setup where i have a node running on Ubuntu that controls a robot and reports its position. The position is encoded into Quaternion as follows:

tf::poseTFToMsg(tf::Transform(tf::createQuaternionFromYaw(theta), tf::Vector3(x, y, z)), position.pose.pose);

Now, i want to consume that information on windows. I've set it all up and everything works meaning that i receive the robot's position and all but win_ros doesn't seem to have tr:: package so i can't extract the 'theta' angle. Can anyone confirm that tr:: has not been ported to windows? And if so what's the proper way of extracting robot's orientation from Quaternion?

Edit:

I've done the conversion myself a while back but forgot to post it here. Even though it's pretty straight forward here is the code that works well for me (i just took it straight out of tf:: code and massaged it a little).

static void convertQuaternion2Matrix(const geometry_msgs::Quaternion& q, double m[3][3])
{
    double d = q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w;
    if(d == 0.) 
        return;
    double s = 2./d;
    double xs = q.x * s,   ys = q.y * s,   zs = q.z * s;
    double wx = q.w * xs,  wy = q.w * ys,  wz = q.w * zs;
    double xx = q.x * xs,  xy = q.x * ys,  xz = q.x * zs;
    double yy = q.y * ys,  yz = q.y * zs,  zz = q.z * zs;
    //row1
    m[0][0] = 1.0 - (yy + zz);
    m[0][1] = xy - wz;
    m[0][2] = xz + wy;
    //row2
    m[1][0] = xy + wz;
    m[1][1] = 1.0 - (xx + zz);
    m[1][2] = yz - wx;
    //row3
    m[2][0] = xz - wy;
    m[2][1] = yz + wx;
    m[2][2] = 1.0 - (xx + yy);
}

static double tfAcos(double x) 
{ 
    if (x < -1.)
        x = -1.;
    if (x > 1.)
        x = 1.;
    return acos(x);
}

static double tfAsin(double x) 
{ 
    if (x < -1.)
        x = -1;
    if (x > 1)
        x = 1;
    return asin(x);
}

static void getEulerYPR(const geometry_msgs::Quaternion& q, double& yaw, double& pitch, double& roll, unsigned int solution_number = 1)
{
    double m[3][3];
    convertQuaternion2Matrix(q, m);
    struct Euler
    {
        double yaw;
        double pitch;
        double roll;
    };

    Euler euler_out;
    Euler euler_out2; //second solution
    //get the pointer to the raw data

    // Check that pitch is not at a singularity
    // Check that pitch is not at a singularity
    if (fabs(m[2][0]) >= 1)
    {
        euler_out.yaw = 0;
        euler_out2.yaw = 0;

        // From difference of angles formula
        double delta = atan2(m[2][1],m[2][2]);
        if (m[2][0] < 0)  //gimbal locked down
        {
            euler_out.pitch = M_PI / 2.0;
            euler_out2.pitch = M_PI / 2.0;
            euler_out.roll = delta;
            euler_out2.roll = delta;
        }
        else // gimbal locked up
        {
            euler_out.pitch = -M_PI / 2.0;
            euler_out2.pitch = -M_PI / 2.0;
            euler_out.roll = delta;
            euler_out2.roll = delta;
        }
    }
    else
    {
        euler_out.pitch = - tfAsin(m[2][0]);
        euler_out2.pitch = M_PI - euler_out.pitch;

        euler_out.roll = atan2 ...