tf_util.py
Go to the documentation of this file.
00001 
00002 import roslib; roslib.load_manifest('pr2_plugs_actions')
00003 
00004 import rospy 
00005 import tf
00006 import tf2
00007 import tf2_ros
00008 import tf2_geometry_msgs
00009 from geometry_msgs.msg import PoseStamped
00010 from std_srvs.srv import *
00011 
00012 import threading
00013 
00014 __all__ = ['TFUtil']
00015 
00016 class TFUtil():
00017     initialized = False
00018 
00019     listener = None
00020     broadcaster = None
00021 
00022     broadcast_lock = None
00023     broadcast_list = {}
00024     broadcast_threads = {}
00025     
00026     def __init__(self):
00027         if not TFUtil.initialized:
00028             TFUtil.listener = tf2_ros.buffer_client.BufferClient('tf2_buffer_server_plugs')
00029             TFUtil.listener.wait_for_server()
00030             TFUtil.broadcaster = tf.TransformBroadcaster()
00031             TFUtil.initialized = True
00032 
00033             TFUtil.broadcast_lock = threading.Lock()
00034         
00035     @staticmethod
00036     def wait_and_transform(frame_id, pose):
00037         try:
00038             result = TFUtil.listener.transform(pose, frame_id, rospy.Duration(5.0))
00039         except (tf2.LookupException, tf2.ConnectivityException, tf2.TimeoutException, tf2.ExtrapolationException, tf2.TransformException) as e:
00040             rospy.logerr("tf2_ros BufferClient threw an exception: %s, trying again"%str(e))
00041             result = TFUtil.listener.transform(pose, frame_id, rospy.Duration(5.0))
00042         return result
00043 
00044     @staticmethod
00045     def wait_and_lookup(parent_frame_id, child_frame_id, time=None):
00046         if time == None:
00047             time = rospy.Time.now()
00048 
00049         ps = PoseStamped()
00050         ps.header.stamp = time
00051         ps.header.frame_id = child_frame_id
00052         ps.pose.position.x = 0.0
00053         ps.pose.position.y = 0.0
00054         ps.pose.position.z = 0.0
00055         ps.pose.orientation.x = 0.0
00056         ps.pose.orientation.y = 0.0
00057         ps.pose.orientation.z = 0.0
00058         ps.pose.orientation.w = 1.0
00059         try:
00060             result = TFUtil.listener.transform(ps, parent_frame_id, rospy.Duration(2.0))
00061         except (tf2.LookupException, tf2.ConnectivityException, tf2.TimeoutException, tf2.ExtrapolationException, tf2.TransformException) as e:
00062             rospy.logerr("tf2_ros BufferClient threw an exception: %s, trying again"%str(e))
00063             result = TFUtil.listener.transform(ps, parent_frame_id, rospy.Duration(2.0))
00064         return result 
00065 
00066     @staticmethod
00067     def broadcast_transform(frame_id, pose,time = None, period = rospy.Duration(0.1)):
00068         if frame_id in TFUtil.broadcast_list:
00069             with TFUtil.broadcast_lock:
00070                 TFUtil.broadcast_list[frame_id] = (pose, time, period)
00071         else:
00072             broadcast_thread = threading.Thread(
00073                     target=TFUtil.broadcast_loop,
00074                     name='TFUtil Broadcaster: \'%s\'' % frame_id,
00075                     args=[frame_id])
00076             TFUtil.broadcast_list[frame_id] = (pose,time,period)
00077             TFUtil.broadcast_threads[frame_id] = broadcast_thread
00078             broadcast_thread.start()
00079 
00080     @staticmethod
00081     def stop_broadcasting_transform(frame_id):
00082         if frame_id in TFUtil.broadcast_list:
00083             with TFUtil.broadcast_lock:
00084                 del TFUtil.broadcast_list[frame_id]
00085 
00086     @staticmethod
00087     def broadcast_loop(frame_id):
00088         while not rospy.is_shutdown():
00089             with TFUtil.broadcast_lock:
00090                 if frame_id not in TFUtil.broadcast_list:
00091                     break
00092                 pose, time, period = TFUtil.broadcast_list[frame_id]
00093                 if time is None:
00094                     time = rospy.Time.now()
00095                 TFUtil.broadcaster.sendTransform(
00096                         [pose.pose.position.x, pose.pose.position.y, pose.pose.position.z],
00097                         [pose.pose.orientation.x,pose.pose.orientation.y,pose.pose.orientation.z,pose.pose.orientation.w],
00098                         time,
00099                         frame_id,
00100                         pose.header.frame_id)
00101             rospy.sleep(period)
00102 


pr2_plugs_actions
Author(s): Jon Bohren, Patrick Mihelich, Wim Meeussen, and Melonee Wise
autogenerated on Thu Nov 28 2013 11:47:13