lookupTransform after publishing transform seems not right

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

Xegara
50 ●10 ●14 ●20

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

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

add a comment

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; }

lookupTransform after publishing transform seems not right

1 Answer

Question Tools

Stats

Related questions

lookupTransform after publishing transform seems not right edit

1 Answer

Question Tools

Stats

Related questions

lookupTransform after publishing transform seems not right