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 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         # check the cache
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) # remove 
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         # if not assocd
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 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


dynamic_tf_publisher
Author(s): Manabu Saito
autogenerated on Sat Mar 23 2013 16:00:57