00001
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
00024
00025 if sys.version_info[:3] == (2, 7, 3):
00026 import threading
00027 threading._DummyThread._Thread__stop = lambda x: 42
00028
00029
00030
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
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
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
00087 self.interface_selector.update_event.subscribe_repeating(self._publish_status)
00088
00089 def _publish_status(self):
00090 now = rospy.get_rostime()
00091
00092
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
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
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
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()