def filter_diagnostics_agg(self, diagnostics_agg):
diagnostics_agg_error_only = DiagnosticArray()
diagnostics_agg_error_only.header = diagnostics_agg.header
for status in diagnostics_agg.status:
if status.level != DiagnosticStatus.OK:
diagnostics_agg_error_only.status.append(status)
return diagnostics_agg_error_only
def publish_diagnostics(self, event):
"""Generate a diagnostic message of device statuses, voltage levels,
and limit switch triggers. Not critical code.
"""
with self.lock:
diagnostic_arr = DiagnosticArray(
) # create object to hold data to publish
# Generate a ROS message header for time stamping
header = Header()
header.stamp = rospy.Time.now()
diagnostic_arr.header = header
# For all the devices, set the status based on Pololu serial variables
# and append these statues to the diagnostic_arr status field
for i in range(len(self.devices)):
dev = self.devices[i]
name = self.dev_names[i]
status = DiagnosticStatus()
status.name = name
status.hardware_id = str(self.devices[i].dev_num)
errstat = dev.get_error_status()
sererr = dev.get_serial_errors()
if errstat == "OK" and sererr == "OK":
status.level = 0
status.message = errstat + "\n" + sererr
else:
status.level = 1
status.message = errstat + "\n" + sererr
# Get voltage level and set error status if it is too high or low
voltage_level = dev.get_input_voltage()
if voltage_level > 30 or voltage_level < 20:
status.level = 2
status.message = "Voltage out of range: %.2f" % (
voltage_level)
status.values.append(
KeyValue("Voltage", "%.2f" % voltage_level))
status.values.append(KeyValue("Speed", str(dev.get_speed())))
diagnostic_arr.status.append(status)
self.diagnostic_pub.publish(diagnostic_arr)
def publish_diagnostics_info(self):
"""
Publishes at a rate of 1Hz ( because thats the rate real kobuki does it )
For this first version we only publish battery status.
:return:
"""
battery_charge = self.get_simulated_battery_status()
msg = DiagnosticArray()
info_msg = DiagnosticStatus()
header = Header()
header.frame_id = ""
header.stamp = rospy.Time.now()
msg.header = header
msg.status.append(info_msg)
values = []
percent_value = KeyValue()
percent_value.key = "Percent"
percent_value.value = str(battery_charge)
# TODO: Add all the other values
voltage_value = KeyValue()
voltage_value.key = "Voltage (V)"
charge_value = KeyValue()
charge_value.key = "Charge (Ah)"
capacity_value = KeyValue()
capacity_value.key = "Capacity (Ah)"
source_value = KeyValue()
source_value.key = "Source"
charging_state_value = KeyValue()
charging_state_value.key = "Charging State"
current_value = KeyValue()
current_value.key = "Current (A)"
if battery_charge <= 10.0:
level = DiagnosticStatus.ERROR
message = "Empty Battery"
elif 10.0 < battery_charge <= 20.0:
level = DiagnosticStatus.WARN
message = "Warning Low Battery"
elif battery_charge >= 100.0:
level = DiagnosticStatus.OK
message = "Maximum"
else:
level = DiagnosticStatus.OK
message = "NormalLevel"
info_msg.name = "mobile_base_nodelet_manager: Battery"
info_msg.hardware_id = "Kobuki"
info_msg.message = message
info_msg.level = level
values.append(percent_value)
values.append(voltage_value)
values.append(charge_value)
values.append(capacity_value)
values.append(source_value)
values.append(charging_state_value)
values.append(current_value)
info_msg.values = values
msg.status.append(info_msg)
self._pub.publish(msg)
