# win_ros Quaternion conversions

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 ...
```

That's useful to have floating around for people, thanks Eugene.