import roslib; roslib.load_manifest('tf2_ros')
import rospy
import rospy
import tf2_ros
import threading
from tf2_msgs.msg import TFMessage

class TransformListener():
    def __init__(self, buffer):
        self.buffer = buffer
        self.tf_sub = rospy.Subscriber("/tf", TFMessage, self.callback)
        self.tf_static_sub = rospy.Subscriber("/tf_static", TFMessage, self.static_callback)

    def __del__(self):
        self.unregister()

    def unregister(self):
        self.tf_sub.unregister()
        self.tf_static_sub.unregister()

    def callback(self, data):
        who = data._connection_header.get('callerid', "default_authority")
        for transform in data.transforms:
            self.buffer.set_transform(transform, who)

    def static_callback(self, data):
        who = data._connection_header.get('callerid', "default_authority")
        for transform in data.transforms:
            self.buffer.set_transform_static(transform, who)