Ask Your Question
1

tf::TransformListener gets wrong tf

asked 2016-09-13 03:57:20 -0600

ToXeR gravatar image

updated 2016-09-13 04:17:04 -0600

Hello,

TL;DR tf::TransformListener gets a different transform than shown in RVIZ

I am having quite a strange problem. I am using a rosbag file to playback recorded data and I am adding new transforms and using them while doing that:

rosparam set use_sim_time true
rosbag play 2016-09-12-14-51-41.bag --clock

The transform is published with a python node, where the origin is static (defined with cfg-values) and the rotation is coming from the values of an IMU. The interesting part how the tf is published should be this (in the callback of the IMU subscription):

  br = tf.TransformBroadcaster()
  br.sendTransform((frame_offset_x, frame_offset_y, frame_offset_z),
               quaternion,
               rospy.Time.now(),
               frame_child,
               frame_parent
              )

With the following values:

  frame_offset_x = 0
  frame_offset_y = 0
  frame_offset_z = 1.2
  quaternion = rotation of the IMU (only around the x and y axis)
  frame_child = level_laser_2
  frame_parent = base_footprint

The created Transform in RVIZ looks like expected. It's 1.2m above base_footprint with some rotation on top. In the picture, the Fixed Frame is set to "base_footprint"

level_tf in RVIZ

However if I lookup the transform in a ROS node using the tf::TransformListener I get a different result:

#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>
...
  tf_listener_ = new tf::TransformListener( ros::Duration(10) ); // in the constructor
...
void callback( const sensor_msgs::PointCloud2Ptr& cloud_in ) {
  ...
  if ( tf_listener_->waitForTransform(tf_target_,
                                      cloud_in->header.frame_id,
                                      cloud_in->header.stamp,
                                      ros::Duration( scan_time_ )) ) {
    tf::StampedTransform transform;
    tf_listener_->lookupTransform(tf_target_,
                                  cloud_in->header.frame_id,
                                  cloud_in->header.stamp,
                                  transform);
    ROS_INFO("%s %s -> %s\tat%lf", node_name_.c_str(),
                                   cloud_in->header.frame_id.c_str(),
                                   tf_target_.c_str(),
                                   cloud_in->header.stamp.toSec());

    tfScalar tf[16];
    transform.getOpenGLMatrix(tf);

    ROS_INFO("%s\n"
             "/ %lf\t%lf\t%lf\t%lf \\\n"
             "| %lf\t%lf\t%lf\t%lf |\n"
             "| %lf\t%lf\t%lf\t%lf |\n"
             "\\ %lf\t%lf\t%lf\t%lf /", node_name_.c_str(),
             tf[0], tf[4], tf[8], tf[12],
             tf[1], tf[5], tf[9], tf[13],
             tf[2], tf[6], tf[10], tf[14],
             tf[3], tf[7], tf[11], tf[15]);
  } else {
    // display error here 
  }
  ...
}

The output of this is:

[ INFO] [1473756806.521699009, 1473684710.202103154]: /level_laser_velodyne:  base_footprint -> level_laser_2 at1473684709.395050
[ INFO] [1473756806.521764836, 1473684710.202103154]: /level_laser_velodyne: 
/ 0.997645  0.001908    -0.068567   0.002741 \
| 0.001908  0.998454    0.055557    -0.002221 |
| 0.068567  -0.055557   0.996098    -0.039822 |
\ 0.000000  0.000000    0.000000    1.000000 /

But it should be something like:

[ INFO] [1473756806.521764836, 1473684710.202103154]: /level_laser_velodyne: 
/ R R R 0 \
| R R R 0 |
| R R R 1.2 |
\ 0 0 0 1 /

Does anybody know what I am doing wrong?

Thank you for your help, Tobias

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
0

answered 2018-11-07 03:47:07 -0600

ToXeR gravatar image

It's not solved, but I've found a workaround by using a different node and therefore I'm closing this question.

I'm sorry that I can't help someone running into the same problem and ending up at this question.

edit flag offensive delete link more
2

answered 2018-11-06 11:26:07 -0600

artifactz gravatar image

I think you need to swap target and source frames in lookupTransform.

I know it is confusing, but I guess the way to think about it is that lookupTransform returns a way of transforming data from source_frame to target_frame. So if you want to know where child is relative to its parent, parent is the target frame, because you want to express the position and orientation of child while looking from parent's frame.

edit flag offensive delete link more

Comments

Thank you, that might've be wrong as well. But I would have expected a translation of 1.2m, instead I got something around 4cm.

However since this question is over 2 years old, I solved it a different way.

ToXeR gravatar imageToXeR ( 2018-11-07 03:45:18 -0600 )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

1 follower

Stats

Asked: 2016-09-13 03:57:20 -0600

Seen: 601 times

Last updated: Nov 07 '18