Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010 import roslib; roslib.load_manifest('dynamic_tf_publisher')
00011 import rospy
00012
00013 from dynamic_tf_publisher.srv import *
00014 from geometry_msgs.msg import TransformStamped,Quaternion,Vector3
00015 import tf
00016 import tf.msg
00017 import thread
00018 import yaml
00019 import os
00020 if os.getenv('ROS_DISTRO') == 'fuerte' :
00021 import genpy
00022
00023 class dynamic_tf_publisher:
00024 def __init__(self):
00025 self.pub_tf = rospy.Publisher("/tf", tf.msg.tfMessage)
00026 self.pub_tf_mine = rospy.Publisher("~tf", tf.msg.tfMessage)
00027 self.cur_tf = dict()
00028 self.original_parent = dict()
00029 self.listener = tf.TransformListener()
00030 self.tf_sleep_time = 1.0
00031 self.lockobj = thread.allocate_lock()
00032 rospy.Service('/set_dynamic_tf', SetDynamicTF, self.set_tf)
00033 rospy.Service('/assoc_tf', AssocTF, self.assoc)
00034 rospy.Service('/dissoc_tf', DissocTF, self.dissoc)
00035 rospy.Service('/delete_tf', DeleteTF, self.delete)
00036
00037 if rospy.has_param('dynamic_tf_publisher'+rospy.get_name()) :
00038 tfm = tf.msg.tfMessage()
00039 if os.getenv('ROS_DISTRO') == 'fuerte' :
00040 genpy.message.fill_message_args(tfm,[yaml.load(rospy.get_param('dynamic_tf_publisher'+rospy.get_name()))])
00041 else :
00042 roslib.message.fill_message_args(tfm,[yaml.load(rospy.get_param('dynamic_tf_publisher'+rospy.get_name()))])
00043 for pose in tfm.transforms :
00044 self.cur_tf[pose.header.frame_id] = pose
00045
00046
00047 def publish_tf(self):
00048 self.lockobj.acquire()
00049 time = rospy.Time.now()
00050 tfm = tf.msg.tfMessage()
00051 for frame_id in self.cur_tf.keys():
00052 pose = self.cur_tf[frame_id]
00053 pose.header.stamp = time
00054 tfm.transforms.append(pose)
00055 rospy.set_param('dynamic_tf_publisher'+rospy.get_name(),tfm.__str__())
00056 self.pub_tf.publish(tfm)
00057 self.pub_tf_mine.publish(tfm)
00058 self.lockobj.release()
00059
00060 def assoc(self,req):
00061 if (not self.cur_tf.has_key(req.child_frame)) or self.cur_tf[req.child_frame] == req.parent_frame:
00062 rospy.logwarn("unkown key %s" % (req.child_frame))
00063 return AssocTFResponse()
00064 rospy.loginfo("assoc %s -> %s"%(req.parent_frame, req.child_frame))
00065 self.listener.waitForTransform(req.parent_frame,
00066 req.child_frame,
00067 req.header.stamp, rospy.Duration(1.0))
00068 ts = TransformStamped()
00069 (trans,rot) = self.listener.lookupTransform(req.parent_frame, req.child_frame, req.header.stamp)
00070 ts.transform.translation = Vector3(*trans)
00071 ts.transform.rotation = Quaternion(*rot)
00072 ts.header.stamp = req.header.stamp
00073 ts.header.frame_id = req.parent_frame
00074 ts.child_frame_id = req.child_frame
00075 self.lockobj.acquire()
00076 self.original_parent[req.child_frame] = self.cur_tf[req.child_frame].header.frame_id
00077 self.cur_tf[req.child_frame] = ts
00078 self.lockobj.release()
00079 self.publish_tf()
00080 return AssocTFResponse()
00081
00082 def dissoc(self,req):
00083 areq = None
00084 rospy.loginfo("dissoc TF %s" % (req.frame_id))
00085 self.lockobj.acquire()
00086 if self.original_parent.has_key(req.frame_id):
00087 areq = AssocTFRequest()
00088 areq.header = req.header
00089 areq.child_frame = req.frame_id
00090 areq.parent_frame = self.original_parent[req.frame_id]
00091 self.lockobj.release()
00092 if areq:
00093 self.assoc(areq)
00094 self.original_parent.pop(req.frame_id)
00095 return DissocTFResponse()
00096
00097 def delete(self,req):
00098 rospy.loginfo("delete TF %s"%(req.header.frame_id))
00099 self.lockobj.acquire()
00100 if self.original_parent.has_key(req.header.frame_id):
00101 del self.original_parent[req.header.frame_id]
00102 if self.cur_tf.has_key(req.header.frame_id):
00103 del self.cur_tf[req.header.frame_id]
00104 self.lockobj.release()
00105 return DeleteTFResponse()
00106
00107 def set_tf(self,req):
00108 self.lockobj.acquire()
00109
00110 if not self.original_parent.has_key(req.cur_tf.child_frame_id):
00111 self.tf_sleep_time = 1.0/req.freq
00112 self.cur_tf[req.cur_tf.child_frame_id] = req.cur_tf
00113 print "Latch [%s]/[%shz]"%(req.cur_tf.child_frame_id,req.freq)
00114 self.lockobj.release()
00115 self.publish_tf()
00116 return SetDynamicTFResponse()
00117
00118 def publish_and_sleep(self):
00119 self.publish_tf()
00120 rospy.sleep(self.tf_sleep_time)
00121
00122
00123 if __name__ == "__main__":
00124 rospy.init_node('tf_publish_server')
00125 pub = dynamic_tf_publisher()
00126 while not rospy.is_shutdown():
00127 pub.publish_and_sleep()
00128 print "exit"
00129