Source Code for Package master_discovery_fkie

  1  #!/usr/bin/env python 
  2  # 
  3  # Software License Agreement (BSD License) 
  4  # 
  5  # Copyright (c) 2012, Fraunhofer FKIE/US, Alexander Tiderko 
  6  # All rights reserved. 
  7  # 
  8  # Redistribution and use in source and binary forms, with or without 
  9  # modification, are permitted provided that the following conditions 
 10  # are met: 
 11  # 
 12  #  * Redistributions of source code must retain the above copyright 
 13  #    notice, this list of conditions and the following disclaimer. 
 14  #  * Redistributions in binary form must reproduce the above 
 15  #    copyright notice, this list of conditions and the following 
 16  #    disclaimer in the documentation and/or other materials provided 
 17  #    with the distribution. 
 18  #  * Neither the name of Fraunhofer nor the names of its 
 19  #    contributors may be used to endorse or promote products derived 
 20  #    from this software without specific prior written permission. 
 21  # 
 22  # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 
 23  # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 
 24  # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 
 25  # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 
 26  # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 
 27  # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 
 28  # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 
 29  # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 
 30  # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 
 31  # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 
 32  # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 
 33  # POSSIBILITY OF SUCH DAMAGE. 
 34   
 35  __author__ = "Alexander Tiderko (Alexander.Tiderko@fkie.fraunhofer.de)" 
 36  __copyright__ = "Copyright (c) 2012 Alexander Tiderko, Fraunhofer FKIE" 
 37  __license__ = "BSD" 
 38  __version__ = "0.2" 
 39  __date__ = "2012-02-01" 
 40   
 41  import sys 
 42   
 43  import roslib; roslib.load_manifest('master_discovery_fkie') 
 44  import rospy 
 45   
 46  from master_monitor import MasterMonitor 
 47   
 48  MCAST_GROUP = "226.0.0.0" # ipv6 multicast group ff02::1 
 49  MCAST_PORT = 11511 
 50   
 51 -def getDefaultRPCPort(zeroconf=False): 
 52    try: 
 53      masteruri = MasterMonitor._masteruri_from_ros() 
 54      rospy.loginfo("ROS Master URI: %s", masteruri) 
 55      from urlparse import urlparse 
 56      return urlparse(masteruri).port+(600 if zeroconf else 300)  
 57    except: 
 58      import traceback 
 59      print traceback.format_exc() 
 60      return 11911 if zeroconf else 11611 
 61   
 62 -def setTerminalName(name): 
 63    ''' 
 64    Change the terminal name. 
 65    @param name: New name of the terminal 
 66    @type name:  str 
 67    ''' 
 68    sys.stdout.write("".join(["\x1b]2;",name,"\x07"])) 
 69   
 70 -def setProcessName(name): 
 71    ''' 
 72    Change the process name. 
 73    @param name: New process name 
 74    @type name:  str 
 75    ''' 
 76    try: 
 77      from ctypes import cdll, byref, create_string_buffer 
 78      libc = cdll.LoadLibrary('libc.so.6') 
 79      buff = create_string_buffer(len(name)+1) 
 80      buff.value = name 
 81      libc.prctl(15, byref(buff), 0, 0, 0) 
 82    except: 
 83      pass 
 84   
 85   
 86 -def main(): 
 87    ''' 
 88    Creates and runs the ROS node using multicast messages for discovering 
 89    ''' 
 90    import master_discovery 
 91    rospy.init_node("master_discovery", log_level=rospy.DEBUG) 
 92    setTerminalName(rospy.get_name()) 
 93    setProcessName(rospy.get_name()) 
 94    mcast_group = rospy.get_param('~mcast_group', MCAST_GROUP) 
 95    mcast_port = rospy.get_param('~mcast_port', MCAST_PORT) 
 96    rpc_port = rospy.get_param('~rpc_port', getDefaultRPCPort()) 
 97    try: 
 98      discoverer = master_discovery.Discoverer(mcast_port, mcast_group, rpc_port) 
 99      discoverer.start() 
100      rospy.spin() 
101    except Exception as e: 
102      rospy.logerr("Error while start master_discovery: %s", str(e)) 
103      import os, signal 
104      os.kill(os.getpid(), signal.SIGKILL) 
105      import time 
106      time.sleep(10) 
107   
108 -def main_zeroconf(): 
109    ''' 
110    Creates and runs the ROS node using zeroconf/avahi for discovering 
111    ''' 
112    import zeroconf 
113    rospy.init_node("zeroconf", log_level=rospy.DEBUG) 
114    setTerminalName(rospy.get_name()) 
115    setProcessName(rospy.get_name()) 
116    rpc_port = rospy.get_param('~rpc_port', getDefaultRPCPort(True)) 
117    discoverer = zeroconf.Discoverer(rpc_port) 
118    discoverer.start() 
119    rospy.spin() 
120