husky_bringup: husky_bringup: diagnostics.py Source File

00001 #!/usr/bin/env python
00002 import roslib; roslib.load_manifest('husky_bringup')
00003 import rospy
00004 
00005 from clearpath_base.msg import SystemStatus, SafetyStatus, PowerStatus
00006 from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
00007 
00008 # Constants
00009 UNDERVOLT_ERROR = 18
00010 UNDERVOLT_WARN = 19
00011 OVERVOLT_ERROR = 30
00012 OVERVOLT_WARN = 29
00013 DRIVER_OVERTEMP_ERROR = 50
00014 DRIVER_OVERTEMP_WARN = 30
00015 MOTOR_OVERTEMP_ERROR = 80
00016 MOTOR_OVERTEMP_WARN = 70
00017 LOWPOWER_ERROR = 0.2
00018 LOWPOWER_WARN = 0.3
00019 SAFETY_TIMEOUT = 0x1
00020 SAFETY_LOCKOUT = 0x2
00021 SAFETY_ESTOP = 0x8
00022 SAFETY_CCI = 0x10
00023 SAFETY_PSU= 0x20
00024 SAFETY_CURRENT = 0x40
00025 SAFETY_WARN = (SAFETY_TIMEOUT | SAFETY_CCI | SAFETY_PSU)
00026 SAFETY_ERROR = (SAFETY_LOCKOUT | SAFETY_ESTOP | SAFETY_CURRENT)
00027 
00028 class HuskyDiagnostics():
00029     def __init__(self):
00030         rospy.init_node('husky_diagnostics')
00031         self.diag_pub = rospy.Publisher('/diagnostics', DiagnosticArray)
00032         self.last_diagnostics_time = rospy.get_rostime()        
00033         rospy.Subscriber('husky/data/system_status', SystemStatus, self.HandleSystemStatus)
00034         rospy.Subscriber('husky/data/safety_status', SafetyStatus, self.HandleSafetyStatus)
00035         rospy.Subscriber('husky/data/power_status', PowerStatus, self.HandlePowerStatus)
00036 
00037         self.stat_estop = [] 
00038         self.stat_power = []
00039         self.stat_voltage = []
00040         self.stat_temp = []
00041         self.stat_uptime = []
00042 
00043         # Should publish: Estop, charge, voltage, current, temperatures
00044 
00045     def _publish(self, time):
00046         """ Publishes diagnostic data"""
00047         # Limit to 1 Hz
00048         if (time - self.last_diagnostics_time).to_sec() < 1.0:
00049             return
00050         self.last_diagnostics_time = time
00051         diag = DiagnosticArray()
00052         diag.header.stamp = time
00053 
00054         # E-Stop
00055         if self.stat_estop:
00056             diag.status.append(self.stat_estop)
00057         # Power
00058         if self.stat_power:
00059             diag.status.append(self.stat_power)
00060         # Voltage
00061         if self.stat_voltage:
00062             diag.status.append(self.stat_voltage)
00063         # Temperature
00064         if self.stat_temp:
00065             diag.status.append(self.stat_temp)
00066         # Uptime
00067         if self.stat_uptime:
00068             diag.status.append(self.stat_uptime)
00069 
00070         # Publish
00071         print diag
00072         self.diag_pub.publish(diag)
00073 
00074     def HandleSystemStatus(self,data):
00075         # Uptime
00076         self.stat_uptime = DiagnosticStatus(name="Uptime",level=DiagnosticStatus.OK, message="Robot Online")
00077         uptime = data.uptime
00078         self.stat_uptime.values = [KeyValue("Uptime (ms)",str(uptime))]
00079     
00080         # Bus voltage
00081         bus_volt = data.voltages[0]
00082         self.stat_voltage = DiagnosticStatus(name="Voltage",level=DiagnosticStatus.OK, message="Voltage OK")
00083         if bus_volt > OVERVOLT_ERROR:
00084             self.stat_voltage.level = DiagnosticStatus.ERROR
00085             self.stat_voltage.message = "Main bus voltage too high"
00086         elif bus_volt > OVERVOLT_WARN:
00087             self.stat_voltage.level = DiagnosticStatus.WARN
00088             self.stat_voltage.message = "Main bus voltage too high"
00089         elif bus_volt < UNDERVOLT_ERROR:
00090             self.stat_voltage.level = DiagnosticStatus.ERROR
00091             self.stat_voltage.message = "Main bus voltage too low"
00092         elif bus_volt < UNDERVOLT_WARN:
00093             self.stat_voltage.level = DiagnosticStatus.WARN
00094             self.stat_voltage.message = "Main bus voltage too low"
00095 
00096         self.stat_voltage.values = [KeyValue("Bus voltage (V)",str(bus_volt))]
00097 
00098         # Temperature
00099         self.stat_temp = DiagnosticStatus(name="Temperature",level=DiagnosticStatus.OK, message="OK")
00100         left_drv = data.temperatures[0]
00101         right_drv = data.temperatures[1]
00102         left_mot = data.temperatures[2]
00103         right_mot = data.temperatures[3]
00104         if max(left_drv,right_drv) > DRIVER_OVERTEMP_ERROR:
00105             self.stat_temp.level = DiagnosticStatus.ERROR
00106             self.stat_temp.message = "Motor drivers too hot"
00107         elif max(left_drv,right_drv) > DRIVER_OVERTEMP_WARN:
00108             self.stat_temp.level = DiagnosticStatus.WARN
00109             self.stat_temp.message = "Motor drivers too hot"
00110         if max(left_mot,right_mot) > MOTOR_OVERTEMP_ERROR:
00111             self.stat_temp.level = DiagnosticStatus.ERROR
00112             self.stat_temp.message = "Motors too hot"
00113         elif max(left_mot,right_mot) > MOTOR_OVERTEMP_WARN:
00114             self.stat_temp.level = DiagnosticStatus.WARN
00115             self.stat_temp.message = "Motors too hot"
00116 
00117         self.stat_temp.values = [KeyValue("Left motor driver (C)",str(left_drv)),
00118                                  KeyValue("Right motor driver (C)",str(right_drv)),
00119                                  KeyValue("Left motor (C)",str(left_mot)),
00120                                  KeyValue("Right motor (C)",str(right_mot))]
00121 
00122         # Publish
00123         self._publish(data.header.stamp)
00124 
00125     def HandleSafetyStatus(self,data):
00126         # Safety
00127         self.stat_estop = DiagnosticStatus(name="Safety System",level=DiagnosticStatus.OK,
00128                                            message="OK")
00129         flags = data.flags        
00130 
00131         if flags & SAFETY_ERROR:
00132             self.stat_estop.level = DiagnosticStatus.ERROR
00133             self.stat_estop.message = "Error"
00134         elif flags & SAFETY_WARN:
00135             self.stat_estop.level = DiagnosticStatus.WARN
00136             self.stat_estop.message = "Warning"
00137 
00138         self.stat_estop.values = [KeyValue("Timeout",str(flags & SAFETY_TIMEOUT > 0)),
00139                                  KeyValue("Lockout",str(flags & SAFETY_LOCKOUT > 0)), 
00140                                  KeyValue("Emergency Stop",str(flags & SAFETY_ESTOP > 0)), 
00141                                  KeyValue("ROS Pause",str(flags & SAFETY_CCI > 0)), 
00142                                  KeyValue("No battery",str(flags & SAFETY_PSU > 0)), 
00143                                  KeyValue("Current limit",str(flags & SAFETY_CURRENT > 0))]
00144         
00145         # Publish
00146         self._publish(data.header.stamp)
00147 
00148     def HandlePowerStatus(self,data):
00149         # Charge
00150         self.stat_power = DiagnosticStatus(name="Power System",level=DiagnosticStatus.OK, message="OK")
00151         charge = data.sources[0].charge
00152         capacity = data.sources[0].capacity
00153         present = data.sources[0].present
00154         bat_type = data.sources[0].description
00155         if charge < LOWPOWER_WARN:
00156             self.stat_power.level = DiagnosticStatus.WARN
00157             self.stat_power.message = "Low power"
00158         if charge < LOWPOWER_ERROR:
00159             self.stat_power.level = DiagnosticStatus.ERROR 
00160         self.stat_power.values = [KeyValue("Charge (%)",str(charge)),
00161                                  KeyValue("Battery Capacity (Wh)",str(capacity))]
00162 
00163         # Publish
00164         self._publish(data.header.stamp)
00165 
00166 if __name__ == "__main__":
00167     obj = HuskyDiagnostics()
00168     try:
00169         rospy.spin()
00170     except rospy.ROSInterruptException:
00171         pass