Ask Your Question
0

wrong python tf2 Buffer.lookup_transfer data

asked 2017-07-26 07:50:11 -0500

arogowiec gravatar image

updated 2017-07-27 00:43:52 -0500

Hi!

I'm following ideas from python broadcaster, listener tutorials on tf2 package. I broadcast some simple transformation in one node and listen for it in other node. However the listener node doesn't get correct transformation values. When I rostopic echo in terminal /tf topic, I see the values I've sent. Please help me why I receive wrong data?

UPDATE I've added below the code, what output I expect and what I get.

Here is my code for listener

import rospy
import tf2_ros
from geometry_msgs.msg import TransformStamped

def foo(ts):

  sec  = ts.header.stamp.secs
  nsec = ts.header.stamp.nsecs
  time = float(sec * 10**9 + nsec) / float(10**9)
  tx   = ts.transform.translation.x
  ty   = ts.transform.translation.y
  tz   = ts.transform.translation.z
  qx   = ts.transform.rotation.x
  qy   = ts.transform.rotation.y
  qz   = ts.transform.rotation.z
  qw   = ts.transform.rotation.w

  gndt_log_str = ('{0:.4f} {1:.4f} {1:.4f} {3:.4f} {4:.4f}'
                  ' {5:.4f} {6:.4f} {7:.4f}').format(time,
                  tx, ty, tz, qx, qy, qz, qw)
  rospy.loginfo(gndt_log_str)

if __name__ == '__main__':
  rospy.init_node('dummy_tf_ls')
  tf_buffer = tf2_ros.Buffer()
  tf_listener = tf2_ros.TransformListener(tf_buffer)

  rate = rospy.Rate(10.0)
  while not rospy.is_shutdown():
    try:
      ts = tf_buffer.lookup_transform('kinect', 'world', rospy.Time())
    except (tf2_ros.LookupException, tf2_ros.ConnectivityException, 
            tf2_ros.ExtrapolationException):
      rate.sleep()
      continue
    foo(ts)
    rate.sleep()

And for broadcaster:

import rospy
import tf
import tf2_ros
import geometry_msgs.msg

def foo():
    br = tf2_ros.TransformBroadcaster()
    ts = geometry_msgs.msg.TransformStamped()

    ts.header.stamp = rospy.Time.now()
    ts.header.frame_id = 'world'
    ts.child_frame_id = 'kinect'
    ts.transform.translation.x = 2.0
    ts.transform.translation.y = 5.0
    ts.transform.translation.z = 1.0
    q = tf.transformations.quaternion_from_euler(0.0, 0.0, 0.0)
    ts.transform.rotation.x = q[0]
    ts.transform.rotation.y = q[1]
    ts.transform.rotation.z = q[2]
    ts.transform.rotation.w = q[3]

    br.sendTransform(ts)

if __name__ == '__main__':
    rospy.init_node('dummy_tf_br')

    rate = rospy.Rate(10.0)
    while not rospy.is_shutdown():
      foo()
      rate.sleep()

To my understanding I should receive transform of following form (x, y, z, qx, qy, qz, qw): 2.0, 5.0, 1.0, 0.0, 0.0, 0.0, 1.0. And instead I get this:

SUMMARY
========

PARAMETERS
 * /rosdistro: kinetic
 * /rosversion: 1.12.7

NODES
  /
    dumy_tf_br_node (benchmark_slam/dumy_tf_br.py)
    dumy_tf_ls_node (benchmark_slam/dumy_tf_ls.py)

auto-starting new master
process[master]: started with pid [17664]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to 4941a668-728d-11e7-a9ee-180373b6b8a4
process[rosout-1]: started with pid [17717]
started core service [/rosout]
process[dumy_tf_br_node-2]: started with pid [17720]
process[dumy_tf_ls_node-3]: started with pid [17721]
[INFO] [1501133690.232944]: 1501133690.1912 -2.0000 -2.0000 -1.0000 0.0000 0.0000 0.0000 1.0000
[INFO] [1501133690.332916]: 1501133690.2913 -2.0000 -2.0000 -1.0000 0.0000 0.0000 0.0000 1.0000
[INFO] [1501133690.432902]: 1501133690.3913 -2.0000 -2.0000 -1.0000 0.0000 0.0000 0.0000 1.0000
[INFO] [1501133690.532912]: 1501133690.4913 -2.0000 -2.0000 -1.0000 0.0000 0.0000 0.0000 1.0000
[INFO] [1501133690.632998]: 1501133690 ...
(more)
edit retag flag offensive close merge delete

Comments

Please edit your question show the outputs you are getting as well as what you expect to get.

tfoote gravatar image tfoote  ( 2017-07-26 20:05:28 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2017-07-27 12:43:49 -0500

tfoote gravatar image

You have a bug in your print formatting which prints the y component twice.

 gndt_log_str = ('{0:.4f} {1:.4f} {1:.4f} {3:.4f} {4:.4f}'
                  ' {5:.4f} {6:.4f} {7:.4f}').format(time,
                  tx, ty, tz, qx, qy, qz, qw)
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

Stats

Asked: 2017-07-26 07:50:11 -0500

Seen: 248 times

Last updated: Jul 27 '17