buffer.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 tf2_msgs.srv import FrameGraph, FrameGraphResponse
00035 import rosgraph.masterapi
00036 
00037 class Buffer(tf2.BufferCore, tf2_ros.BufferInterface):
00038     def __init__(self, cache_time = None, debug = True):
00039         if cache_time != None:
00040             tf2.BufferCore.__init__(self, cache_time)
00041         else:
00042             tf2.BufferCore.__init__(self)
00043         tf2_ros.BufferInterface.__init__(self)
00044 
00045         if debug:
00046             #Check to see if the service has already been advertised in this node
00047             try:
00048                 m = rosgraph.masterapi.Master(rospy.get_name())
00049                 m.lookupService('~tf2_frames')
00050             except (rosgraph.masterapi.Error, rosgraph.masterapi.Failure):   
00051                 self.frame_server = rospy.Service('~tf2_frames', FrameGraph, self.__get_frames)
00052 
00053     def __get_frames(self, req):
00054        return FrameGraphResponse(self.all_frames_as_yaml()) 
00055         
00056     # lookup, simple api 
00057     def lookup_transform(self, target_frame, source_frame, time, timeout=rospy.Duration(0.0)):
00058         self.can_transform(target_frame, source_frame, time, timeout)
00059         return self.lookup_transform_core(target_frame, source_frame, time)
00060 
00061     # lookup, advanced api 
00062     def lookup_transform_full(self, target_frame, target_time, source_frame, source_time, fixed_frame, timeout=rospy.Duration(0.0)):
00063         self.can_transform_full(target_frame, target_time, source_frame, source_time, fixed_frame, timeout)
00064         return self.lookup_transform_full_core(target_frame, target_time, source_frame, source_time, fixed_frame)
00065 
00066 
00067     # can, simple api
00068     def can_transform(self, target_frame, source_frame, time, timeout=rospy.Duration(0.0)):
00069         if timeout != rospy.Duration(0.0):
00070             start_time = rospy.Time.now()
00071             while (rospy.Time.now() < start_time + timeout and 
00072                    not self.can_transform_core(target_frame, source_frame, time)):
00073                 rospy.Duration(0.05).sleep()
00074         return self.can_transform_core(target_frame, source_frame, time)
00075     
00076     # can, advanced api
00077     def can_transform_full(self, target_frame, target_time, source_frame, source_time, fixed_frame, timeout=rospy.Duration(0.0)):
00078         if timeout != rospy.Duration(0.0):
00079             start_time = rospy.Time.now()
00080             while (rospy.Time.now() < start_time + timeout and 
00081                    not self.can_transform_full_core(target_frame, target_time, source_frame, source_time, fixed_frame)):
00082                 rospy.Duration(0.05).sleep()
00083         return self.can_transform_full_core(target_frame, target_time, source_frame, source_time, fixed_frame)
00084 
 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