buffer_interface.py
Go to the documentation of this file.
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 tf2
00033 import tf2_ros
00034 from copy import deepcopy
00035 from std_msgs.msg import Header
00036 
00037 class BufferInterface:
00038     def __init__(self):
00039         self.registration = tf2_ros.TransformRegistration()
00040 
00041     # transform, simple api
00042     def transform(self, object_stamped, target_frame, timeout=rospy.Duration(0.0), new_type = None):
00043         do_transform = self.registration.get(type(object_stamped))
00044         res = do_transform(object_stamped, self.lookup_transform(target_frame, object_stamped.header.frame_id,
00045                                                                  object_stamped.header.stamp, timeout))
00046         if new_type == None:
00047             return res
00048 
00049         return convert(res, new_type)
00050     
00051     # transform, advanced api
00052     def transform_full(self, object_stamped, target_frame, target_time, fixed_frame, timeout=rospy.Duration(0.0), new_type = None):
00053         do_transform = self.registration.get(type(object_stamped))
00054         res = do_transform(object_stamped, self.lookup_transform_full(target_frame, target_time,
00055                                                                      object_stamped.header.frame_id, object_stamped.header.stamp, 
00056                                                                      fixed_frame, timeout))
00057         if new_type == None:
00058             return res
00059 
00060         return convert(res, new_type)
00061 
00062     # lookup, simple api 
00063     def lookup_transform(self, target_frame, source_frame, time, timeout=rospy.Duration(0.0)):
00064         raise NotImplementedException()        
00065 
00066     # lookup, advanced api 
00067     def lookup_transform_full(self, target_frame, target_time, source_frame, source_time, fixed_frame, timeout=rospy.Duration(0.0)):
00068         raise NotImplementedException()        
00069 
00070     # can, simple api
00071     def can_transform(self, target_frame, source_frame, time, timeout=rospy.Duration(0.0)):
00072         raise NotImplementedException()        
00073     
00074     # can, advanced api
00075     def can_transform_full(self, target_frame, target_time, source_frame, source_time, fixed_frame, timeout=rospy.Duration(0.0)):
00076         raise NotImplementedException()        
00077 
00078 
00079 def Stamped(obj, stamp, frame_id):
00080     obj.header = Header(frame_id=frame_id, stamp=stamp)
00081     return obj
00082 
00083 
00084 
00085 class TypeException(Exception):
00086     def __init__(self, errstr):
00087         self.errstr = errstr
00088 
00089 class NotImplementedException(Exception):
00090     def __init__(self):
00091         self.errstr = 'CanTransform or LookupTransform not implemented'
00092 
00093 
00094 class TransformRegistration():
00095     __type_map = {}
00096     
00097     def print_me(self):
00098         print TransformRegistration.__type_map
00099 
00100     def add(self, key, callback):
00101         TransformRegistration.__type_map[key] = callback
00102 
00103     def get(self, key):
00104         if not key in TransformRegistration.__type_map:
00105             raise TypeException('Type %s if not loaded or supported'% str(key))
00106         else:
00107             return TransformRegistration.__type_map[key]
00108 
00109 class ConvertRegistration():
00110     __to_msg_map = {}
00111     __from_msg_map = {}
00112     __convert_map = {}
00113     
00114     def add_from_msg(self, key, callback):
00115         ConvertRegistration.__from_msg_map[key] = callback
00116 
00117     def add_to_msg(self, key, callback):
00118         ConvertRegistration.__to_msg_map[key] = callback
00119 
00120     def add_convert(self, key, callback):
00121         ConvertRegistration.__convert_map[key] = callback
00122 
00123     def get_from_msg(self, key):
00124         if not key in ConvertRegistration.__from_msg_map:
00125             raise TypeException('Type %s if not loaded or supported'% str(key))
00126         else:
00127             return ConvertRegistration.__from_msg_map[key]
00128 
00129     def get_to_msg(self, key):
00130         if not key in ConvertRegistration.__to_msg_map:
00131             raise TypeException('Type %s if not loaded or supported'%str(key))
00132         else:
00133             return ConvertRegistration.__to_msg_map[key]
00134 
00135     def get_convert(self, key):
00136         if not key in ConvertRegistration.__convert_map:
00137             raise TypeException("Type %s if not loaded or supported" % str(key))
00138         else:
00139             return ConvertRegistration.__convert_map[key]
00140 
00141 def convert(a, b_type):
00142     c = ConvertRegistration()
00143     #check if an efficient conversion function between the types exists
00144     try:
00145         f = c.get_convert((type(a), b_type))
00146         print "efficient copy"
00147         return f(a)
00148     except TypeException:
00149         if type(a) == b_type:
00150             print "deep copy"
00151             return deepcopy(a)
00152 
00153         f_to = c.get_to_msg(type(a))
00154         f_from = c.get_from_msg(b_type)
00155         print "message copy"
00156         return f_from(f_to(a))
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines


tf2_ros
Author(s): Wim Meeussen, Eitan Marder-Eppstein
autogenerated on Mon Aug 19 2013 10:27:02