Source Code for Module rocon_utilities.ros_utilities

  1  #!/usr/bin/env python 
  2  # 
  3  # License: BSD 
  4  #   https://raw.github.com/robotics-in-concert/rocon_multimaster/master/rocon_utilities/LICENSE 
  5  # 
  6   
  7  ############################################################################## 
  8  # Imports 
  9  ############################################################################## 
 10   
 11  import rospy 
 12  import rospkg 
 13  import roslib 
 14   
 15  ############################################################################## 
 16  # Resources 
 17  ############################################################################## 
 18   
 19   
 20 -def find_resource(package, resource): 
 21      ''' 
 22        Convenience wrapper around roslib to find a resource (file) inside 
 23        a package. It checks the output, and provides the appropriate 
 24        error if there is one. 
 25   
 26        @param package : ros package 
 27        @param resource : some file inside the specified package 
 28        @return str : absolute path to the file 
 29   
 30        @raise IOError : raised if there is nothing found or multiple objects found. 
 31      ''' 
 32      try: 
 33          resolved = roslib.packages.find_resource(package, resource) 
 34          if not resolved: 
 35              raise IOError("cannot locate [%s] in package [%s]" % (resource, package)) 
 36          elif len(resolved) == 1: 
 37              return resolved[0] 
 38          elif len(resolved) > 1: 
 39              raise IOError("multiple resources named [%s] in package [%s]:%s\nPlease specify full path instead" % (resource, package, ''.join(['\n- %s' % r for r in resolved]))) 
 40      except rospkg.ResourceNotFound: 
 41          raise IOError("[%s] is not a package or launch file name" % package) 
 42      return None 
 43   
 44   
 45 -def is_absolute_name(name): 
 46      ''' 
 47        Checks if the name begins with a leading slash which validates it 
 48        either as an absolute or relative name in the ros world. 
 49      ''' 
 50      return name[:1] == '/' 
 51   
 52  ############################################################################## 
 53  # Subscriber Proxy 
 54  ############################################################################## 
 55   
 56   
 57 -class SubscriberProxy(): 
 58      ''' 
 59        Works like a service proxy, but using a subscriber instead. 
 60      ''' 
 61 -    def __init__(self, topic, msg_type): 
 62          ''' 
 63            @param topic : the topic name to subscriber to 
 64            @type str 
 65            @param msg_type : any ros message type (typical arg for the subscriber) 
 66            @type msg 
 67            @param timeout : timeout on the wait operation (None = /infty) 
 68            @type rospy.Duration() 
 69            @return msg type data or None 
 70          ''' 
 71          self._subscriber = rospy.Subscriber(topic, msg_type, self._callback) 
 72          self._data = None 
 73   
 74 -    def __call__(self, timeout=None): 
 75          ''' 
 76            Returns immediately with the latest data or waits for 
 77            incoming data. 
 78   
 79            @param timeout : time to wait for data, polling at 10Hz. 
 80            @type rospy.Duration 
 81            @return latest data or None 
 82          ''' 
 83          r = rospy.Rate(10) 
 84          start_time = rospy.Time.now() 
 85          while not rospy.is_shutdown() and self._data == None: 
 86              r.sleep() 
 87              if timeout: 
 88                  if rospy.Time.now() - start_time > timeout: 
 89                      return None 
 90          return self._data 
 91   
 92 -    def wait_for_next(self, timeout=None): 
 93          ''' 
 94            Makes sure any current data is cleared and waits for new data. 
 95          ''' 
 96          self._data = None 
 97          return self.__call__(timeout) 
 98   
 99 -    def wait_for_publishers(self): 
100          ''' 
101            Blocks until publishers are seen. 
102   
103            @raise rospy.exceptions.ROSInterruptException if we are in shutdown. 
104          ''' 
105          r = rospy.Rate(10) 
106          while not rospy.is_shutdown(): 
107              if self._subscriber.get_num_connections() != 0: 
108                  return 
109              else: 
110                  r.sleep() 
111          # we are shutting down 
112          raise rospy.exceptions.ROSInterruptException 
113   
114 -    def _callback(self, data): 
115          self._data = data 
116