Rebroadcast tf transforms with different frame_id on the fly

edit

Node for this job, I have in daily use: https://github.com/andreasBihlmaier/a...

Here is my Code, which takes a transform A=>B and republishes it to B=>A_new. I used it to make markers, which appear as children of the camera, a parent of the camera.

Just change the values.

Cheers,

Rabe

#!/usr/bin/env python

import rospy
import tf

if __name__ == '__main__':
    rospy.init_node("transform_inverter")
    rospy.sleep(0.5)

    child_frame = rospy.get_param("~child_frame")
    parent_frame = rospy.get_param("~parent_frame")
    new_parent_name = rospy.get_param("~new_parent_frame")

    listener = tf.TransformListener()
    broadcaster = tf.TransformBroadcaster()
    rate = rospy.Rate(100)

    while not rospy.is_shutdown():
        try:
            listener.waitForTransform(parent_frame, child_frame, rospy.Time.now(), rospy.Duration(2.0))
            (trans, rot) = listener.lookupTransform(child_frame, parent_frame, rospy.Time(0))
            broadcaster.sendTransform(trans,rot, rospy.Time.now(), parent_frame, new_parent_name)
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException, tf.Exception):
            rospy.loginfo("Couldn't find a transform")
            continue