Skip to content

Instantly share code, notes, and snippets.

@codyhex
Created February 5, 2019 22:34
Show Gist options
  • Save codyhex/29b309802f15eea631416356dfa7522b to your computer and use it in GitHub Desktop.
Save codyhex/29b309802f15eea631416356dfa7522b to your computer and use it in GitHub Desktop.
#!/usr/bin/env python
import rospy
import tf
from tf import transformations as ts
from geometry_msgs.msg import Transform, Vector3, Quaternion
from geometry_msgs.msg import TransformStamped
from std_msgs.msg import Header
if __name__ == '__main__':
rospy.init_node('fixed_tf_broadcaster')
br = tf.TransformBroadcaster()
listener = tf.TransformListener()
rate = rospy.Rate(10.0)
while not rospy.is_shutdown():
# this is here to show your that this function signature is not consistent with others
br.sendTransform((3.0, 2.0, 0.0),
tf.transformations.quaternion_from_euler(0, 0, 45.),
rospy.Time.now(),
"bad_python_child",
"world")
# the Transform in python is not from tf::Transform, you need to feed in the exactly msg type.
trans = Transform(translation=Vector3(1.0, 2.0, 0),
rotation=Quaternion(*tf.transformations.quaternion_from_euler(0, 0, 0))
)
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = 'world' # the parent link
# I use the stamped msg signature call to prevent the order confusion
trans_stamp = TransformStamped(header, 'child_py', trans)
br.sendTransformMessage(trans_stamp)
try:
(trans, rot) = listener.lookupTransform('world', 'child_py', rospy.Time(0))
# because it is not the tf::Transform, there is no reverse member function call, do it manually
transform = ts.concatenate_matrices(ts.translation_matrix(trans), ts.quaternion_matrix(rot))
inversed_trans = ts.inverse_matrix(transform)
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
continue
# now publish the inverse frame back, it should stay at the same location of '/world'
header.frame_id = 'child_py'
trans = Transform(translation=Vector3(*ts.translation_from_matrix(inversed_trans)),
rotation=Quaternion(*ts.quaternion_from_matrix(inversed_trans))
)
trans_stamp_inv = TransformStamped(header, 'world_inv_py', trans)
br.sendTransformMessage(trans_stamp_inv)
rate.sleep()
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment