# 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 ...
edit retag close merge delete

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

( 2013-06-14 16:01:10 -0600 )edit

Sort by » oldest newest most voted

According to the wiki, tf and tf_conversion packages are part of the geometry stack. I don't know if the win_ros wiki is up-to-date, but its supported stacks list does not include the geometry stack.

As you've probably already seen, the "normal" way in ROS is to use the tf:: functions:

tf::Matrix3x3(q).getRPY(R,P,Y)


But, if you can't build the tf package in win_ros, you can always copy-paste the math from the relevant tf functions:

1) build a rotation matrix from quaternion data (see setRotation)

2) convert rotation matrix to Euler Angles (see getEulerYPR)

Alternatively, you could use the transform functions built into Eigen, which should be available on Windows.

more

Thanks for the reply. I just wanted to check that i'm not missing anything before i code the conversion myself. I thought that maybe somebody had to deal with this already. But i guess not too many people are using win_ros.

more

tf and rqt are probably the next stacks needing porting for winros now that the build environment and basic comms are working. Porting isn't too hard and I'd be happy to help anyone step through the process if they want to try.

( 2013-05-13 21:10:06 -0600 )edit

Hmmm, that's a thought. I have some backlog of stuff i need to do but once i get some free time perhaps i'll contact you to get some guidance on how to port them.

( 2013-05-14 05:13:47 -0600 )edit