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)