tf_relay.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import roslib; roslib.load_manifest('tf_relay')
00004 
00005 import rospy
00006 import tf.msg as tf
00007 import geometry_msgs.msg as geometry_msgs
00008 
00009 class TFRelay(object):
00010     def __init__(self):
00011         self.tf_prefix = rospy.get_param('~tf_prefix')
00012         self.fixed_frame = rospy.get_param('~fixed_frame', '/map')
00013         if self.fixed_frame[0] != '/':
00014             self.fixed_frame = '/' + self.fixed_frame
00015         self.sub = rospy.Subscriber('tf_in', tf.tfMessage, self.callback)
00016         self.pub = rospy.Publisher('tf_out', tf.tfMessage)
00017 
00018     def is_fixed_frame(self, frame_id):
00019         if frame_id[0] == '/' and frame_id == self.fixed_frame:
00020             return True
00021         elif frame_id == self.fixed_frame[1:]:
00022             return True
00023         else:
00024             return False
00025 
00026     def prepend_prefix(self, frame_id):
00027         if frame_id[0] == '/':
00028             return self.tf_prefix + frame_id
00029         else:
00030             return self.tf_prefix + '/' + frame_id
00031 
00032     def callback(self, msg):
00033         for trans in msg.transforms:
00034             if not self.is_fixed_frame(trans.header.frame_id):
00035                 trans.header.frame_id = self.prepend_prefix(trans.header.frame_id)
00036             trans.child_frame_id = self.prepend_prefix(trans.child_frame_id)
00037 
00038         self.pub.publish(msg)
00039 
00040 if __name__ == '__main__':
00041     rospy.init_node('tf_relay')
00042     relay = TFRelay()
00043     rospy.spin()
00044 


tf_relay
Author(s): Lorenz Moesenlechner
autogenerated on Mon Oct 6 2014 08:40:01