00001
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
