Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 from __future__ import with_statement, division
00036
00037 import roslib; roslib.load_manifest('diagnostic_common_diagnostics')
00038 import rospy
00039 import diagnostic_updater as DIAG
00040
00041 import socket, string
00042 import subprocess
00043 import math
00044 import re
00045 from StringIO import StringIO
00046
00047 class Sensor(object):
00048 def __init__(self):
00049 self.critical = None
00050 self.min = None
00051 self.max = None
00052 self.input = None
00053 self.name = None
00054 self.type = None
00055 self.high = None
00056 self.alarm = None
00057
00058 def getCrit(self):
00059 return self.critical
00060
00061 def getMin(self):
00062 return self.min
00063
00064 def getMax(self):
00065 return self.max
00066
00067 def getInput(self):
00068 return self.input
00069
00070 def getName(self):
00071 return self.name
00072
00073 def getType(self):
00074 return self.type
00075
00076 def getHigh(self):
00077 return self.high
00078
00079 def getAlarm(self):
00080 return self.alarm
00081
00082 def __str__(self):
00083 lines = []
00084 lines.append(str(self.name))
00085 lines.append("\t" + "Type: " + str(self.type))
00086 if self.input:
00087 lines.append("\t" + "Input: " + str(self.input))
00088 if self.min:
00089 lines.append("\t" + "Min: " + str(self.min))
00090 if self.max:
00091 lines.append("\t" + "Max: " + str(self.max))
00092 if self.high:
00093 lines.append("\t" + "High: " + str(self.high))
00094 if self.critical:
00095 lines.append("\t" + "Crit: " + str(self.critical))
00096 lines.append("\t" + "Alarm: " + str(self.alarm))
00097 return "\n".join(lines)
00098
00099
00100 def parse_sensor_line(line):
00101 sensor = Sensor()
00102 line = line.lstrip()
00103 [name, reading] = line.split(":")
00104
00105
00106 if name.find("temp") != -1:
00107 return None
00108 else:
00109 [sensor.name, sensor.type] = name.rsplit(" ",1)
00110
00111 if sensor.name == "Core":
00112 sensor.name = name
00113 sensor.type = "Temperature"
00114 elif sensor.name.find("Physical id") != -1:
00115 sensor.name = name
00116 sensor.type = "Temperature"
00117
00118 [reading, params] = reading.lstrip().split("(")
00119
00120 sensor.alarm = False
00121 if line.find("ALARM") != -1:
00122 sensor.alarm = True
00123
00124 if reading.find("\xc2\xb0C") == -1:
00125 sensor.input = float(reading.split()[0])
00126 else:
00127 sensor.input = float(reading.split("\xc2\xb0C")[0])
00128
00129 params = params.split(",")
00130 for param in params:
00131 m = re.search("[0-9]+.[0-9]*", param)
00132 if param.find("min") != -1:
00133 sensor.min = float(m.group(0))
00134 elif param.find("max") != -1:
00135 sensor.max = float(m.group(0))
00136 elif param.find("high") != -1:
00137 sensor.high = float(m.group(0))
00138 elif param.find("crit") != -1:
00139 sensor.critical = float(m.group(0))
00140
00141 return sensor
00142
00143 def _rads_to_rpm(rads):
00144 return rads / (2 * math.pi) * 60
00145
00146 def _rpm_to_rads(rpm):
00147 return rpm * (2 * math.pi) / 60
00148
00149 def parse_sensors_output(output):
00150 out = StringIO(output)
00151
00152 sensorList = []
00153 for line in out.readlines():
00154
00155 if line.find(":") != -1 and line.find("Adapter") == -1:
00156 s = parse_sensor_line(line)
00157 if s is not None:
00158 sensorList.append(s)
00159 return sensorList
00160
00161 def get_sensors():
00162 p = subprocess.Popen('sensors', stdout = subprocess.PIPE,
00163 stderr = subprocess.PIPE, shell = True)
00164 (o,e) = p.communicate()
00165 if not p.returncode == 0:
00166 return ''
00167 if not o:
00168 return ''
00169 return o
00170
00171 class SensorsMonitor(object):
00172 def __init__(self, hostname):
00173 self.hostname = hostname
00174 self.ignore_fans = rospy.get_param('~ignore_fans', False)
00175 rospy.loginfo("Ignore fanspeed warnings: %s" % self.ignore_fans)
00176
00177 self.updater = DIAG.Updater()
00178 self.updater.setHardwareID("none")
00179 self.updater.add('%s Sensor Status'%self.hostname, self.monitor)
00180
00181 self.timer = rospy.Timer(rospy.Duration(1), self.timer_cb)
00182
00183 def timer_cb(self, dummy):
00184 self.updater.update()
00185
00186 def monitor(self, stat):
00187 try:
00188 stat.summary(DIAG.OK, "OK")
00189 for sensor in parse_sensors_output(get_sensors()):
00190 if sensor.getType() == "Temperature":
00191 if sensor.getInput() > sensor.getCrit():
00192 stat.mergeSummary(DIAG.ERROR, "Critical Temperature")
00193 elif sensor.getInput() > sensor.getHigh():
00194 stat.mergeSummary(DIAG.WARN, "High Temperature")
00195 stat.add(" ".join([sensor.getName(), sensor.getType()]), sensor.getInput())
00196 elif sensor.getType() == "Voltage":
00197 if sensor.getInput() < sensor.getMin():
00198 stat.mergeSummary(DIAG.ERROR, "Low Voltage")
00199 elif sensor.getInput() > sensor.getMax():
00200 stat.mergeSummary(DIAG.ERROR, "High Voltage")
00201 stat.add(" ".join([sensor.getName(), sensor.getType()]), sensor.getInput())
00202 elif sensor.getType() == "Speed":
00203 if not ignore_fans:
00204 if sensor.getInput() < sensor.getMin():
00205 stat.mergeSummary(DIAG.ERROR, "No Fan Speed")
00206 stat.add(" ".join([sensor.getName(), sensor.getType()]), sensor.getInput())
00207 except Exception, e:
00208 import traceback
00209 rospy.logerr('Unable to process lm-sensors data')
00210 rospy.logerr(traceback.format_exc())
00211 return stat
00212
00213 if __name__ == '__main__':
00214 hostname = socket.gethostname()
00215 hostname_clean = string.translate(hostname, string.maketrans('-','_'))
00216 try:
00217 rospy.init_node('sensors_monitor_%s'%hostname_clean)
00218 except rospy.ROSInitException:
00219 print >> sys.stderr, 'Unable to initialize node. Master may not be running'
00220 sys.exit(0)
00221
00222 monitor = SensorsMonitor(hostname)
00223 rospy.spin()