wrong python tf2 Buffer.lookup_transfer data
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 ...
Please edit your question show the outputs you are getting as well as what you expect to get.