tf_publish.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #
00003 # publish static tf which is set by SetDynamicTF service
00004 # publishing tf is uniquely by child_frame_id
00005 #
00006 # TODO: delete target tf
00007 #       check tf tree consistency
00008 #       change frequency by frame_id
00009 #
00010 import roslib; roslib.load_manifest('dynamic_tf_publisher')
00011 import rospy
00012 
00013 from dynamic_tf_publisher.srv import * # SetDynamicTF
00014 from geometry_msgs.msg import TransformStamped,Quaternion,Vector3
00015 from std_srvs.srv import Empty, EmptyResponse
00016 import tf
00017 import tf.msg
00018 import thread
00019 import yaml
00020 import os
00021 if os.getenv('ROS_DISTRO') != 'electric' :
00022     import genpy
00023 
00024 class dynamic_tf_publisher:
00025     def __init__(self):
00026         self.pub_tf = rospy.Publisher("/tf", tf.msg.tfMessage)
00027         self.pub_tf_mine = rospy.Publisher("~tf", tf.msg.tfMessage)
00028         self.cur_tf = dict()
00029         self.original_parent = dict()
00030         self.update_tf = dict()
00031         self.listener = tf.TransformListener()
00032         self.tf_sleep_time = 1.0
00033         self.lockobj = thread.allocate_lock()
00034         rospy.Service('/set_dynamic_tf', SetDynamicTF, self.set_tf)
00035         rospy.Service('/assoc_tf', AssocTF, self.assoc)
00036         rospy.Service('/publish_tf', Empty, self.publish_tf)
00037         rospy.Service('/dissoc_tf', DissocTF, self.dissoc)
00038         rospy.Service('/delete_tf', DeleteTF, self.delete)
00039 
00040         self.use_cache = rospy.get_param('~use_cache', True)
00041         self.check_update = rospy.get_param('~check_update', False)
00042         self.check_update_sleep_time = rospy.get_param('~check_update_sleep_time', 1.0)
00043         self.check_update_last_update = rospy.Time(0)
00044         # check the cache
00045         if self.use_cache and rospy.has_param('dynamic_tf_publisher'+rospy.get_name()) :
00046             tfm = tf.msg.tfMessage()
00047             if os.getenv('ROS_DISTRO') != 'electric' :
00048                 genpy.message.fill_message_args(tfm,[yaml.load(rospy.get_param('dynamic_tf_publisher'+rospy.get_name()))])
00049             else :
00050                 roslib.message.fill_message_args(tfm,[yaml.load(rospy.get_param('dynamic_tf_publisher'+rospy.get_name()))])
00051             for pose in tfm.transforms :
00052                 self.cur_tf[pose.child_frame_id] = pose
00053 
00054     def publish_tf(self, req=None):
00055         self.lockobj.acquire()
00056         time = rospy.Time.now()
00057         tfm = tf.msg.tfMessage()
00058 
00059         if self.check_update:
00060             publish_all = False
00061             if self.check_update_last_update + rospy.Duration(self.check_update_sleep_time) < time:
00062                 publish_all = True
00063                 self.check_update_last_update = time
00064 
00065         for frame_id in self.cur_tf.keys():
00066             if (not self.check_update) or publish_all or self.update_tf[frame_id]:
00067                 pose = self.cur_tf[frame_id]
00068                 pose.header.stamp = time
00069                 tfm.transforms.append(pose)
00070                 self.update_tf[frame_id] = False
00071 
00072         if len(tfm.transforms) > 0:
00073             self.pub_tf.publish(tfm)
00074             self.pub_tf_mine.publish(tfm)
00075         self.lockobj.release()
00076         return EmptyResponse()
00077 
00078     def assoc(self,req):
00079         if (not self.cur_tf.has_key(req.child_frame)) or self.cur_tf[req.child_frame] == req.parent_frame:
00080             rospy.logwarn("unkown key %s" % (req.child_frame))
00081             return AssocTFResponse()
00082         rospy.loginfo("assoc %s -> %s"%(req.parent_frame, req.child_frame))
00083         self.listener.waitForTransform(req.parent_frame,
00084                                        req.child_frame,
00085                                        req.header.stamp, rospy.Duration(1.0))
00086         ts = TransformStamped()
00087         (trans,rot) = self.listener.lookupTransform(req.parent_frame, req.child_frame, req.header.stamp)
00088         ts.transform.translation = Vector3(*trans)
00089         ts.transform.rotation = Quaternion(*rot)
00090         ts.header.stamp = req.header.stamp
00091         ts.header.frame_id = req.parent_frame
00092         ts.child_frame_id = req.child_frame
00093         self.lockobj.acquire()
00094         self.original_parent[req.child_frame] = self.cur_tf[req.child_frame].header.frame_id
00095         self.cur_tf[req.child_frame] = ts
00096         self.update_tf[req.child_frame] = True
00097         self.lockobj.release()
00098         self.publish_tf()
00099         return AssocTFResponse()
00100 
00101     def dissoc(self,req):
00102         areq = None
00103         rospy.loginfo("dissoc TF %s" % (req.frame_id))
00104         self.lockobj.acquire()
00105         if self.original_parent.has_key(req.frame_id):
00106             areq = AssocTFRequest()
00107             areq.header = req.header
00108             areq.child_frame = req.frame_id
00109             areq.parent_frame = self.original_parent[req.frame_id]
00110         self.lockobj.release()
00111         if areq:
00112             self.assoc(areq)
00113             self.original_parent.pop(req.frame_id) # remove 
00114         return DissocTFResponse()
00115 
00116     def delete(self,req):
00117         rospy.loginfo("delete TF %s"%(req.header.frame_id))
00118         self.lockobj.acquire()
00119         if self.original_parent.has_key(req.header.frame_id):
00120             del self.original_parent[req.header.frame_id]
00121         if self.cur_tf.has_key(req.header.frame_id):
00122             del self.cur_tf[req.header.frame_id]
00123         if self.update_tf.has_key(req.header.frame_id):
00124             del self.update_tf[req.header.frame_id]
00125         self.lockobj.release()
00126         return DeleteTFResponse()
00127 
00128     def set_tf(self,req):
00129         self.lockobj.acquire()
00130         # if not assocd
00131         if not self.original_parent.has_key(req.cur_tf.child_frame_id):
00132             self.tf_sleep_time = 1.0/req.freq
00133             self.cur_tf[req.cur_tf.child_frame_id] = req.cur_tf
00134             self.update_tf[req.cur_tf.child_frame_id] = True
00135             print "Latch [%s]/[%shz]"%(req.cur_tf.child_frame_id,req.freq)
00136         self.lockobj.release()
00137         # set parameter
00138         if self.use_cache:
00139             time = rospy.Time.now()
00140             tfm = tf.msg.tfMessage()
00141             for frame_id in self.cur_tf.keys():
00142                 pose = self.cur_tf[frame_id]
00143                 pose.header.stamp = time
00144                 tfm.transforms.append(pose)
00145             rospy.set_param('dynamic_tf_publisher'+rospy.get_name(),tfm.__str__())
00146         return SetDynamicTFResponse()
00147 
00148     def publish_and_sleep(self):
00149         self.publish_tf()
00150         rospy.sleep(self.tf_sleep_time)
00151 
00152 
00153 if __name__ == "__main__":
00154     rospy.init_node('tf_publish_server')
00155     pub = dynamic_tf_publisher()
00156     while not rospy.is_shutdown():
00157         pub.publish_and_sleep()
00158     print "exit"
00159 


dynamic_tf_publisher
Author(s): Manabu Saito
autogenerated on Sun Jan 25 2015 00:28:48