1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35 import rospy
36
37 from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
38
41 self.charging_state = {0:"Not Charging",
42 1:"Reconditioning Charging",
43 2:"Full Charging",
44 3:"Trickle Charging",
45 4:"Waiting",
46 5:"Charging Fault Condition"}
47 self.charging_source = {0:"None",
48 1:"Internal Charger",
49 2:"Base Dock"}
50 self.digital_outputs = {0:"OFF",
51 1:"ON"}
52 self.oi_mode = {1:"Passive",
53 2:"Safe",
54 3:"Full"}
55 self.diag_pub = rospy.Publisher('/diagnostics', DiagnosticArray)
56 self.last_diagnostics_time = rospy.get_rostime()
57
59 curr_time = rospy.Time.now()
60 diag = DiagnosticArray()
61 diag.header.stamp = curr_time
62 stat = DiagnosticStatus()
63 if status == "error":
64 stat = DiagnosticStatus(name="TurtleBot Node", level=DiagnosticStatus.ERROR, message=msg)
65 if status == "warn":
66 stat = DiagnosticStatus(name="TurtleBot Node", level=DiagnosticStatus.WARN, message=msg)
67 diag.status.append(stat)
68 self.diag_pub.publish(diag)
69
70
71 - def publish(self, sensor_state, gyro):
72 curr_time = sensor_state.header.stamp
73
74 if (curr_time - self.last_diagnostics_time).to_sec() < 0.2:
75 return
76 self.last_diagnostics_time = curr_time
77
78 diag = DiagnosticArray()
79 diag.header.stamp = curr_time
80 stat = DiagnosticStatus()
81
82
83 stat = DiagnosticStatus(name="TurtleBot Node", level=DiagnosticStatus.OK, message="RUNNING")
84 diag.status.append(stat)
85
86
87 stat = DiagnosticStatus(name="Operating Mode", level=DiagnosticStatus.OK)
88 try:
89 stat.message = self.oi_mode[sensor_state.oi_mode]
90 except KeyError as ex:
91 stat.level=DiagnosticStatus.ERROR
92 stat.message = "Invalid OI Mode Reported %s"%ex
93 rospy.logwarn(stat.message)
94 diag.status.append(stat)
95
96
97 stat = DiagnosticStatus(name="Battery", level=DiagnosticStatus.OK, message="OK")
98 values = stat.values
99 if sensor_state.charging_state == 5:
100 stat.level = DiagnosticStatus.ERROR
101 stat.message = "Charging Fault Condition"
102 values.append(KeyValue("Charging State", self.charging_state[sensor_state.charging_state]))
103 values.extend([KeyValue("Voltage (V)", str(sensor_state.voltage/1000.0)),
104 KeyValue("Current (A)", str(sensor_state.current/1000.0)),
105 KeyValue("Temperature (C)",str(sensor_state.temperature)),
106 KeyValue("Charge (Ah)", str(sensor_state.charge/1000.0)),
107 KeyValue("Capacity (Ah)", str(sensor_state.capacity/1000.0))])
108 diag.status.append(stat)
109
110
111 stat = DiagnosticStatus(name="Charging Sources", level=DiagnosticStatus.OK)
112 try:
113 stat.message = self.charging_source[sensor_state.charging_sources_available]
114 except KeyError as ex:
115 stat.level=DiagnosticStatus.ERROR
116 stat.message = "Invalid Charging Source %s, actual value: %i"%(ex,sensor_state.charging_sources_available)
117 rospy.logwarn(stat.message)
118 diag.status.append(stat)
119
120 stat = DiagnosticStatus(name="Cliff Sensor", level=DiagnosticStatus.OK, message="OK")
121 if sensor_state.cliff_left or sensor_state.cliff_front_left or sensor_state.cliff_right or sensor_state.cliff_front_right:
122 stat.level = DiagnosticStatus.WARN
123 if (sensor_state.current/1000.0)>0:
124 stat.message = "Near Cliff: While the irobot create is charging, the create thinks it's near a cliff."
125 stat.level = DiagnosticStatus.OK
126 else:
127 stat.message = "Near Cliff"
128 stat.values = [KeyValue("Left", str(sensor_state.cliff_left)),
129 KeyValue("Left Signal", str(sensor_state.cliff_left_signal)),
130 KeyValue("Front Left", str(sensor_state.cliff_front_left)),
131 KeyValue("Front Left Signal", str(sensor_state.cliff_front_left_signal)),
132 KeyValue("Front Right", str(sensor_state.cliff_right)),
133 KeyValue("Front Right Signal", str(sensor_state.cliff_right_signal)),
134 KeyValue("Right", str(sensor_state.cliff_front_right)),
135 KeyValue("Right Signal", str(sensor_state.cliff_front_right_signal))]
136 diag.status.append(stat)
137
138 stat = DiagnosticStatus(name="Wall Sensor", level=DiagnosticStatus.OK, message="OK")
139
140 if sensor_state.wall:
141 stat.level = DiagnosticStatus.ERROR
142 stat.message = "Near Wall"
143 stat.values = [KeyValue("Wall", str(sensor_state.wall)),
144 KeyValue("Wall Signal", str(sensor_state.wall_signal)),
145 KeyValue("Virtual Wall", str(sensor_state.virtual_wall))]
146 diag.status.append(stat)
147
148 stat = DiagnosticStatus(name="Gyro Sensor", level = DiagnosticStatus.OK, message = "OK")
149 if gyro is None:
150 stat.level = DiagnosticStatus.WARN
151 stat.message = "Gyro Not Enabled: To enable the gyro set the has_gyro param in the turtlebot_node."
152 elif gyro.cal_offset < 100:
153 stat.level = DiagnosticStatus.ERROR
154 stat.message = "Gyro Power Problem: For more information visit http://answers.ros.org/question/2091/turtlebot-bad-gyro-calibration-also."
155 elif gyro.cal_offset > 575.0 or gyro.cal_offset < 425.0:
156 stat.level = DiagnosticStatus.ERROR
157 stat.message = "Bad Gyro Calibration Offset: The gyro average is outside the standard deviation."
158
159 if gyro is not None:
160 stat.values = [KeyValue("Gyro Enabled", str(gyro is not None)),
161 KeyValue("Raw Gyro Rate", str(sensor_state.user_analog_input)),
162 KeyValue("Calibration Offset", str(gyro.cal_offset)),
163 KeyValue("Calibration Buffer", str(gyro.cal_buffer))]
164 diag.status.append(stat)
165
166 stat = DiagnosticStatus(name="Digital Outputs", level = DiagnosticStatus.OK, message = "OK")
167 out_byte = sensor_state.user_digital_outputs
168 stat.values = [KeyValue("Raw Byte", str(out_byte)),
169 KeyValue("Digital Out 2", self.digital_outputs[out_byte%2]),
170 KeyValue("Digital Out 1", self.digital_outputs[(out_byte >>1)%2]),
171 KeyValue("Digital Out 0", self.digital_outputs[(out_byte >>2)%2])]
172 diag.status.append(stat)
173
174 self.diag_pub.publish(diag)
175