Source Code for Module master_discovery_fkie.interface_finder

  1  # Software License Agreement (BSD License) 
  2  # 
  3  # Copyright (c) 2012, Fraunhofer FKIE/US, Alexander Tiderko 
  4  # All rights reserved. 
  5  # 
  6  # Redistribution and use in source and binary forms, with or without 
  7  # modification, are permitted provided that the following conditions 
  8  # are met: 
  9  # 
 10  #  * Redistributions of source code must retain the above copyright 
 11  #    notice, this list of conditions and the following disclaimer. 
 12  #  * Redistributions in binary form must reproduce the above 
 13  #    copyright notice, this list of conditions and the following 
 14  #    disclaimer in the documentation and/or other materials provided 
 15  #    with the distribution. 
 16  #  * Neither the name of Fraunhofer nor the names of its 
 17  #    contributors may be used to endorse or promote products derived 
 18  #    from this software without specific prior written permission. 
 19  # 
 20  # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 
 21  # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 
 22  # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 
 23  # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 
 24  # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 
 25  # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 
 26  # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 
 27  # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 
 28  # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 
 29  # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 
 30  # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 
 31  # POSSIBILITY OF SUCH DAMAGE. 
 32   
 33  import time 
 34  import xmlrpclib 
 35   
 36  import roslib; roslib.load_manifest('master_discovery_fkie') 
 37  import rospy 
 38   
 39   
 40 -def hostFromUri(uri): 
 41    ''' 
 42    Extracts the hostname from given uri.  
 43    @param uri: the uri to parse 
 44    @type uri:  C{str} 
 45    @return: the hostname or None, if the uri is None or invalid 
 46    @rtype: C{str} or C{None} 
 47    @see: U{http://docs.python.org/library/urlparse.html} 
 48    ''' 
 49    if uri is None: 
 50      return None 
 51    from urlparse import urlparse 
 52    try: 
 53      o = urlparse(uri) 
 54      return o.hostname 
 55    except: 
 56      return None 
 57   
 58   
 59 -def get_changes_topic(masteruri, wait=True): 
 60    ''' 
 61    Search in publishers of ROS master for a topic with type MasterState and  
 62    returns his name, if it runs on the local host. Returns empty list if no topic 
 63    was found and C{wait} is C{False}. 
 64    @param masteruri: the URI of the ROS master 
 65    @type masteruri: C{str} 
 66    @param wait: check every second for the topic 
 67    @type wait: C{boolean} 
 68    @return: the list with names of the topic with type L{MasterState} 
 69    @rtype: C{[str]} 
 70    ''' 
 71    result = [] 
 72    while not result and not rospy.is_shutdown(): 
 73      master = xmlrpclib.ServerProxy(masteruri) 
 74      # get the system state to resolve the published nodes 
 75      code, message, state = master.getSystemState(rospy.get_name()) 
 76      # read topic types 
 77      code, msg, val = master.getPublishedTopics(rospy.get_name(), '') 
 78      if code == 1: 
 79        # search for a topic with type MasterState 
 80        for topic, type in val: 
 81          if type.endswith('MasterState'): 
 82            # get the name of the publisher node 
 83            for t, l in state[0]: 
 84              if topic == t: 
 85                # get the URI of the publisher node 
 86                for n in l: 
 87                  code, msg, val = master.lookupNode(rospy.get_name(), n) 
 88                  # only local publisher will be tacked 
 89                  if code == 1 and hostFromUri(val) == hostFromUri(masteruri): 
 90                    result.append(topic) 
 91        if not result and wait: 
 92          rospy.logwarn("Master_discovery node appear not to running. Wait for topic with type 'MasterState'.") 
 93          time.sleep(1) 
 94      elif not result and wait: 
 95        rospy.logwarn("Can't get published topics from ROS master: %s, %s. Will keep trying!", str(code), msg) 
 96        time.sleep(1) 
 97      if not wait: 
 98        return result 
 99    return result 
100   
101 -def get_stats_topic(masteruri, wait=True): 
102    ''' 
103    Search in publishers of ROS master for a topic with type LinkStatesStamped and  
104    returns his name, if it runs on the local host. Returns empty list if no topic 
105    was found and C{wait} is C{False}. 
106    @param masteruri: the URI of the ROS master 
107    @type masteruri: C{str} 
108    @param wait: check every second for the topic 
109    @type wait: C{boolean} 
110    @return: the list of names of the topic with type L{LinkStatesStamped} 
111    @rtype: C{[str]} 
112    ''' 
113    result = [] 
114    while not result and not rospy.is_shutdown(): 
115      master = xmlrpclib.ServerProxy(masteruri) 
116      # get the system state to resolve the published nodes 
117      code, message, state = master.getSystemState(rospy.get_name()) 
118      # read topic types 
119      code, msg, val = master.getPublishedTopics(rospy.get_name(), '') 
120      if code == 1: 
121        # search for a topic with type MasterState 
122        for topic, type in val: 
123          if type.endswith('LinkStatesStamped'): 
124            # get the name of the publisher node 
125            for t, l in state[0]: 
126              if topic == t: 
127                # get the URI of the publisher node 
128                for n in l: 
129                  code, msg, val = master.lookupNode(rospy.get_name(), n) 
130                  # only local publisher will be tacked 
131                  if code == 1 and hostFromUri(val) == hostFromUri(masteruri): 
132                    result.append(topic) 
133        if not result and wait: 
134          rospy.logwarn("Master_discovery node appear not to running. Wait for topic with type 'LinkStatesStamped'.") 
135          time.sleep(1) 
136      elif not result and wait: 
137        rospy.logwarn("Can't get published topics from ROS master: %s, %s. Will keep trying!", str(code), msg) 
138        time.sleep(1) 
139      if not wait: 
140        return result 
141    return result 
142   
143   
144 -def get_listmaster_service(masteruri, wait=True): 
145    ''' 
146    Search in services of ROS master for a service with name ending by  
147    C{list_masters} and returns his name, if it runs on the local host. Returns  
148    empty list if no service was found and C{wait} is C{False}. 
149    @param masteruri: the URI of the ROS master 
150    @type masteruri: C{str} 
151    @param wait: check every second for the service 
152    @type wait: C{boolean} 
153    @return: the list with names of the services ending with C{list_masters} 
154    @rtype: C{[str]} 
155    ''' 
156    result = [] 
157    while not result and not rospy.is_shutdown(): 
158      master = xmlrpclib.ServerProxy(masteruri) 
159      code, msg, val = master.getSystemState(rospy.get_name()) 
160      if code == 1: 
161        pubs, subs, srvs = val 
162        # search for a service 
163        for srv, providers in srvs: 
164          if srv.endswith('list_masters'): 
165            # only local service will be tacked 
166            code, msg, val = master.lookupService(rospy.get_name(), srv) 
167            if code == 1 and hostFromUri(val) == hostFromUri(masteruri): 
168              result.append(srv) 
169        if not result and wait: 
170          rospy.logwarn("Master_discovery node appear not to running. Wait for service 'list_masters'.") 
171          time.sleep(1) 
172      elif not result and wait: 
173        rospy.logwarn("can't get state from ROS master: %s, %s", str(code), msg) 
174        time.sleep(1) 
175      if not wait: 
176        return result 
177    return result 
178