Source Code for Module master_discovery_fkie.udp

  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 socket 
 34  import struct 
 35  import fcntl 
 36  import array 
 37  import platform 
 38   
 39  import rospy 
 40   
 41 -class McastSocket(socket.socket): 
 42    ''' 
 43    The McastSocket class enables the send and receive UDP messages to a multicast  
 44    group. 
 45    ''' 
 46   
 47 -  def __init__(self, port, mgroup, reuse=True, ttl=20): 
 48      ''' 
 49      Creates a socket, bind it to a given port and join to a given multicast group. 
 50      IPv4 and IPv6 are supported. 
 51      @param port: the port to bind the socket 
 52      @type port: int 
 53      @param mgroup: the multicast group to join 
 54      @type mgroup: str 
 55      @param reuse: allows the reusing of the port 
 56      @type reuse: boolean (Default: True) 
 57      @param ttl: time to leave 
 58      @type ttl: int (Default: 20) 
 59      ''' 
 60      # get info about the IP version (4 or 6) 
 61      addrinfo = socket.getaddrinfo(mgroup, None)[0] 
 62      socket.socket.__init__(self, addrinfo[0], socket.SOCK_DGRAM, socket.IPPROTO_UDP) 
 63       
 64      # Allow multiple copies of this program on one machine 
 65      if(reuse): 
 66        self.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) 
 67        if hasattr(socket, "SO_REUSEPORT"): 
 68          self.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1) 
 69      # Bind to the port 
 70      self.bind(('', port)) 
 71      # Set Time-to-live (optional) and loop count 
 72      ttl_bin = struct.pack('@i', ttl) 
 73      if addrinfo[0] == socket.AF_INET: # IPv4 
 74        self.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_TTL, ttl_bin) 
 75        self.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_LOOP, 1) 
 76      else:# IPv6 
 77        self.setsockopt(socket.IPPROTO_IPV6, socket.IPV6_MULTICAST_HOPS, ttl_bin) 
 78        socket.setsockopt(socket.IPPROTO_IPV6, socket.IPV6_MULTICAST_LOOP, 1) 
 79   
 80      #join to the multicast group  
 81      group_bin = socket.inet_pton(addrinfo[0], addrinfo[4][0]) 
 82      try: 
 83        if addrinfo[0] == socket.AF_INET: # IPv4 
 84          mreq = group_bin + struct.pack('=I', socket.INADDR_ANY) 
 85          self.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, mreq) 
 86        else: #IPv6 
 87          mreq = group_bin + struct.pack('@I', 0) 
 88          self.setsockopt(socket.IPPROTO_IPV6, socket.IPV6_JOIN_GROUP, mreq) 
 89      except socket.error, (errn, msg): 
 90        err = str(msg) 
 91        if errn in [19]: 
 92          err = ''.join(["socket.error[", str(errn), "]: ", msg, ",\nis multicast route set? e.g. sudo route add -net 224.0.0.0 netmask 224.0.0.0 eth0"]) 
 93        raise Exception(err) 
 94   
 95      self.addrinfo = addrinfo 
 96      self.group_bin = group_bin 
 97      self.sock_5_error_printed = [] 
 98   
 99   
100 -  def close(self): 
101      ''' 
102      Unregister from the multicast group and close the socket. 
103      ''' 
104      if self.addrinfo[0] == socket.AF_INET: # IPv4 
105          mreq = self.group_bin + struct.pack('=I', socket.INADDR_ANY) 
106          self.setsockopt(socket.IPPROTO_IP, socket.IP_DROP_MEMBERSHIP, mreq) 
107      else: #IPv6 
108          mreq = self.group_bin + struct.pack('@I', 0) 
109          self.setsockopt(socket.IPPROTO_IPV6, socket.IPV6_LEAVE_GROUP, mreq) 
110      socket.socket.close(self) 
111   
112 -  def send2group(self, msg): 
113      ''' 
114      Sends the given message to the joined multicast group. Some errors on send  
115      will be ignored (C{ENETRESET}, C{ENETDOWN}, C{ENETUNREACH}) 
116      @param msg: message to send 
117      @type msg: C{str} 
118      ''' 
119      try: 
120        self.sendto(msg, (self.addrinfo[4][0], self.getsockname()[1])) 
121      except socket.error, (errn, msg): 
122        if not errn in [100, 101, 102]: 
123          raise 
124   
125 -  def send2addr(self, msg, addr): 
126      ''' 
127      Sends the given message to the joined multicast group. Some errors on send  
128      will be ignored (C{ENETRESET}, C{ENETDOWN}, C{ENETUNREACH}) 
129      @param msg: message to send 
130      @type msg: C{str} 
131      @param addr: IPv4 or IPv6 address 
132      @type addr: C{str} 
133      ''' 
134      try: 
135        self.sendto(msg, (addr, self.getsockname()[1])) 
136      except socket.error, (errn, msg): 
137        if errn in [-5]: 
138          if not addr in self.sock_5_error_printed: 
139            rospy.logwarn("socket.error[%d]: %s, addr: %s", errn, msg, addr) 
140            self.sock_5_error_printed.append(addr) 
141        elif not errn in [100, 101, 102]: 
142          raise 
143   
144 -  def hasEnabledMulticastIface(self): 
145      ''' 
146      Test all enabled interfaces for a MULTICAST flag. If no enabled interfaces  
147      has a multicast support, False will be returned. 
148      @return: C{True}, if any interfaces with multicast support are enabled. 
149      @rtype: C{boolean} 
150      ''' 
151      SIOCGIFFLAGS = 0x8913 
152      IFF_MULTICAST = 0x1000 # Supports multicast. 
153      IFF_UP = 0x1           # Interface is up. 
154      for (ifname, ip) in McastSocket.localifs(): 
155        args = (ifname + '\0'*32)[:32] 
156        try: 
157          result = fcntl.ioctl(self.fileno(), SIOCGIFFLAGS, args) 
158        except IOError: 
159          return False 
160        flags, = struct.unpack('H', result[16:18]) 
161        if ((flags & IFF_MULTICAST) != 0) & ((flags & IFF_UP) != 0): 
162          return True 
163      return False 
164   
165    @staticmethod 
166 -  def localifs(): 
167      ''' 
168      Used to get a list of the up interfaces and associated IP addresses 
169      on this machine (linux only). 
170     
171      @return: 
172          List of interface tuples.  Each tuple consists of 
173          (interface name, interface IP) 
174      @rtype: C{[(str, str)]} 
175      ''' 
176      SIOCGIFCONF = 0x8912 
177      MAXBYTES = 8096 
178     
179      arch = platform.architecture()[0] 
180     
181      # I really don't know what to call these right now 
182      var1 = -1 
183      var2 = -1 
184      if arch == '32bit': 
185        var1 = 32 
186        var2 = 32 
187      elif arch == '64bit': 
188        var1 = 16 
189        var2 = 40 
190      else: 
191        raise OSError("Unknown architecture: %s" % arch) 
192     
193      sockfd = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) 
194      names = array.array('B', '\0' * MAXBYTES) 
195      outbytes = struct.unpack('iL', fcntl.ioctl( 
196          sockfd.fileno(), 
197          SIOCGIFCONF, 
198          struct.pack('iL', MAXBYTES, names.buffer_info()[0]) 
199          ))[0] 
200     
201      namestr = names.tostring() 
202      return [(namestr[i:i+var1].split('\0', 1)[0], socket.inet_ntoa(namestr[i+20:i+24])) \ 
203              for i in xrange(0, outbytes, var2)] 
204