00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 import os
00019 import sys
00020 from subprocess import Popen, PIPE
00021 import paramiko
00022
00023 import rospy
00024 from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
00025
00026 class IwConfigParser(object):
00027 def _parse_info(self, info):
00028 if 'ESSID' in info:
00029 return self._parse_client(info)
00030 else:
00031 return self._parse_ap(info)
00032
00033 def _parse_ap(self, info):
00034 values = []
00035 try:
00036 split = info.split('IEEE ',1)
00037 split = split[1].split('Mode:',1)
00038 norm = split[0].encode('utf8').strip()
00039 values.append(KeyValue("IEEE Norm", norm))
00040 split = split[1].split('\n',1)
00041 mode = split[0].encode('utf8').strip()
00042 values.append(KeyValue("Mode", mode))
00043 if split[1].find('Retry short limit:') != -1:
00044 split = split[1].split('Retry short limit:',1)
00045 if split[1].find('Retry short limit=') != -1:
00046 split = split[1].split('Retry short limit=',1)
00047 if split[1].find('RTS thr:') != -1:
00048 split = split[1].split('RTS thr:',1)
00049 if split[1].find('RTS thr=') != -1:
00050 split = split[1].split('RTS thr=',1)
00051 retry_short_limit = split[0].encode('utf8').strip()
00052 values.append(KeyValue("Retry short limit", str(retry_short_limit)))
00053 if split[1].find('Fragment thr:') != -1:
00054 split = split[1].split('Fragment thr:',1)
00055 if split[1].find('Fragment thr=') != -1:
00056 split = split[1].split('Fragment thr=',1)
00057 rts_thr = split[0].encode('utf8').strip()
00058 values.append(KeyValue("RTS thr", rts_thr))
00059 split = split[1].split('\n',1)
00060 fragment_thr = split[0].encode('utf8').strip()
00061 values.append(KeyValue("Fragment thr", fragment_thr))
00062 split = split[1].split('Power Management:',1)
00063 split = split[1].split('\n',1)
00064 power_managment = split[0].encode('utf8').strip()
00065 values.append(KeyValue("Power Managment", power_managment))
00066
00067 except Exception, e:
00068 rospy.logerr("IwConfigParser parsing exception: %s" %e)
00069 values = [ KeyValue(key = 'parsing exception', value = str(e)) ]
00070
00071 return values
00072
00073 def _parse_client(self, info):
00074 values = []
00075 try:
00076 split = info.split('IEEE ',1)
00077 split = split[1].split('ESSID:',1)
00078 norm = split[0].encode('utf8').strip()
00079 values.append(KeyValue("IEEE Norm", norm))
00080 split = split[1].split('\n',1)
00081 essid = split[0].encode('utf8').strip()
00082 values.append(KeyValue("ESSID", essid))
00083 split = split[1].split('Mode:',1)
00084 split = split[1].split('Frequency:',1)
00085 mode = split[0].encode('utf8').strip()
00086 values.append(KeyValue("Mode", mode))
00087 split = split[1].split(' GHz',1)
00088 frequency = float(split[0].strip())
00089 values.append(KeyValue("Frequency", str(frequency)))
00090 split = split[1].split('Access Point: ',1)
00091 split = split[1].split('\n',1)
00092 access_point = split[0].encode('utf8').strip()
00093 values.append(KeyValue("Access Point", access_point))
00094 split = split[1].split('Bit Rate=',1)
00095 split = split[1].split(' Mb/s',1)
00096 bit_rate = float(split[0].strip())
00097 values.append(KeyValue("Bit Rate [Mb/s]", str(bit_rate)))
00098 if split[1].find('Tx-Power') != -1:
00099 split = split[1].split('Tx-Power=',1)
00100 split = split[1].split(' dBm',1)
00101 tx_power = float(split[0].strip())
00102 values.append(KeyValue("Tx-Power [dBm]", str(tx_power)))
00103 if split[1].find('Retry short limit:') != -1:
00104 split = split[1].split('Retry short limit:',1)
00105 if split[1].find('Retry short limit=') != -1:
00106 split = split[1].split('Retry short limit=',1)
00107 if split[1].find('RTS thr:') != -1:
00108 split = split[1].split('RTS thr:',1)
00109 if split[1].find('RTS thr=') != -1:
00110 split = split[1].split('RTS thr=',1)
00111 retry_short_limit = split[0].encode('utf8').strip()
00112 values.append(KeyValue("Retry short limit", str(retry_short_limit)))
00113 if split[1].find('Fragment thr:') != -1:
00114 split = split[1].split('Fragment thr:',1)
00115 if split[1].find('Fragment thr=') != -1:
00116 split = split[1].split('Fragment thr=',1)
00117 rts_thr = split[0].encode('utf8').strip()
00118 values.append(KeyValue("RTS thr", rts_thr))
00119 split = split[1].split('\n',1)
00120 fragment_thr = split[0].encode('utf8').strip()
00121 values.append(KeyValue("Fragment thr", fragment_thr))
00122 split = split[1].split('Power Management:',1)
00123 split = split[1].split('\n',1)
00124 power_managment = split[0].encode('utf8').strip()
00125 values.append(KeyValue("Power Managment", power_managment))
00126 split = split[1].split('Link Quality=',1)
00127 split = split[1].split('Signal level=',1)
00128 link_quality = split[0].encode('utf8').strip()
00129 values.append(KeyValue("Link Quality", link_quality))
00130 link_quality_percent = split[0].split('/')
00131 link_quality_percent = int(float(link_quality_percent[0].strip()) / float(link_quality_percent[1].strip())*100.0)
00132 values.append(KeyValue("Link Quality %", str(link_quality_percent)))
00133 split = split[1].split(' dBm',1)
00134 signal_level = float(split[0].strip())
00135 values.append(KeyValue("Signal level [dBm]", str(signal_level)))
00136 split = split[1].split('Rx invalid nwid:',1)
00137 split = split[1].split('Rx invalid crypt:',1)
00138 rx_invalid_nwid = int(split[0].strip())
00139 values.append(KeyValue("Rx invalid nwid", str(rx_invalid_nwid)))
00140 split = split[1].split('Rx invalid frag:',1)
00141 rx_invalid_crypt = int(split[0].strip())
00142 values.append(KeyValue("Rx invalid crypt", str(rx_invalid_crypt)))
00143 split = split[1].split('\n',1)
00144 rx_invalid_frag = int(split[0].strip())
00145 values.append(KeyValue("Rx invalid frag", str(rx_invalid_frag)))
00146 split = split[1].split('Tx excessive retries:',1)
00147 split = split[1].split('Invalid misc:',1)
00148 tx_excessive_retries = int(split[0].strip())
00149 values.append(KeyValue("Tx excessive retries", str(tx_excessive_retries)))
00150 split = split[1].split('Missed beacon:',1)
00151 invalid_misc = int(split[0].strip())
00152 values.append(KeyValue("Invalid misc", str(invalid_misc)))
00153 split = split[1].split('\n',1)
00154 missed_beacon = int(split[0].strip())
00155 values.append(KeyValue("Missed beacon", str(missed_beacon)))
00156
00157 except Exception, e:
00158 rospy.logerr("IwConfigParser parsing exception: %s" %e)
00159 values = [ KeyValue(key = 'parsing exception', value = str(e)) ]
00160
00161 return values
00162
00163 class IwConfigLocal(IwConfigParser):
00164 def __init__(self):
00165 IwConfigParser.__init__(self)
00166
00167 self.interfaces = []
00168 self.stat = DiagnosticStatus()
00169 self.stat.level = DiagnosticStatus.OK
00170 self.stat.message = "OK"
00171 self.stat.values = []
00172 try:
00173 p = Popen("iw dev | awk '$1==\"Interface\"{print $2}'", stdout=PIPE, stdin=PIPE, stderr=PIPE, shell=True)
00174 res = p.wait()
00175 (stdout,stderr) = p.communicate()
00176 self.interfaces = sorted(os.linesep.join([s for s in stdout.splitlines() if s]).split('\n'))
00177 except Exception, e:
00178 rospy.logerr("IwConfigLocal init exception: %s" %e)
00179
00180 def update(self):
00181 self.stat.level = DiagnosticStatus.OK
00182 self.stat.message = "OK"
00183 self.stat.values = []
00184 for interface in self.interfaces:
00185 self.stat.values.append(KeyValue(key = str(interface), value = "======================="))
00186 try:
00187 p = Popen(["iwconfig", interface], stdout=PIPE, stdin=PIPE, stderr=PIPE)
00188 res = p.wait()
00189 (stdout,stderr) = p.communicate()
00190
00191 if res != 0:
00192 self.stat.values.append(KeyValue(key = 'iwconfig stderr', value = stderr))
00193 self.stat.values.append(KeyValue(key = 'iwconfig stdout', value = stdout))
00194 else:
00195 self.stat.values += self._parse_info(stdout)
00196 except Exception, e:
00197 rospy.logerr("IwConfigLocal update exception: %s" %e)
00198 self.stat.values.append(KeyValue(key = 'update exception', value = str(e)))
00199
00200 class IwConfigSSH(IwConfigParser):
00201 def __init__(self, hostname, user, password):
00202 IwConfigParser.__init__(self)
00203 self.ssh = paramiko.SSHClient()
00204 self.ssh.load_system_host_keys()
00205 ssh_key_file = os.getenv("HOME")+'/.ssh/id_rsa.pub'
00206 self.ssh.set_missing_host_key_policy(paramiko.AutoAddPolicy())
00207 self.ssh.connect(str(hostname), username=user, key_filename=ssh_key_file)
00208
00209
00210 self.interfaces = []
00211 self.stat = DiagnosticStatus()
00212 self.stat.level = DiagnosticStatus.OK
00213 self.stat.message = "OK"
00214 self.stat.values = []
00215 try:
00216 (stdin, stdout, stderr) = self.ssh.exec_command("iw dev | awk '$1==\"Interface\"{print $2}'")
00217 output = ''.join(stdout.readlines())
00218 self.interfaces = sorted(os.linesep.join([s for s in output.splitlines() if s]).split('\n'))
00219 except Exception, e:
00220 rospy.logerr("IwConfigSSH init exception: %s" %e)
00221
00222 def update(self):
00223 self.stat.level = DiagnosticStatus.OK
00224 self.stat.message = "OK"
00225 self.stat.values = []
00226 for interface in self.interfaces:
00227 self.stat.values.append(KeyValue(key = str(interface), value = "======================="))
00228 try:
00229 (stdin, stdout, stderr) = self.ssh.exec_command("iwconfig %s"%interface)
00230
00231 output = ''.join(stdout.readlines())
00232 self.stat.values += self._parse_info(output)
00233 except Exception, e:
00234 rospy.logerr("IwConfigSSH update exception: %s" %e)
00235 self.stat.values.append(KeyValue(key = 'update exception', value = str(e)))
00236
00237 class WlanMonitor():
00238 def __init__(self):
00239 rospy.init_node("wlan_monitor")
00240 self.get_params()
00241
00242 self._wlan_stat = DiagnosticStatus()
00243 self._wlan_stat.name = '%s WLAN Info' % self.diag_hostname
00244 self._wlan_stat.hardware_id = self.diag_hostname
00245 self._wlan_stat.level = DiagnosticStatus.OK
00246 self._wlan_stat.message = 'No Data'
00247 self._wlan_stat.values = []
00248 self.msg = DiagnosticArray()
00249 self.msg.header.stamp = rospy.get_rostime()
00250 self.msg.status = [self._wlan_stat]
00251
00252 self.diag_pub = rospy.Publisher("/diagnostics", DiagnosticArray, queue_size=1)
00253 self.diag_timer = rospy.Timer(rospy.Duration(1.0), self.publish_diagnostics)
00254
00255 if self.monitor_local:
00256 self.iwconfig = IwConfigLocal()
00257 else:
00258 try:
00259 self.iwconfig = IwConfigSSH(self.diag_hostname, self.user, self.password)
00260 except Exception, e:
00261 rospy.logerr("Error connecting ssh to host: %s",e.message)
00262 self._wlan_stat.level = DiagnosticStatus.ERROR
00263 self._wlan_stat.message = 'iwconfig init error'
00264 self._wlan_stat.values = [ KeyValue(key = 'Exception', value = str(e)) ]
00265 self.msg.status = [self._wlan_stat]
00266 return
00267
00268 self.monitor_timer = rospy.Timer(rospy.Duration(1.0), self.update_diagnostics)
00269
00270 def update_diagnostics(self, event):
00271 self.iwconfig.update()
00272
00273 self.msg = DiagnosticArray()
00274 self.msg.header.stamp = rospy.get_rostime()
00275 self._wlan_stat.level = self.iwconfig.stat.level
00276 self._wlan_stat.message = self.iwconfig.stat.message
00277 self._wlan_stat.values = self.iwconfig.stat.values
00278 self.msg.status = [self._wlan_stat]
00279
00280 def publish_diagnostics(self, event):
00281 self.diag_pub.publish(self.msg)
00282
00283 def get_params(self):
00284 self.diag_hostname = rospy.get_param('~diag_hostname', "localhost")
00285 self.monitor_local = rospy.get_param("~monitor_local", True)
00286 self.user = rospy.get_param('~user', "")
00287 self.password = rospy.get_param('~password', "")
00288
00289 if __name__ == "__main__":
00290 monitor = WlanMonitor()
00291 rospy.spin()