Ask Your Question
0

win_ros Quaternion conversions

asked 2013-05-09 09:51:27 -0500

Eugene Simine gravatar image

updated 2014-01-28 17:16:29 -0500

ngrennan gravatar image

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 ...
(more)
edit retag flag offensive close merge delete

Comments

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

Daniel Stonier gravatar image Daniel Stonier  ( 2013-06-14 16:01:10 -0500 )edit

2 Answers

Sort by ยป oldest newest most voted
1

answered 2013-05-09 19:28:22 -0500

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.

edit flag offensive delete link more
0

answered 2013-05-10 10:46:37 -0500

Eugene Simine gravatar image

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.

edit flag offensive delete link more

Comments

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.

Daniel Stonier gravatar image Daniel Stonier  ( 2013-05-13 21:10:06 -0500 )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.

Eugene Simine gravatar image Eugene Simine  ( 2013-05-14 05:13:47 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

Stats

Asked: 2013-05-09 09:51:27 -0500

Seen: 592 times

Last updated: Jun 14 '13