$search
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()