Go to the documentation of this file.00001
00002
00003 import roslib
00004 roslib.load_manifest('tf')
00005 roslib.load_manifest('rospy')
00006 roslib.load_manifest('geometry_msgs')
00007 roslib.load_manifest('hrl_lib')
00008 import rospy, optparse, math, time
00009 import numpy as np
00010 import tf
00011 import tf.transformations as tr
00012 import cPickle as pkl
00013 import hrl_lib.transforms as hrl_tr
00014 import hrl_lib.util as ut
00015
00016 from geometry_msgs.msg import TransformStamped
00017
00018 def log_parse():
00019 parser = optparse.OptionParser('Input the source frame name \
00020 and the target frame name')
00021
00022 parser.add_option("-s", "--source", action="store", type="string",\
00023 dest="source_frame", default="l_gripper_tool_frame")
00024 parser.add_option("-t", "--target" , action="store", type="string",\
00025 dest="target_frame",default="base_link")
00026 (options, args) = parser.parse_args()
00027
00028 return options.source_frame, options.target_frame
00029
00030
00031 class tf_frame_publisher():
00032 def __init__(self):
00033 self.source_frame, self.target_frame = log_parse()
00034 self.pub = rospy.Publisher('/frame/'+self.source_frame,\
00035 TransformStamped)
00036 rospy.init_node('pub_tf_'+self.source_frame, anonymous = True)
00037 self.tflistener = tf.TransformListener()
00038 self.pos = np.matrix([0.,0.,0.]).T
00039 self.rot = np.matrix([0.,0.,0.]).T
00040 self.init_rot = np.matrix([0.,0.,0.]).T
00041 self.quat = [0.,0.,0.,0.]
00042 self.tf = TransformStamped()
00043
00044
00045 def listen_pub(self):
00046 while not rospy.is_shutdown():
00047 p, q = self.tflistener.lookupTransform(self.target_frame,\
00048 self.source_frame, rospy.Time(0))
00049 self.tf.header.frame_id = '/'+self.target_frame
00050 self.tf.header.stamp = rospy.Time.now()
00051 self.tf.child_frame_id = '/'+self.source_frame
00052 self.tf.transform.translation.x = p[0]
00053 self.tf.transform.translation.y = p[1]
00054 self.tf.transform.translation.z = p[2]
00055 self.tf.transform.rotation.x = q[0]
00056 self.tf.transform.rotation.y = q[1]
00057 self.tf.transform.rotation.z = q[2]
00058 self.tf.transform.rotation.w = q[3]
00059 self.pub.publish(self.tf)
00060 rospy.sleep(1/100.)
00061
00062 if __name__ == '__main__':
00063 frame = tf_frame_publisher()
00064 rospy.sleep(1)
00065 try:
00066 frame.listen_pub()
00067 except rospy.ROSInterruptException: pass
00068