00001 # Copyright (c) 2008, Willow Garage, Inc. 00002 # All rights reserved. 00003 # 00004 # Redistribution and use in source and binary forms, with or without 00005 # modification, are permitted provided that the following conditions are met: 00006 # 00007 # * Redistributions of source code must retain the above copyright 00008 # notice, this list of conditions and the following disclaimer. 00009 # * Redistributions in binary form must reproduce the above copyright 00010 # notice, this list of conditions and the following disclaimer in the 00011 # documentation and/or other materials provided with the distribution. 00012 # * Neither the name of the Willow Garage, Inc. nor the names of its 00013 # contributors may be used to endorse or promote products derived from 00014 # this software without specific prior written permission. 00015 # 00016 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00017 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00018 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00019 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00020 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00021 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00022 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00023 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00024 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00025 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00026 # POSSIBILITY OF SUCH DAMAGE. 00027 00028 # author: Wim Meeussen 00029 00030 import roslib; roslib.load_manifest('tf2_ros') 00031 import rospy 00032 import rospy 00033 import tf2_ros 00034 import threading 00035 from tf2_msgs.msg import TFMessage 00036 00037 class TransformListener(): 00038 def __init__(self, buffer): 00039 self.listenerthread = TransformListenerThread(buffer) 00040 self.listenerthread.setDaemon(True) 00041 self.listenerthread.start() 00042 00043 00044 class TransformListenerThread(threading.Thread): 00045 def __init__(self, buffer): 00046 self.buffer = buffer 00047 threading.Thread.__init__(self) 00048 00049 00050 00051 def run(self): 00052 rospy.Subscriber("/tf", TFMessage, self.callback) 00053 rospy.spin() 00054 00055 def callback(self, data): 00056 who = data._connection_header.get('callerid', "default_authority") 00057 for transform in data.transforms: 00058 self.buffer.set_transform(transform, who)