Ask Your Question
0

lookupTransform after publishing transform seems not right

asked 2014-06-21 10:17:21 -0600

Xegara gravatar image

updated 2014-06-21 10:20:14 -0600

There seems a problem in the lookupTransform function. The code provided below... Please just ignore the other variables.. I just copied a code snippet from my ROS project. My concern is.. After publishing the transform, I tried looking up the transform to check and see if the transformation has indeed applied successfully! But what turns out was the x,y,z,roll and pitch are consistent but the yaw is inconsistent! First, I tried getting the ros::Time(0) latest available transformation but the yaw was inconsistent. Hoping that the yaw will be consistent if I lookupTransform at the exact time as when the transformation was published, it really just showed inconsistency.

Kindly go through the code and take special notice to the codes that I will highlight.

ros::Time now = ros::Time::now();

try
{
    tf_.lookupTransform("/map", "/map1Origin", ros::Time(0), transform1);
    map2_.x = transform1.getOrigin().x() - map2Origin_.x;
    map2_.y = transform1.getOrigin().y() - map2Origin_.y;
    map2_.z = transform1.getOrigin().z() - map2Origin_.z;
    map2_.roll = transform1.getRotation().x() - map2Origin_.roll;
    map2_.pitch = transform1.getRotation().y() - map2Origin_.pitch;
    map2_.yaw = transform1.getRotation().z() + m.yaw * CV_PI/180 - map2Origin_.yaw;

    transform_.setOrigin( tf::Vector3 (map2_.x, map2_.y, map2_.z));
    tf::Quaternion q;
    q.setRPY(map2_.roll, map2_.pitch, map2_.yaw);
    transform_.setRotation(q);
    br_.sendTransform(tf::StampedTransform(transform_, now, "/map", "/map2"));

    std::cerr << "(y)Map Merge Successful [" << m.x << "," << m.y << ","  << m.yaw  << "].." << std::endl;
}
catch (tf::TransformException ex)
{
    std::cerr << "Catched error" << std::endl;
}

// HIGHLIGHT: map2_ is the exact tf that was used to broadcast the transform.
std::cerr << "map2_="<<map2_.x << "," << map2_.y << "," << map2_.z << "," << map2_.roll << "," << map2_.pitch << "," << map2_.yaw << std::endl;

try
{
    tf_.waitForTransform("/map", "/map2", now, ros::Duration(3.0));
    tf_.lookupTransform("/map", "/map2", now, transform1);
}
catch (tf::TransformException ex)
{
    std::cerr << "Catched error" << std::endl;
}
// HIGHLIGHT: Transform1 is the queried transform of the newly broadcasted transform.
std::cerr << transform1.getOrigin().x() << "," << transform1.getOrigin().y() << "," <<
             transform1.getOrigin().z() << "," << transform1.getRotation().x() << "," <<
             transform1.getRotation().y() << "," << transform1.getRotation().z() << std::endl;

Here is the output:

http://postimg.org/image/bc3k76vub/

Notice these lines:

map2_=-0.05,-0.45,0,0,0,1.93732
-0.05,-0.45,0,0,0,0.824126

The lookup transform is different. Why is that so?

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
3

answered 2014-06-22 01:23:02 -0600

Xegara gravatar image

I managed to find a workaround. I researched on other possible answers. I realized that there are two representations of the angles -- quaternion and euler.

What I inputted was expressed in euler -- RPY -- but then when we get the rotation, it returns a quaternion.

On the steps that I did, I did not use the constructor in tf::Quaternion even though it did accept parameters as yaw, pitch, roll. I tried that but the RPY keeps on changing.. Instead I used the member function of tf::Quaternion in setting the yaw, pitch, roll which totally worked.

So here are the steps:

 1. To broadcast RPY transformation in radians

    tf::Quaternion q;
    q.setRPY(roll,pitch,yaw);
    transform.setRotation(q);

  2. To read the RPY in radians

    tf.waitForTransform("turtle1", "carrot1", now, ros::Duration(3) );
    tf.lookupTransform("turtle1", "carrot1", now, transform_);
    transform_.getBasis().getRPY(roll,pitch,yaw);

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <tf/LinearMath/Matrix3x3.h>

int main(int argc, char**argv){
    ros::init(argc, argv, "sample");
    ros::NodeHandle node;

    tf::TransformBroadcaster br;
    tf::StampedTransform transform;
    tf::StampedTransform transform_;
    tf::TransformListener tf;

    ros::Rate rate(10.0);
    int i = 0;
    double roll = 0, pitch = 1, yaw = 3.14/180*90;
    while(node.ok()){

        transform.setOrigin( tf::Vector3(0.0,0.0,0.0));
        tf::Quaternion q;
        q.setRPY(roll,pitch,yaw);
        transform.setRotation(q);
        ros::Time now = ros::Time::now();
        br.sendTransform(tf::StampedTransform(transform, now, "turtle1", "carrot1"));

        std::cerr << "One: " << roll << "," << pitch << "," << yaw << std::endl;

        tf.waitForTransform("turtle1", "carrot1", now, ros::Duration(3) );
        tf.lookupTransform("turtle1", "carrot1", now, transform_);

        transform_.getBasis().getRPY(roll,pitch,yaw);

        std::cerr << "Two: " << roll << "," << pitch << "," << yaw << std::endl;
        rate.sleep();
    }
    return 0;
}

Hope this helps other people too. :-)

edit flag offensive delete link more

Your Answer

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

Add Answer

Question Tools

1 follower

Stats

Asked: 2014-06-21 10:17:21 -0600

Seen: 516 times

Last updated: Jun 22 '14