00001
00002
00003 import roslib; roslib.load_manifest('mrp2_analyzer')
00004
00005 import rospy
00006 import std_msgs
00007 from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
00008 from std_msgs.msg import Bool
00009
00010 stall_l = False
00011 stall_r = False
00012 batt_low = False
00013 batt_high = False
00014 controller = False
00015 aux_lights = False
00016
00017 def motor_stall_l_callback(data):
00018 global stall_l
00019 stall_l = data.data
00020
00021 def motor_stall_r_callback(data):
00022 global stall_r
00023 stall_r = data.data
00024
00025 def batt_low_callback(data):
00026 global batt_low
00027 batt_low = data.data
00028
00029 def batt_high_callback(data):
00030 global batt_high
00031 batt_high = data.data
00032
00033 def controller_callback(data):
00034 global controller
00035 controller = data.data
00036
00037 def aux_lights_callback(data):
00038 global aux_lights
00039 aux_lights = data.data
00040
00041 if __name__ == '__main__':
00042 rospy.init_node('mrp2_diagnostics_publisher')
00043
00044 pub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size = 10)
00045
00046 rospy.Subscriber("hw_monitor/diagnostics/motor_stall_l", Bool, motor_stall_l_callback)
00047 rospy.Subscriber("hw_monitor/diagnostics/motor_stall_r", Bool, motor_stall_r_callback)
00048 rospy.Subscriber("hw_monitor/diagnostics/batt_low", Bool, batt_low_callback)
00049 rospy.Subscriber("hw_monitor/diagnostics/batt_high", Bool, batt_high_callback)
00050 rospy.Subscriber("hw_monitor/diagnostics/controller", Bool, controller_callback)
00051 rospy.Subscriber("hw_monitor/diagnostics/aux_lights", Bool, aux_lights_callback)
00052
00053 array = DiagnosticArray()
00054
00055 my_rate = rospy.Rate(1.0)
00056 while not rospy.is_shutdown():
00057
00058 motor_l_stat = DiagnosticStatus(name = 'Motor Left', level = 0, message = 'Running')
00059 motor_l_stat.values = [ KeyValue(key = 'Stall', value = 'False'), KeyValue(key = 'Controller Halt', value = 'False')]
00060
00061 motor_r_stat = DiagnosticStatus(name = 'Motor Right', level = 0, message = 'Running')
00062 motor_r_stat.values = [ KeyValue(key = 'Stall', value = 'False'), KeyValue(key = 'Controller Halt', value = 'False')]
00063
00064 if controller == True:
00065 motor_l_stat = DiagnosticStatus(name = 'Motor Left', level = 0, message = 'Halted')
00066 motor_l_stat.values = [ KeyValue(key = 'Stall', value = 'False'),KeyValue(key = 'Controller Halt', value = 'True')]
00067 motor_r_stat = DiagnosticStatus(name = 'Motor Right', level = 0, message = 'Halted')
00068 motor_r_stat.values = [ KeyValue(key = 'Stall', value = 'False'),KeyValue(key = 'Controller Halt', value = 'True')]
00069
00070 if stall_l == True:
00071 motor_l_stat = DiagnosticStatus(name = 'Motor Left', level = 0, message = 'Stalled')
00072 motor_l_stat.values = [ KeyValue(key = 'Stall', value = 'True'), KeyValue(key = 'Controller Halt', value = 'False')]
00073
00074 if stall_l == True & controller == True:
00075 motor_l_stat = DiagnosticStatus(name = 'Motor Left', level = 0, message = 'Stopped')
00076 motor_l_stat.values = [ KeyValue(key = 'Stall', value = 'True'), KeyValue(key = 'Controller Halt', value = 'True')]
00077
00078 if controller == True:
00079 motor_r_stat = DiagnosticStatus(name = 'Motor Right', level = 0, message = 'Halted')
00080 motor_r_stat.values = [ KeyValue(key = 'Stall', value = 'False'), KeyValue(key = 'Controller Halt', value = 'True')]
00081
00082 if stall_r == True:
00083 motor_r_stat = DiagnosticStatus(name = 'Motor Right', level = 0, message = 'Stalled')
00084 motor_r_stat.values = [ KeyValue(key = 'Stall', value = 'True'), KeyValue(key = 'Controller Halt', value = 'False')]
00085
00086 if stall_r == True & controller == True:
00087 motor_r_stat = DiagnosticStatus(name = 'Motor Right', level = 0, message = 'Stopped')
00088 motor_r_stat.values = [ KeyValue(key = 'Stall', value = 'True'), KeyValue(key = 'Controller Halt', value = 'True')]
00089
00090 battery_stat = DiagnosticStatus(name = 'Battery', level = 0, message = 'OK')
00091 battery_stat.values = [ KeyValue(key = 'Over Voltage', value = 'False'), KeyValue(key = 'Under Voltage', value = 'False')]
00092
00093 if batt_high == True:
00094 battery_stat = DiagnosticStatus(name = 'Battery', level = 0, message = 'Voltage error')
00095 battery_stat.values = [ KeyValue(key = 'Over Voltage', value = 'True'), KeyValue(key = 'Under Voltage', value = 'False')]
00096
00097 if batt_low == True:
00098 battery_stat = DiagnosticStatus(name = 'Battery', level = 0, message = 'Voltage error')
00099 battery_stat.values = [ KeyValue(key = 'Over Voltage', value = 'False'), KeyValue(key = 'Under Voltage', value = 'True')]
00100
00101 lights_stat = DiagnosticStatus(name = 'Lights', level = 0, message = 'OK')
00102 lights_stat.values = [ KeyValue(key = 'Controller Halt', value = 'False')]
00103
00104 if aux_lights == True:
00105 lights_stat = DiagnosticStatus(name = 'Lights', level = 0, message = 'Controller Error')
00106 lights_stat.values = [ KeyValue(key = 'Controller Halt', value = 'True')]
00107
00108 array.status = [ motor_l_stat, motor_r_stat, battery_stat, lights_stat ]
00109 pub.publish(array)
00110 my_rate.sleep()