multi_interface_roam: multi_interface_roam: roam

00001 #! /usr/bin/env python
00002 
00003 import roslib; roslib.load_manifest('multi_interface_roam')
00004 import rospy
00005 import config
00006 import dynamic_reconfigure.server
00007 import twisted.internet.reactor as reactor
00008 from multi_interface_roam.cfg import MultiInterfaceRoamConfig
00009 from multi_interface_roam.msg import MultiInterfaceStatus, InterfaceStatus
00010 from diagnostic_msgs.msg import DiagnosticStatus, KeyValue, DiagnosticArray
00011 from pr2_msgs.msg import AccessPoint
00012 import mac_addr
00013 import interface_selector
00014 import sys
00015 from std_msgs.msg import Int32
00016 import sigblock
00017 import signal
00018 import interface
00019 from netlink_monitor import IFSTATE
00020 from ieee80211_channels.channels import IEEE80211_Channels
00021 import os
00022 
00023 ##### monkey-patch to suppress threading error message in python 2.7.3
00024 ##### See http://stackoverflow.com/questions/13193278/understand-python-threading-bug
00025 if sys.version_info[:3] == (2, 7, 3):
00026     import threading
00027     threading._DummyThread._Thread__stop = lambda x: 42
00028 #####
00029 
00030 # Make sure states are nice and consistent...
00031 assert(InterfaceStatus.STATE_NO_INTERFACE == -1)
00032 assert(IFSTATE.PLUGGED == InterfaceStatus.STATE_PLUGGED)
00033 assert(IFSTATE.UP == InterfaceStatus.STATE_UP)
00034 assert(IFSTATE.LINK == InterfaceStatus.STATE_LINK)
00035 assert(IFSTATE.LINK_ADDR == InterfaceStatus.STATE_LINK_ADDR)
00036 assert(IFSTATE.ADDR == InterfaceStatus.STATE_ADDR)
00037 assert(InterfaceStatus.STATE_PINGING == 5)
00038 
00039 STATUSES = { 
00040     InterfaceStatus.STATE_NO_INTERFACE : "Interface not found",
00041     InterfaceStatus.STATE_PLUGGED      : "Interface is down", 
00042     InterfaceStatus.STATE_UP           : "Interface is up with no link", 
00043     InterfaceStatus.STATE_LINK         : "Interface has a link with no address",
00044     InterfaceStatus.STATE_LINK_ADDR    : "Interface has a link but no IP address",
00045     InterfaceStatus.STATE_ADDR         : "Interface has an IP address but pings are failing",
00046     InterfaceStatus.STATE_PINGING      : "Interface is validated",
00047 }
00048 
00049 # FIXME May want to kill this at some point
00050 import asmach as smach
00051 smach.logdebug = lambda x: None
00052 
00053 class Node:
00054     def __init__(self, *args, **kwargs):
00055         sigblock.save_mask()
00056         sigblock.block_signal(signal.SIGCHLD)
00057         rospy.init_node(*args, **kwargs)
00058         sigblock.restore_mask()
00059         rospy.core.add_shutdown_hook(self._shutdown_by_ros)
00060         reactor.addSystemEventTrigger('after', 'shutdown', self._shutdown_by_reactor)
00061         rospy.loginfo("Node __init__ done");
00062 
00063     def _shutdown_by_reactor(self):
00064         rospy.signal_shutdown("Reactor shutting down.")
00065 
00066     def _shutdown_by_ros(self, why):
00067         reactor.fireSystemEvent('shutdown')
00068 
00069 class RoamNode:
00070     def __init__(self):
00071         Node("multi_interface_roam")
00072         self.interface_selector = interface_selector.InterfaceSelector()
00073         self.reconfig_server = dynamic_reconfigure.server.Server(MultiInterfaceRoamConfig, self.reconfigure)
00074         self._interfaces = self.interface_selector.interfaces.values()
00075         self.hostname = os.uname()[1]
00076         
00077         # Prepare topics to publish
00078         pub_namespace = rospy.remap_name('wifi')
00079         self.diag_pub = rospy.Publisher("/diagnostics", DiagnosticArray)
00080         self.ap_pub = rospy.Publisher(pub_namespace+"/accesspoint", AccessPoint)
00081         self.status_pub = rospy.Publisher(pub_namespace+"/status", MultiInterfaceStatus)
00082         self.iface_id_pub = rospy.Publisher(pub_namespace+'/current_iface_id', Int32, latch = True)
00083         self._wireless_interfaces = [ i for i in self._interfaces if i.__class__ == interface.WirelessInterface ]
00084         self.all_ap_pub = dict((iface, rospy.Publisher(pub_namespace+"/"+iface.iface+"/accesspoint", AccessPoint)) for iface in self._wireless_interfaces)
00085         
00086         # Kick off publication updates.
00087         self.interface_selector.update_event.subscribe_repeating(self._publish_status)
00088 
00089     def _publish_status(self):
00090         now = rospy.get_rostime()
00091 
00092         # current_iface_id
00093         ai = self.interface_selector.active_interfaces
00094         if not ai or ai[0] not in self._interfaces:
00095             index = -1
00096         else:
00097             index = self._interfaces.index(ai[0])
00098         self.iface_id_pub.publish(index)
00099 
00100         # accesspoint
00101         best_active = self.interface_selector.radio_manager.best_active
00102         for iface in self._wireless_interfaces:
00103             msg = self.gen_accesspoint_msg(iface)
00104             msg.header.stamp = now
00105             self.all_ap_pub[iface].publish(msg)
00106             if iface == best_active:
00107                 self.ap_pub.publish(msg)
00108         if best_active is None:
00109             self.ap_pub.publish(AccessPoint())
00110 
00111         # status
00112         msg = MultiInterfaceStatus()
00113         for iface in self._interfaces:
00114             msg.interfaces.append(self.gen_status_msg(iface))
00115         self.status_pub.publish(msg)
00116 
00117         # diagnostics
00118         diags = []
00119         diags.append(('Tunnel Interface', self.interface_selector.tunnel_interface))
00120         if self.interface_selector.active_interfaces and \
00121              self.interface_selector.active_interfaces[0].score != self.interface_selector.TERRIBLE_INTERFACE:
00122             act_iface = self.interface_selector.active_interfaces[0]
00123             diags.append(('Active Interface', act_iface.iface ))
00124             diags += act_iface.diags
00125             if act_iface.goodness > 95:
00126                 diag_summary = "Active interface %s running strong"%act_iface.iface
00127                 diag_level = 0
00128             elif act_iface.goodness > 50:
00129                 diag_summary = "Active interface %s is lossy"%act_iface.iface
00130                 diag_level = 1
00131             else:
00132               if act_iface.goodness > 0:
00133                 diag_summary = "Active interface %s is very poor"%act_iface.iface
00134               else:
00135                 diag_summary = "Active interface %s is failing to ping"%act_iface.iface
00136               diag_level = 2
00137         else:
00138             diags.append(('Active Interface', "none"))
00139             diag_summary = 'No active interface'
00140             diag_level = 2
00141         ds = self.fill_diags("synthetic interface", diag_summary, self.hostname, diags)
00142         ds.level = diag_level
00143         statuses = [ds]
00144 
00145         for iface in self._interfaces:
00146             status = iface.status
00147             if status == InterfaceStatus.STATE_ADDR and iface.ping_loss < 100:
00148                 status = InterfaceStatus.STATE_PINGING
00149                 diag_summary = "Connected (goodness %f, reliability %f)"%(iface.goodness, iface.reliability)
00150             else:
00151                 diag_summary = STATUSES[status]
00152             ds = self.fill_diags(iface.iface, diag_summary, self.hostname, iface.diags)
00153             statuses.append(ds)
00154 
00155         da = DiagnosticArray()
00156         da.header.stamp = rospy.get_rostime()
00157         da.status = statuses
00158         self.diag_pub.publish(da)
00159     
00160     
00161     @staticmethod
00162     def fill_diags(name, summary, hid, diags):
00163         ds = DiagnosticStatus()
00164         ds.values = [KeyValue(k, str(v)) for (k, v) in diags]
00165         ds.hardware_id = hid
00166         ds.name = rospy.get_caller_id().lstrip('/') + ": " + name
00167         ds.message = summary
00168         return ds
00169     
00170                 
00171     @staticmethod
00172     def gen_status_msg(iface):
00173         msg = InterfaceStatus()
00174         msg.pretty_name = iface.prettyname
00175         msg.interface = iface.iface
00176         msg.state = iface.status
00177         if msg.state == InterfaceStatus.STATE_ADDR and iface.ping_loss < 100:
00178            msg.state = InterfaceStatus.STATE_PINGING
00179         msg.active_interface_rank = iface.rank
00180         msg.goodness = iface.goodness
00181         msg.reliability = iface.reliability
00182         msg.score = iface.score
00183         msg.prescore = iface.prescore
00184         msg.latency = iface.ping_latency
00185         msg.loss = iface.ping_loss
00186         if iface.__class__ == interface.WirelessInterface:
00187             cur_bss = iface.radio_sm.associated.get()
00188             if cur_bss:
00189                 msg.bss = cur_bss
00190         return msg
00191 
00192     @staticmethod
00193     def gen_accesspoint_msg(iface):
00194         msg = AccessPoint()
00195         msg.essid = iface.essid
00196         msg.macaddr = iface.bssid
00197         msg.signal = iface.wifi_signal 
00198         msg.noise = iface.wifi_noise
00199         msg.snr = msg.signal - msg.noise
00200         msg.quality = iface.wifi_quality
00201         msg.rate = iface.wifi_rate
00202         msg.tx_power = iface.wifi_txpower
00203         msg.channel = IEEE80211_Channels.get_channel(iface.wifi_frequency * 1e6)
00204         return msg
00205 
00206     def reconfigure(self, config, level):
00207         if config['interface'] not in self.interface_selector.interfaces:
00208             config['interface'] = ''
00209         interface = config['interface']
00210         ssid = config['ssid'] = config['ssid'][0:32]
00211         bssid = ""
00212         if not mac_addr.is_str(config['bssid']):
00213             config['bssid'] = ""
00214         else:
00215             bssid = mac_addr.str_to_packed(config['bssid'])
00216         if not self.interface_selector.tunnel_interface:
00217             config['use_tunnel'] = False
00218         use_tunnel = config['use_tunnel']
00219 
00220         self.interface_selector.set_mode(ssid, bssid, interface, use_tunnel, config['band'], config['scan_only'])
00221 
00222         return config
00223 
00224 if __name__ == "__main__":
00225     def start():
00226         print "roam_node2 starting..."
00227         try:
00228             RoamNode()
00229         except:
00230             import traceback
00231             traceback.print_exc()
00232             print >> sys.stderr, "\nCaught exception during startup. Shutting down."
00233             reactor.fireSystemEvent('shutdown')
00234     reactor.addSystemEventTrigger('before', 'startup', start)
00235     reactor.run()
roam_node2.py
