class TFHelper():
__metaclass__=Singleton
"""
Helps broadcasting static tf frames
"""
def __init__(self,frequency=50):
self.frequency=frequency
self.post = Post(self)
self.mutex = Lock()
self.tb = tf.TransformBroadcaster()
self.listener = tf.TransformListener()
self.transforms={}
self.thread=None
self._stop_broadcast=False
self.last_keys=None
self.medians = {}
def __del__(self):
self.stop()
def start(self):
""" Start broadcasting thread, blocking """
with self.mutex:
if self.thread:
self.stop()
self._stop_broadcast=False
self.thread=self.post.__broadcastLoop()
def stop(self):
""" Stop broadcasting thread, blocking """
with self.mutex:
if self.thread:
self._stop_broadcast=True
self.thread.join()
self.thread=None
def __broadcastLoop(self):
"""
Loop that broadcasts all transformations. Started by (BaxterFrame)
"""
with self.mutex:
self.last_keys=None
rate=rospy.Rate(self.frequency)
while not rospy.is_shutdown() and not self._stop_broadcast:
with self.mutex:
if not self.last_keys or str(self.last_keys)!=str(self.transforms.keys()):
rospy.loginfo("Broadcasting %d frames: %s"%(len(self.transforms.items()) , self.transforms.keys() ) )
self.last_keys=str(self.transforms.keys())
for id,t in self.transforms.items():
if not rospy.is_shutdown():
try:
self.tb.sendTransform(self.pos2list(t.pose.position),self.quat2list(t.pose.orientation),rospy.Time.now(), "/"+id, "/"+t.header.frame_id)
except rospy.ROSException,e:
rospy.logwarn("Could not publish on TF topic, closing broadcaster: "+str(e))
break
rate.sleep()
