static broadcaser

static-web-tf-broadcaster.py

#!/usr/bin/env python
import roslib
# roslib.load_manifest('lighthouse_tf')
import rospy

import tf
import geometry_msgs


def handle_static_tf_topic(msg, frames):
    print("subscription event")
    br = tf.TransformBroadcaster()
    translateMultiplier = 2
    br.sendTransform((msg.transform.translation.x * translateMultiplier,
                      msg.transform.translation.y * translateMultiplier,
                      msg.transform.translation.z * translateMultiplier),
                     (msg.transform.rotation.x,
                      msg.transform.rotation.y,
                      msg.transform.rotation.z,
                      msg.transform.rotation.w),
                     msg.header.stamp,
                     frames[0],
                     frames[1])


if __name__ == '__main__':
    #this function discovers topics published under the /componentTFs/ branch.
    # each will be converted based on the name into a static broadcast (__)
    rospy.init_node('web_static_tf_broadcaster')
    availableTopics = rospy.get_published_topics()
    for data in availableTopics:
        for topic in data:
            topicElements = topic.split('/')
            if topicElements[1] == 'componentTFs':
                # this is a topic we want to subscribe to
                rospy.Subscriber(
                    ("/").join(topicElements),
                    geometry_msgs.msg.TransformStamped,
                    handle_static_tf_topic,
                    (topicElements[2].split("__")[2], topicElements[2].split("__")[0])
                )
    rospy.spin()