lookupTransform after publishing transform seems not right

Question

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?

---

Originally posted by Xegara on ROS Answers with karma: 52 on 2014-06-21

Post score: 0

Answer 1

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. :-)

---

Originally posted by Xegara with karma: 52 on 2014-06-22

This answer was ACCEPTED on the original site

Post score: 3