def diagnosticsCallback (event): array = DiagnosticArray() # Set the timestamp for diagnostics array.header.stamp = rospy.Time.now() motors_message = DiagnosticStatus(name = 'PhidgetMotors', level = 0,message = 'initialized', hardware_id='Phidget') if (motorsError == 1): motors_message.level = 2 motors_message.message = "Phidget Motor controller can't be initialized" motors_message.values = [ KeyValue(key = 'Recommendation', value = motorControllerDisconnected)] if (motorsError == 2): motors_message.level = 2 motors_message.message = "Can't set up motor speed" motors_message.values = [ KeyValue(key = 'Recommendation', value = motorSpeedError)] encoders_message = DiagnosticStatus(name = 'PhidgetEncoders', level = 0,message = 'initialized', hardware_id='Phidget') if (encodersError == 1): encoders_message.level = 2 encoders_message.message = "Phidget Encoder board can't be initialized" encoders_message.values = [ KeyValue(key = 'Recommendation', value = encoderBoardDisconnected)] if (encodersError == 2): encoders_message.level = 2 encoders_message.message = "Can't get encoder value" encoders_message.values = [ KeyValue(key = 'Recommendation', value = encoderValueError)] array.status = [ motors_message, encoders_message ] diagnosticPub.publish(array)
def update_diagnostics(self, event=None): stat = DiagnosticStatus() stat.level = DiagnosticStatus.WARN stat.name = '%s NTP Offset' % self.diag_hostname stat.message = 'No Data' stat.hardware_id = self.diag_hostname stat.values = [] try: for st, host, off in [(stat, self.ntp_server, self.offset)]: p = Popen(["ntpdate", "-q", host], stdout=PIPE, stdin=PIPE, stderr=PIPE) retcode = p.wait() stdout, stderr = p.communicate() try: stdout = stdout.decode() #python3 except (UnicodeDecodeError, AttributeError): pass if retcode != 0: st.level = DiagnosticStatus.ERROR st.message = 'ntpdate Error' st.values = [ KeyValue(key='ntpdate Error', value=stderr), KeyValue(key='Output', value=stdout) ] continue measured_offset = float( re.search("offset (.*),", stdout).group(1)) * 1000000 st.level = DiagnosticStatus.OK st.message = "OK" st.values = [ KeyValue("NTP Server", self.ntp_server), KeyValue("Offset (us)", str(measured_offset)), KeyValue("Offset tolerance (us)", str(off)), KeyValue("Offset tolerance (us) for Error", str(self.error_offset)) ] if (abs(measured_offset) > off): st.level = DiagnosticStatus.WARN st.message = "NTP Offset Too High" if (abs(measured_offset) > self.error_offset): st.level = DiagnosticStatus.ERROR st.message = "NTP Offset Too High" except Exception as e: stat.level = DiagnosticStatus.ERROR stat.message = 'ntpdate Exception' stat.values = [KeyValue(key='Exception', value=str(e))] self.msg = DiagnosticArray() self.msg.header.stamp = rospy.get_rostime() self.msg.status = [stat]
def ntp_monitor(ntp_hostname, offset=500, self_offset=500): try: offset = int(offset) self_offset = int(self_offset) except: parser.error("offset must be a number") pub = rospy.Publisher("/diagnostics", DiagnosticArray) rospy.init_node(NAME, anonymous=True) hostname = socket.gethostname() stat = DiagnosticStatus() stat.level = 0 stat.name = "NTP offset from: "+ hostname + " to: " +ntp_hostname stat.message = "Acceptable synchronization" stat.hardware_id = hostname stat.values = [] self_stat = DiagnosticStatus() self_stat.level = 0 self_stat.name = "NTP offset from: "+ hostname + " to: self." self_stat.message = "Acceptable synchronization" self_stat.hardware_id = hostname self_stat.values = [] while not rospy.is_shutdown(): for st,host,off in [(stat,ntp_hostname,offset), (self_stat, hostname,self_offset)]: try: p = Popen(["ntpdate", "-q", host], stdout=PIPE, stdin=PIPE, stderr=PIPE) res = p.wait() (o,e) = p.communicate() except OSError, (errno, msg): if errno == 4: break #ctrl-c interrupt else: raise if (res == 0): measured_offset = float(re.search("offset (.*),", o).group(1))*1000000 st.level = 0 st.message = "Acceptable synchronization" st.values = [ KeyValue("offset (us)", str(measured_offset)) ] if (abs(measured_offset) > off): st.level = 2 st.message = "Offset too great" else: st.level = 2 st.message = "Error running ntpupdate" st.values = [ KeyValue("offset (us)", "N/A") ] pub.publish(DiagnosticArray(None, [stat, self_stat])) time.sleep(1)
def status_cb(msg): global BATTERY_STATES global diag_publisher diag_msg = DiagnosticArray() diag_msg.header.stamp = rospy.Time.now() diag_msg.status = [] temp_status = DiagnosticStatus() temp_status.name = "Chassis Temperature" temp_status.hardware_id = "automow_pcb" temp_status.values = [] top_F = msg.temperature_1 * 9/5 + 32 bot_F = msg.temperature_2 * 9/5 + 32 temp_status.values.append(KeyValue(key="Top Celsius", value="%3.2f C"%msg.temperature_1)) temp_status.values.append(KeyValue(key="Bottom Celsius", value="%3.2f C"%msg.temperature_2)) temp_status.values.append(KeyValue(key="Top Fahrenheit", value="%3.2f F"%(top_F))) temp_status.values.append(KeyValue(key="Bottom Fahrenheit", value="%3.2f F"%(bot_F))) if top_F > 100 or bot_F > 100: temp_status.message = "High Temperature" temp_status.level = temp_status.WARN elif top_F > 125 or bot_F > 125: temp_status.message = "Critical Temperature" temp_status.level = temp_status.ERROR else: temp_status.message = "OK" temp_status.level = temp_status.OK diag_msg.status.append(temp_status) batt_status = DiagnosticStatus() batt_status.name = "Battery Status" batt_status.hardware_id = "automow_pcb" batt_status.values = [] state = BATTERY_STATES[msg.battery_state] batt_status.values.append(KeyValue(key="State", value=state)) batt_status.values.append(KeyValue(key="Charge", value="{:.0%}".format(msg.charge/100.0))) batt_status.values.append(KeyValue(key="Voltage", value="%3.2f V"%(msg.voltage/1000.0))) batt_status.values.append(KeyValue(key="Battery Current", value="%3.2f A"%(msg.current/1000.0))) diag_msg.status.append(batt_status) if msg.battery_state >= 4: batt_status.level = batt_status.ERROR else: batt_status.level = batt_status.OK batt_status.message = state diag_publisher.publish(diag_msg)
def battery_status(self): stat = DiagnosticStatus(name="Battery", level=DiagnosticStatus.OK, message="OK") stat.values = [ KeyValue("Voltage avg(Vin)", str(self.robot.get_avg_voltageIn())), KeyValue("Voltage max(Vin)", str(self.robot.get_max_voltageIn())), KeyValue("Voltage min(Vin)", str(self.robot.get_min_voltageIn())), KeyValue("Current avg(A)", str(self.robot.get_avg_current())), KeyValue("Current max(A)", str(self.robot.get_avg_current())), KeyValue("Current min(A)", str(self.robot.get_avg_current())), KeyValue("Voltage avg(V5V)", str(self.robot.get_avg_5voltage())), KeyValue("Voltage max(V5V)", str(self.robot.get_max_5voltage())), KeyValue("Voltage min(V5V)", str(self.robot.get_min_5voltage())), KeyValue("Voltage avg(V3.3)", str(self.robot.get_avg_3voltage())), KeyValue("Voltage max(V3.3)", str(self.robot.get_max_3voltage())), KeyValue("Voltage min(V3.3)", str(self.robot.get_min_3voltage()))] if self.robot.get_status().VoltageLow == True: stat.level = DiagnosticStatus.WARN stat.message = "Voltage too low" if self.robot.get_status().CurrentError == True: stat.level = DiagnosticStatus.WARN stat.message = "Current error" if self.robot.get_status().Voltage3v3Low == True: stat.level = DiagnosticStatus.WARN stat.message = "Voltage3.3 too low" return stat
def _camera_diag(level = 0): array = DiagnosticArray() stat = DiagnosticStatus() stat.name = 'wge100: Driver Status' stat.level = level stat.message = 'OK' motor_stat = DiagnosticStatus() motor_stat.name = 'EtherCAT Master' motor_stat.level = 0 motor_stat.values = [ KeyValue(key='Dropped Packets', value='0'), KeyValue(key='RX Late Packet', value='0')] mcb_stat = DiagnosticStatus() mcb_stat.name = 'EtherCAT Device (my_motor)' mcb_stat.level = 0 mcb_stat.values.append(KeyValue('Num encoder_errors', '0')) array.header.stamp = rospy.get_rostime() array.status.append(stat) array.status.append(motor_stat) array.status.append(mcb_stat) return array
def fill_diags(name, summary, hid, diags): ds = DiagnosticStatus() ds.values = [KeyValue(k, str(v)) for (k, v) in diags] ds.hardware_id = hid ds.name = rospy.get_caller_id().lstrip('/') + ": " + name ds.message = summary return ds
def info_status(self): stat = DiagnosticStatus(name="Info_Platform", level=DiagnosticStatus.OK, message="OK") stat.values = [ KeyValue("", str(self.robot.get_status()[0])), KeyValue("", str(self.robot.get_status()[1])), KeyValue("", str(self.robot.get_status()[2]))] return stat
def _publish_diagnostics(self): pilot = self._get_pilot() if pilot: status = DiagnosticStatus(name='mode', message='not_available') status.values = [ KeyValue(key='steer', value='{:+2.5f}'.format(pilot.get('steering'))), KeyValue(key='throttle', value='{:+2.5f}'.format(pilot.get('throttle'))) ] driver_mode = pilot.get('driver') if driver_mode == 'driver_mode.teleop.direct': status.message = 'teleoperation' elif driver_mode == 'driver_mode.inference.dnn': status.message = 'autopilot' status.values += [ KeyValue(key='maximum_speed', value='{:+2.5f}'.format( pilot.get('cruise_speed'))), KeyValue(key='desired_speed', value='{:+2.5f}'.format( pilot.get('desired_speed'))) ] diagnostics = DiagnosticArray() diagnostics.header.stamp = self.get_clock().now().to_msg() diagnostics.status = [status] self._ros_publisher.publish(diagnostics)
def _on_packet_pong(self, packet): """ Re-publishes the pong responses as a diagnostic message. """ try: packet_dict = self.get_packet_dict(packet) if not packet_dict: return except (ValueError, TypeError) as e: return self.last_pong = time.time() self.last_pong_dt = datetime.now() total = packet_dict['total'] #print('pong total:', total) array = DiagnosticArray() pong_status = DiagnosticStatus(name='%s Arduino' % self.name.title(), level=OK) pong_status.values = [ KeyValue(key='total', value=str(total)), ] array.status = [ pong_status, ] self.diagnostics_pub.publish(array)
def speed_status(self): stat = DiagnosticStatus(name="Speed", level=DiagnosticStatus.OK, message="OK") stat.values = [ KeyValue("Linear speed (Vx)", str(self.robot.get_movesteer(None)[0])), KeyValue("Angular speed (Vth)", str(self.robot.get_movesteer(None)[2]))] if self.robot.get_movesteer(None)[0] > self.speedLimit: stat.level = DiagnosticStatus.WARN stat.message = "speed is too high" return stat
def connection_status(self): stat = DiagnosticStatus(name="Connection", level=DiagnosticStatus.OK, message="OK") stat.values = [ KeyValue("Baudrate", str(self.robot.get_connection_info()["baudrate"])), KeyValue("Comport", str(self.robot.get_connection_info()["comport"]))] if self.robot.is_connected() == False: stat.message = "disconnected" stat.level = DiagnosticStatus.ERROR return stat
def cb_diagnostics_update(self): tabpage_status = DiagnosticStatus() tabpage_status.level = tabpage_status.OK tabpage_status.name = self.handler_name + " Handler" tabpage_status.hardware_id = self.ros_name tabpage_status.message = "OK" tabpage_status.values = [] tabpage_status.values.append(KeyValue(key="Number of Clients", value=str(len(self.osc_clients)))) return tabpage_status
def ntp_monitor(ntp_hostname, offset=500, self_offset=500, diag_hostname=None, error_offset=5000000, do_self_test=True): pub = rospy.Publisher("/diagnostics", DiagnosticArray) rospy.init_node(NAME, anonymous=True) hostname = socket.gethostname() if diag_hostname is None: diag_hostname = hostname stat = DiagnosticStatus() stat.level = DiagnosticStatus.OK stat.name = "NTP offset from " + diag_hostname + " to " + ntp_hostname stat.message = "OK" stat.hardware_id = hostname stat.values = [] self_stat = DiagnosticStatus() self_stat.level = DiagnosticStatus.OK self_stat.name = "NTP self-offset for " + diag_hostname self_stat.message = "OK" self_stat.hardware_id = hostname self_stat.values = [] while not rospy.is_shutdown(): msg = DiagnosticArray() msg.header.stamp = rospy.get_rostime() st = ntp_diag(stat, ntp_hostname, offset, error_offset) if st is not None: msg.status.append(st) if do_self_test: st = ntp_diag(self_stat, hostname, self_offset, error_offset) if st is not None: msg.status.append(st) pub.publish(msg) time.sleep(1)
def get_diagnostics_message(self): message = DiagnosticStatus() message.name = "ODrive report" message.message = "ODrive diagnostics" message.hardware_id = self.serial_no if self.device is None: message.level = 1 message.values = [KeyValue("connected", str(False))] else: message.values = [ KeyValue("connected", str(True)), KeyValue("bus-voltage", str(self.device.vbus_voltage)), KeyValue("axis0-error", oenums.errors.axis[self.device.axis0.error]), KeyValue( "axis0-encoder-error", oenums.errors.encoder[self.device.axis0.encoder.error]), KeyValue("axis0-motor-error", oenums.errors.motor[self.device.axis0.motor.error]), # KeyValue("axis0-controller-error", oenums.errors.controller[self.device.axis0.controller.error]), KeyValue( "axis0-ctrl-mode", oenums.control_modes[ self.device.axis0.controller.config.control_mode]), KeyValue("axis0-state", oenums.axis_states[self.device.axis0.current_state]), KeyValue("axis1-error", oenums.errors.axis[self.device.axis1.error]), KeyValue( "axis1-encoder-error", oenums.errors.encoder[self.device.axis1.encoder.error]), KeyValue("axis1-motor-error", oenums.errors.motor[self.device.axis1.motor.error]), # KeyValue("axis1-controller-error", oenums.errors.controller[self.device.axis1.controller.error]), KeyValue( "axis1-ctrl-mode", oenums.control_modes[ self.device.axis1.controller.config.control_mode]), KeyValue("axis1-state", oenums.axis_states[self.device.axis1.current_state]), ] # print(repr(message)) return message
def cb_diagnostics_update(self): tabpage_status = DiagnosticStatus() tabpage_status.level = tabpage_status.OK tabpage_status.name = self.handler_name + " Handler" tabpage_status.hardware_id = self.ros_name tabpage_status.message = "OK" tabpage_status.values = [] tabpage_status.values.append( KeyValue(key="Number of Clients", value=str(len(self.osc_clients)))) return tabpage_status
def publish_motor_status(self, event=None): status = self.motor.get_status() data_list = [] for key in status: data_list.append(KeyValue(key, str(status[key]))) msg = DiagnosticStatus() msg.values = data_list self.motor_status_pub.publish(msg)
def get_diagnostic(self): diag_msg = DiagnosticStatus() sys_status, gyro_status, accel_status, mag_status = self.bno.get_calibration_status( ) sys = KeyValue("System status", str(sys_status)) gyro = KeyValue("Gyro calibration status", str(gyro_status)) accel = KeyValue("Accelerometer calibration status", str(accel_status)) mag = KeyValue("Magnetometer calibration status", str(mag_status)) diag_msg.values = [sys, gyro, accel, mag] return diag_msg
def make_status_msg(count): array = DiagnosticArray() stat = DiagnosticStatus() stat.level = 0 stat.message = 'OK' stat.name = 'Unit Test' stat.hardware_id = 'HW ID' stat.values = [ KeyValue('Value A', str(count)), KeyValue('Value B', str(count)), KeyValue('Value C', str(count))] array.status = [ stat ] return array
def joint_to_diag(js): global has_warned_invalid ds = DiagnosticStatus() ds.level = DiagnosticStatus.OK ds.message = 'OK' # Hack to stop gripper joints from being "uncalibrated" if not js.is_calibrated and js.name.find("float") < 0 and js.name.find( "finger") < 0: ds.level = DiagnosticStatus.WARN ds.message = 'Uncalibrated' if check_nan and (math.isnan(js.position) or math.isnan(js.velocity) or math.isnan(js.measured_effort) or math.isnan(js.commanded_effort)): ds.level = DiagnosticStatus.ERROR ds.message = 'NaN in joint data' if not has_warned_invalid: rospy.logerr( "NaN value for joint data. controller_manager restart required." ) has_warned_invalid = True if check_nan and (math.isinf(js.position) or math.isinf(js.velocity) or math.isinf(js.measured_effort) or math.isinf(js.commanded_effort)): ds.level = DiagnosticStatus.ERROR ds.message = 'Inf in joint data' if not has_warned_invalid: rospy.logerr( "Infinite value for joint data. controller_manager restart required." ) has_warned_invalid = True ds.name = "Joint (%s)" % js.name ds.values = [ KeyValue('Position', str(js.position)), KeyValue('Velocity', str(js.velocity)), KeyValue('Measured Effort', str(js.measured_effort)), KeyValue('Commanded Effort', str(js.commanded_effort)), KeyValue('Calibrated', str(js.is_calibrated)), KeyValue('Odometer', str(js.odometer)), KeyValue('Max Position', str(js.max_position)), KeyValue('Min Position', str(js.min_position)), KeyValue('Max Abs. Velocity', str(js.max_abs_velocity)), KeyValue('Max Abs. Effort', str(js.max_abs_effort)), KeyValue('Limits Hit', str(js.violated_limits)) ] return ds
def odom_cb(self, msg): with self.lock: dist = msg.distance + (msg.angle * 0.25) # check if base moved if dist > self.dist + EPS: self.start_time = rospy.Time.now() self.start_angle = self.last_angle self.dist = dist # do imu test if possible if rospy.Time.now() > self.start_time + rospy.Duration(10.0): self.drift = fabs(self.start_angle - self.last_angle) * 180 / (pi * 10) self.start_time = rospy.Time.now() self.start_angle = self.last_angle self.last_measured = rospy.Time.now() # publish diagnostics d = DiagnosticArray() d.header.stamp = rospy.Time.now() ds = DiagnosticStatus() ds.name = "imu_node: Imu Drift Monitor" if self.drift < 0.5: ds.level = DiagnosticStatus.OK ds.message = 'OK' elif self.drift < 1.0: ds.level = DiagnosticStatus.WARN ds.message = 'Drifting' else: ds.level = DiagnosticStatus.ERROR ds.message = 'Drifting' drift = self.drift if self.drift < 0: last_measured = 'No measurements yet, waiting for base to stop moving before measuring' drift = 'N/A' else: age = (rospy.Time.now() - self.last_measured).to_sec() if age < 60: last_measured = '%f seconds ago' % age elif age < 3600: last_measured = '%f minutes ago' % (age / 60) else: last_measured = '%f hours ago' % (age / 3600) ds.values = [ KeyValue('Last measured', last_measured), KeyValue('Drift (deg/sec)', str(drift)) ] d.status = [ds] self.pub_diag.publish(d)
def update(self): msg = DiagnosticArray() msg.header.stamp = rospy.Time.now() msg.status = [] it = 0 while it < 20: status = DiagnosticStatus() status.level = random.randint(0,2) status.name = "Test %i"%it status.hardware_id = "Dummy Diagnostics" if status.level == 0: message = "OK" elif status.level == 1: message = "WARN" elif status.level == 2: message = "ERROR" status.message = message status.values = [] ii = 0 while ii < 20: status.values.append(KeyValue("Key %i"%ii,str(random.randint(0,50)))) ii += 1 it += 1 msg.status.append(status) self.publisher.publish(msg) msg = DiagnosticArray() msg.header.stamp = rospy.Time.now() msg.status = [] status = DiagnosticStatus() status.level = status.WARN status.name = "Test Warn" status.hardware_id = "Dummy Diagnostics" status.message = "Warning - This is a test" status.values = [] msg.status.append(status) self.publisher.publish(msg)
def _diag(ID, category, state, msg='', key_value={}): array = DiagnosticArray() full_name = "mcr_diag" + rospy.get_name() + "/" + ID + "/" full_name = full_name.replace("//", "/") s1 = DiagnosticStatus(name=full_name, level=state, message=msg) values = [] for k, v in key_value.items(): values.append(KeyValue(key=k, value=v)) values.append(KeyValue(key='failure_category', value=category)) s1.values = values array.status = [s1] _pub.publish(array) if (s1.level != _STATUS_OK): diag_msgs.append(s1)
def publish_status(self): stat = DiagnosticStatus() stat.name = self.name is_connect_ok = self.check_connection() if is_connect_ok: stat.hardware_id = str(self._info_data['serial']) model = str(self._info_data['model']) top_temp = self.convertTemp( self._diag_data['volt_temp']['top']['lm20_temp']) bot_temp = self.convertTemp( self._diag_data['volt_temp']['bot']['lm20_temp']) i_out = self.convertAmp( self._diag_data['volt_temp']['bot']['i_out']) v_in = self.convertVolt( self._diag_data['volt_temp']['bot']['pwr_v_in']) stat.values = [ KeyValue(key='ConnectionStatus', value='OK'), KeyValue(key='Model', value=str(model)), KeyValue(key='TopTemp[DegC]', value=str(round(top_temp, 3))), KeyValue(key='BottomTemp[DegC]', value=str(round(bot_temp, 3))), KeyValue(key='Iout[V]', value=str(round(i_out, 3))), KeyValue(key='Vin[V]', value=str(round(v_in, 3))) ] self.judge_risk_level(top_temp, bot_temp, i_out, v_in, stat) else: stat.level = DiagnosticStatus.ERROR stat.hardware_id = 'Velodyne' stat.message = 'Connection Lost ' + self._ip stat.values = [KeyValue(key='ConnectionStatus', value='N/A')] msg = DiagnosticArray() msg.header.stamp = rospy.get_rostime() msg.status.append(stat) self._pub.publish(msg)
def odom_cb(self, msg): with self.lock: dist = msg.distance + (msg.angle * 0.25) # check if base moved if dist > self.dist + EPS: self.start_time = rospy.Time.now() self.start_angle = self.last_angle self.dist = dist # do imu test if possible if rospy.Time.now() > self.start_time + rospy.Duration(10.0): self.drift = fabs(self.start_angle - self.last_angle)*180/(pi*10) self.start_time = rospy.Time.now() self.start_angle = self.last_angle self.last_measured = rospy.Time.now() # publish diagnostics d = DiagnosticArray() d.header.stamp = rospy.Time.now() ds = DiagnosticStatus() ds.name = "imu_node: Imu Drift Monitor" if self.drift < 0.5: ds.level = DiagnosticStatus.OK ds.message = 'OK' elif self.drift < 1.0: ds.level = DiagnosticStatus.WARN ds.message = 'Drifting' else: ds.level = DiagnosticStatus.ERROR ds.message = 'Drifting' drift = self.drift if self.drift < 0: last_measured = 'No measurements yet, waiting for base to stop moving before measuring' drift = 'N/A' else: age = (rospy.Time.now() - self.last_measured).to_sec() if age < 60: last_measured = '%f seconds ago'%age elif age < 3600: last_measured = '%f minutes ago'%(age/60) else: last_measured = '%f hours ago'%(age/3600) ds.values = [ KeyValue('Last measured', last_measured), KeyValue('Drift (deg/sec)', str(drift)) ] d.status = [ds] self.pub_diag.publish(d)
def cb_diagnostics_update(self): """ Callback periodically called to update the diagnostics status of the tabpage handler. @return: A status message for the tabpage handler @rtype: C{diagnostic_msgs/DiagnosticStatus} """ tabpage_status = DiagnosticStatus() tabpage_status.level = tabpage_status.OK tabpage_status.name = " ".join([self.handler_name, "Handler"]) tabpage_status.hardware_id = self.parent.ros_name tabpage_status.message = "OK" tabpage_status.values = [] return tabpage_status
def joint_to_diag(js): d = DiagnosticStatus() d.level = 0 d.message = '' if not js.is_calibrated: d.level = 1 d.message = 'UNCALIBRATED' d.name = "Joint (%s)" % js.name d.values = [ KeyValue('Position', str(js.position)), KeyValue('Velocity', str(js.velocity)), KeyValue('Applied Effort', str(js.applied_effort)), KeyValue('Commanded Effort', str(js.commanded_effort)), KeyValue('Calibrated', str(js.is_calibrated)) ] return d
def publish(self, msg): """Publish a single diagnostic status or a vector of diagnostic statuses.""" if not type(msg) is list: msg = [msg] for stat in msg: stat.name = self.node.get_name() + ': ' + stat.name now = self.clock.now() da = DiagnosticArray() db = DiagnosticStatus() db.name = stat.name db.message = stat.message db.hardware_id = stat.hardware_id db.values = stat.values da.status.append(db) da.header.stamp = now.to_msg() # Add timestamp for ROS 0.10 self.publisher.publish(da)
def reset(self): """Resets the environment to an initial state and returns an initial observation. Note that this function should not reset the environment's random number generator(s); random variables in the environment's state should be sampled independently between multiple calls to `reset()`. In other words, each call of `reset()` should yield an environment suitable for a new episode, independent of previous episodes. Returns: observation (object): the initial observation. """ if self._has_state(): # generate log info = { 'episode': self.__episode, 'net_reward': self.__net_reward, 'duration': (rospy.Time.now() - self.__start_time).to_sec() } info.update(self._get_state()[3]) # send message msg = DiagnosticStatus() msg.level = DiagnosticStatus.OK msg.name = 'ROS-Gym Interface' msg.message = 'log of episode data' msg.hardware_id = self.__LOG_ID msg.values = [ KeyValue(key=key, value=str(value)) for key, value in info.items() ] self.__log_pub.publish(msg) # update variables (update time after reset) self.__episode += 1 self.__net_reward = 0 # reset if not self.__EVAL_MODE: self._reset_env() self._reset_self() self.__step_time_and_wait_for_state(5) self.__start_time = rospy.Time.now() # logging return self._get_state()[0]
def check_ok(self): with self._mutex: stat = 0 if self._ok else 2 msg = '' if self._ok else 'Dropped Packets' if rospy.get_time() - self._update_time > 3: stat = 3 msg = 'Packet Data Stale' if self._update_time == -1: msg = 'No Packet Data' diag = DiagnosticStatus() diag.name = 'EC Stats Packet Data' diag.level = stat diag.message = msg if diag.level == 0: diag.message = 'OK' diag.values = [ KeyValue(key='Has Link?', value=str(self._has_link)), KeyValue(key='Dropped Since Reset', value=str(self._total_dropped - self._drop_count_at_reset)), KeyValue(key='Total Drops', value=str(self._total_dropped)), KeyValue(key='Lost Link Count', value=str(self._lost_link_count)), KeyValue(key='Lost Links Since Reset', value=str(self._lost_link_count_since_reset)), KeyValue(key='Number of Resets', value=str(self._reset_count)), KeyValue(key='Time Since Last Reset', value=str(rospy.get_time() - self._time_at_last_reset)), KeyValue(key='Drops at Last Reset', value=str(self._drop_count_at_reset)), KeyValue(key='Max Device Count', value=str(self._max_device_count)), KeyValue(key='Interval Drops', value=str(self._interval_dropped)), KeyValue(key='Total Late Packets', value=str(self._total_late)), KeyValue(key='Interval Late Packets', value=str(self._interval_late)), KeyValue(key='Total Sent Packets', value=str(self._total_sent)), KeyValue(key='Interval Sent Packets', value=str(self._interval_sent)), KeyValue(key='Total Bandwidth', value=str(self._total_bandwidth)), KeyValue(key='Interval Bandwidth', value=str(self._interval_bandwidth)) ] return stat, msg, [ diag ]
def __init__(self, argv=sys.argv): rospy.init_node("ntp_monitor") self.parse_args(argv) stat = DiagnosticStatus() stat.level = DiagnosticStatus.WARN stat.name = '%s NTP Offset' % self.diag_hostname stat.message = 'No Data' stat.hardware_id = self.diag_hostname stat.values = [] self.msg = DiagnosticArray() self.msg.header.stamp = rospy.get_rostime() self.msg.status = [stat] self.update_diagnostics() self.diag_pub = rospy.Publisher("/diagnostics", DiagnosticArray, queue_size=1) self.diag_timer = rospy.Timer(rospy.Duration(1.0), self.publish_diagnostics) self.monitor_timer = rospy.Timer(rospy.Duration(60.0), self.update_diagnostics)
def publish(self, sensor_state, gyro): curr_time = sensor_state.header.stamp # limit to 5hz if (curr_time - self.last_diagnostics_time).to_sec() < 0.2: return self.last_diagnostics_time = curr_time diag = DiagnosticArray() diag.header.stamp = curr_time stat = DiagnosticStatus() #node status stat = DiagnosticStatus(name="TurtleBot Node", level=DiagnosticStatus.OK, message="RUNNING") diag.status.append(stat) #mode info stat = DiagnosticStatus(name="Operating Mode", level=DiagnosticStatus.OK) try: stat.message = self.oi_mode[sensor_state.oi_mode] except KeyError as ex: stat.level = DiagnosticStatus.ERROR stat.message = "Invalid OI Mode Reported %s" % ex rospy.logwarn(stat.message) diag.status.append(stat) #battery info stat = DiagnosticStatus(name="Battery", level=DiagnosticStatus.OK, message="OK") values = stat.values if sensor_state.charging_state == 5: stat.level = DiagnosticStatus.ERROR stat.message = "Charging Fault Condition" values.append( KeyValue("Charging State", self.charging_state[sensor_state.charging_state])) values.extend([ KeyValue("Voltage (V)", str(sensor_state.voltage / 1000.0)), KeyValue("Current (A)", str(sensor_state.current / 1000.0)), KeyValue("Temperature (C)", str(sensor_state.temperature)), KeyValue("Charge (Ah)", str(sensor_state.charge / 1000.0)), KeyValue("Capacity (Ah)", str(sensor_state.capacity / 1000.0)) ]) diag.status.append(stat) #charging source stat = DiagnosticStatus(name="Charging Sources", level=DiagnosticStatus.OK) try: stat.message = self.charging_source[ sensor_state.charging_sources_available] except KeyError as ex: stat.level = DiagnosticStatus.ERROR stat.message = "Invalid Charging Source %s, actual value: %i" % ( ex, sensor_state.charging_sources_available) rospy.logwarn(stat.message) diag.status.append(stat) #cliff sensors stat = DiagnosticStatus(name="Cliff Sensor", level=DiagnosticStatus.OK, message="OK") if sensor_state.cliff_left or sensor_state.cliff_front_left or sensor_state.cliff_right or sensor_state.cliff_front_right: stat.level = DiagnosticStatus.WARN if (sensor_state.current / 1000.0) > 0: stat.message = "Near Cliff: While the irobot create is charging, the create thinks it's near a cliff." stat.level = DiagnosticStatus.OK # We're OK here else: stat.message = "Near Cliff" stat.values = [ KeyValue("Left", str(sensor_state.cliff_left)), KeyValue("Left Signal", str(sensor_state.cliff_left_signal)), KeyValue("Front Left", str(sensor_state.cliff_front_left)), KeyValue("Front Left Signal", str(sensor_state.cliff_front_left_signal)), KeyValue("Front Right", str(sensor_state.cliff_right)), KeyValue("Front Right Signal", str(sensor_state.cliff_right_signal)), KeyValue("Right", str(sensor_state.cliff_front_right)), KeyValue("Right Signal", str(sensor_state.cliff_front_right_signal)) ] diag.status.append(stat) #Wall sensors stat = DiagnosticStatus(name="Wall Sensor", level=DiagnosticStatus.OK, message="OK") #wall always seems to be false??? if sensor_state.wall: stat.level = DiagnosticStatus.ERROR stat.message = "Near Wall" stat.values = [ KeyValue("Wall", str(sensor_state.wall)), KeyValue("Wall Signal", str(sensor_state.wall_signal)), KeyValue("Virtual Wall", str(sensor_state.virtual_wall)) ] diag.status.append(stat) #Gyro stat = DiagnosticStatus(name="Gyro Sensor", level=DiagnosticStatus.OK, message="OK") if gyro is None: stat.level = DiagnosticStatus.WARN stat.message = "Gyro Not Enabled: To enable the gyro set the has_gyro param in the turtlebot_node." elif gyro.cal_offset < 100: stat.level = DiagnosticStatus.ERROR stat.message = "Gyro Power Problem: For more information visit http://answers.ros.org/question/2091/turtlebot-bad-gyro-calibration-also." elif gyro.cal_offset > 575.0 or gyro.cal_offset < 425.0: stat.level = DiagnosticStatus.ERROR stat.message = "Bad Gyro Calibration Offset: The gyro average is outside the standard deviation." if gyro is not None: stat.values = [ KeyValue("Gyro Enabled", str(gyro is not None)), KeyValue("Raw Gyro Rate", str(sensor_state.user_analog_input)), KeyValue("Calibration Offset", str(gyro.cal_offset)), KeyValue("Calibration Buffer", str(gyro.cal_buffer)) ] diag.status.append(stat) #Digital IO stat = DiagnosticStatus(name="Digital Outputs", level=DiagnosticStatus.OK, message="OK") out_byte = sensor_state.user_digital_outputs stat.values = [ KeyValue("Raw Byte", str(out_byte)), KeyValue("Digital Out 2", self.digital_outputs[out_byte % 2]), KeyValue("Digital Out 1", self.digital_outputs[(out_byte >> 1) % 2]), KeyValue("Digital Out 0", self.digital_outputs[(out_byte >> 2) % 2]) ] diag.status.append(stat) #publish self.diag_pub.publish(diag)
def __init__(self): rospy.init_node('system_node', log_level=rospy.DEBUG) # self.load_publisher = rospy.Publisher('~load', msgs.CPUUsage, queue_size=1) # self.cpu_publisher = rospy.Publisher('~cpu', msgs.CPUUsage, queue_size=1) self.memory_publisher = rospy.Publisher('~memory', msgs.MemoryUsage, queue_size=1) self.disk_publisher = rospy.Publisher('~disk', msgs.DiskUsage, queue_size=1) self.diagnostics_pub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=10) self.debug_level_sub = rospy.Subscriber('~debug_level', Int16, self.on_debug_level) self.debug_level = ONLY_ERRORS self.drive_path = '/dev/root' self.rate = 1 #float(rospy.get_param("~rate", 1)) # hertz rospy.loginfo('Ready') r = rospy.Rate(self.rate) while not rospy.is_shutdown(): # Find normalized system load. # The ideal normalized system load less than or equal 1.0, meaning the processor is getting no more than the maximum number of tasks it can handle. proc_count = multiprocessing.cpu_count() load_1, load_5, load_15 = os.getloadavg() normalized_load_1 = load_1 / float(proc_count) normalized_load_5 = load_5 / float(proc_count) normalized_load_15 = load_15 / float(proc_count) normalized_load_level = OK if normalized_load_level >= 4.0: normalized_load_level = ERROR elif normalized_load_level >= 2.0: normalized_load_level = WARN # Find CPU. cpu_usage_percent = to_percent( getoutput( "grep 'cpu ' /proc/stat | awk '{usage=($2+$4)*100/($2+$4+$5)} END {print usage }'" )) cpu_usage_percent_level = OK if cpu_usage_percent >= c.CPU_USAGE_PERCENT_ERROR: cpu_usage_percent_level = ERROR elif cpu_usage_percent >= c.CPU_USAGE_PERCENT_WARN: cpu_usage_percent_level = WARN # msg = msgs.CPUUsage() # msg.header.stamp = rospy.Time.now() # msg.percent_used = cpu_usage_percent # self.cpu_publisher.publish(msg) # Find memory. memory_usage_free_gbytes = to_float( getoutput("free -m|grep -i 'Mem:'|awk '{print $4}'")) memory_usage_used_gbytes = to_float( getoutput("free -m|grep -i 'Mem:'|awk '{print $3}'")) memory_usage_total_gbytes = to_float( getoutput("free -m|grep -i 'Mem:'|awk '{print $2}'")) memory_usage_percent = -1 if memory_usage_used_gbytes != -1 and memory_usage_total_gbytes != -1: memory_usage_percent = memory_usage_used_gbytes / memory_usage_total_gbytes * 100 memory_usage_percent_level = OK if memory_usage_percent >= c.MEMORY_USAGE_PERCENT_ERROR: memory_usage_percent_level = ERROR elif memory_usage_percent >= c.MEMORY_USAGE_PERCENT_WARN: memory_usage_percent_level = WARN msg = msgs.MemoryUsage() msg.header.stamp = rospy.Time.now() msg.used_gbytes = memory_usage_used_gbytes msg.free_gbytes = memory_usage_free_gbytes msg.total_gbytes = memory_usage_total_gbytes msg.percent_used = memory_usage_percent self.memory_publisher.publish(msg) # Find disk. disk_usage_percent = to_percent( getoutput("df -H | grep -i " + self.drive_path + " | awk '{print $5}'")) disk_usage_free_gbytes = to_gbytes( getoutput("df -H | grep -i " + self.drive_path + " | awk '{print $4}'")) disk_usage_used_gbytes = to_gbytes( getoutput("df -H | grep -i " + self.drive_path + " | awk '{print $3}'")) disk_usage_total_gbytes = to_gbytes( getoutput("df -H | grep -i " + self.drive_path + " | awk '{print $2}'")) disk_usage_level = OK if disk_usage_percent >= c.DISK_USAGE_PERCENT_ERROR: disk_usage_level = ERROR if disk_usage_percent >= c.DISK_USAGE_PERCENT_WARN: disk_usage_level = WARN msg = msgs.DiskUsage() msg.header.stamp = rospy.Time.now() msg.used_gbytes = disk_usage_used_gbytes msg.free_gbytes = disk_usage_free_gbytes msg.total_gbytes = disk_usage_total_gbytes msg.percent_used = disk_usage_percent self.disk_publisher.publish(msg) # Find CPU clock speed. min_ghz = get_cpu_clock_speed_min() / 1000. / 1000. max_ghz = get_cpu_clock_speed_max() / 1000. / 1000. cpu_clock_speed = get_cpu_clock_speed() # print('cpu_clock_speed:', cpu_clock_speed) cpu_clock_speed_percent = get_cpu_clock_speed_percent() # print('cpu_clock_speed_percent:', cpu_clock_speed_percent) cpu_clock_speed_percent_level = OK # if cpu_clock_speed_percent <= c.CPU_CLOCK_SPEED_PERCENT_ERROR: # cpu_clock_speed_percent_level = ERROR # elif cpu_clock_speed_percent <= c.CPU_CLOCK_SPEED_PERCENT_WARN: # cpu_clock_speed_percent_level = WARN # Find CPU temperature. cpu_temp = get_cpu_temp() cpu_temp_level = OK if cpu_temp >= c.CPU_TEMP_ERROR: cpu_temp_level = ERROR elif cpu_temp >= c.CPU_TEMP_WARN: cpu_temp_level = WARN # Publish standard diagnostics. array = DiagnosticArray() statuses = [] normalized_load_status = DiagnosticStatus( name='Normalized System Load', level=normalized_load_level, message=str(normalized_load_15)) normalized_load_status.values = [ KeyValue(key='normalized load 1', value=str(normalized_load_1)), KeyValue(key='normalized load 5', value=str(normalized_load_5)), KeyValue(key='normalized load 15', value=str(normalized_load_15)), ] if self.debug_level >= UP_TO_OK or normalized_load_level != OK: statuses.append(normalized_load_status) cpu_temperature_status = DiagnosticStatus(name='CPU Temperature', level=cpu_temp_level, message=str(cpu_temp)) cpu_temperature_status.values = [ KeyValue(key='celcius', value=str(cpu_temp)), ] if self.debug_level >= UP_TO_OK or cpu_temp >= c.CPU_TEMP_ERROR: statuses.append(cpu_temperature_status) cpu_usage_status = DiagnosticStatus(name='CPU Usage', level=cpu_usage_percent_level, message=str(cpu_usage_percent)) cpu_usage_status.values = [ KeyValue(key='percent', value=str(cpu_usage_percent)), ] if self.debug_level >= UP_TO_OK or cpu_usage_percent_level != OK: statuses.append(cpu_usage_status) cpu_clock_speed_status = DiagnosticStatus( name='CPU Speed', level=cpu_clock_speed_percent_level, message=str(cpu_clock_speed_percent)) cpu_clock_speed_status.values = [ KeyValue(key='clock speed (GHz)', value=str(cpu_clock_speed)), KeyValue(key='min clock speed (GHz)', value=str(min_ghz)), KeyValue(key='max clock speed (GHz)', value=str(max_ghz)), KeyValue(key='clock speed (percent)', value=str(cpu_clock_speed_percent)), ] if self.debug_level >= UP_TO_OK or cpu_clock_speed_percent_level != OK: statuses.append(cpu_clock_speed_status) disk_usage_status = DiagnosticStatus( name='Disk Usage', level=disk_usage_level, message=str(disk_usage_percent)) disk_usage_status.values = [ KeyValue(key='percent', value=str(disk_usage_percent)), KeyValue(key='free gb', value=str(disk_usage_free_gbytes)), KeyValue(key='used gb', value=str(disk_usage_used_gbytes)), KeyValue(key='total gb', value=str(disk_usage_total_gbytes)), ] if self.debug_level >= UP_TO_OK or disk_usage_level != OK: statuses.append(disk_usage_status) memory_usage_status = DiagnosticStatus( name='Memory Usage', level=memory_usage_percent_level, message=str(memory_usage_percent)) memory_usage_status.values = [ KeyValue(key='percent', value=str(memory_usage_percent)), KeyValue(key='free gb', value=str(memory_usage_free_gbytes)), KeyValue(key='used gb', value=str(memory_usage_used_gbytes)), KeyValue(key='total gb', value=str(memory_usage_total_gbytes)), ] if self.debug_level >= UP_TO_OK or memory_usage_percent_level != OK: statuses.append(memory_usage_status) if statuses: array.status = statuses # [ # normalized_load_status, # cpu_temperature_status, # cpu_usage_status, # cpu_clock_speed_status, # disk_usage_status, # memory_usage_status, # ] self.diagnostics_pub.publish(array) r.sleep()
def update(self, mech_state): self._rx_count += 1 diag = DiagnosticStatus() diag.level = 0 # Default the level to 'OK' diag.name = "Trans. Monitor: %s" % self._joint diag.message = "OK" diag.values = [] diag.values.append(KeyValue("Joint", self._joint)) diag.values.append(KeyValue("Actuator", self._actuator)) diag.values.append(KeyValue("Up position", str(self._up_ref))) diag.values.append(KeyValue("Down position", str(self._down_ref))) diag.values.append(KeyValue("Wrap", str(self._wrap))) diag.values.append(KeyValue("Max Limit", str(self._max))) diag.values.append(KeyValue("Min Limit", str(self._min))) diag.values.append(KeyValue("Deadband", str(self._deadband))) diag.values.append(KeyValue("Mech State RX Count", str(self._rx_count))) # Check if we can find both the joint and actuator act_names = [x.name for x in mech_state.actuator_states] act_exists = self._actuator in act_names act_exists_msg = 'Error' if act_exists: cal_reading = mech_state.actuator_states[act_names.index( self._actuator)].calibration_reading joint_names = [x.name for x in mech_state.joint_states] joint_exists = self._joint in joint_names jnt_exists_msg = 'Error' if joint_exists: position = mech_state.joint_states[joint_names.index( self._joint)].position calibrated = (mech_state.joint_states[joint_names.index( self._joint)].is_calibrated == 1) diag.values.append(KeyValue('Actuator Exists', str(act_exists))) diag.values.append(KeyValue('Joint Exists', str(joint_exists))) # First check existance of joint, actuator if not (act_exists and joint_exists): diag.level = 2 diag.message = 'Actuators, Joints missing' self._ok = False return diag, False # Monitor current state reading_msg = 'OK' if not self.check_device(position, cal_reading, calibrated): self._ok = False self._num_errors += 1 self._num_errors_since_reset += 1 reading_msg = 'Broken' # If we've had an error since the last reset, we're no good if not self._ok: diag.message = 'Broken' diag.level = 2 diag.values.insert(0, KeyValue('Transmission Status', diag.message)) diag.values.insert(1, KeyValue('Current Reading', reading_msg)) diag.values.append(KeyValue('Is Calibrated', str(calibrated))) diag.values.append(KeyValue('Calibration Reading', str(cal_reading))) diag.values.append(KeyValue('Joint Position', str(position))) diag.values.append(KeyValue('Total Errors', str(self._num_errors))) diag.values.append( KeyValue('Errors Since Reset', str(self._num_errors_since_reset))) self._max_position = max(self._max_position, position) diag.values.append( KeyValue('Max Obs. Position', str(self._max_position))) self._min_position = min(self._min_position, position) diag.values.append( KeyValue('Min Obs. Position', str(self._min_position))) return self._ok, diag
latch=True) array = DiagnosticArray() status = DiagnosticStatus(name='chronyStatus',\ level=0,\ hardware_id=hostname) array.status = [status] rate = rospy.Rate( 0.2) # query and publish chrony information once every 5 seconds chronyVersion = checkChronyVersion() #chronyMinVersion = 1.29 #publish error and exit if chronyMinVersion is not satisfied #if chronyVersion < chronyMinVersion: # rospy.logerr('ChronyStatus requires chrony version ' + str(chronyMinVersion) + \ # ' or greater, version ' + str(chronyVersion) + ' detected, exiting') #else: while not rospy.is_shutdown(): status.values = [] status.values.append( KeyValue(key='chrony version', value=chronyVersion)) getTracking(status) getSources(status) pub.publish(array) rate.sleep()
def publish(self, sensor_state, gyro): curr_time = sensor_state.header.stamp # limit to 5hz if (curr_time - self.last_diagnostics_time).to_sec() < 0.2: return self.last_diagnostics_time = curr_time diag = DiagnosticArray() diag.header.stamp = curr_time stat = DiagnosticStatus() #node status stat = DiagnosticStatus(name="TurtleBot Node", level=DiagnosticStatus.OK, message="RUNNING") diag.status.append(stat) #mode info stat = DiagnosticStatus(name="Operating Mode", level=DiagnosticStatus.OK) try: stat.message = self.oi_mode[sensor_state.oi_mode] except KeyError as ex: stat.message = "Invalid OI Mode %s"%ex rospy.logwarn("Invalid OI Mode %s"%ex) diag.status.append(stat) #battery info stat = DiagnosticStatus(name="Battery", level=DiagnosticStatus.OK, message="OK") values = stat.values if sensor_state.charging_state == 5: stat.level = DiagnosticStatus.ERROR stat.message = "Charging Fault Condition" values.append(KeyValue("Charging State", self.charging_state[sensor_state.charging_state])) values.extend([KeyValue("Voltage (V)", str(sensor_state.voltage/1000.0)), KeyValue("Current (A)", str(sensor_state.current/1000.0)), KeyValue("Temperature (C)",str(sensor_state.temperature)), KeyValue("Charge (Ah)", str(sensor_state.charge/1000.0)), KeyValue("Capacity (Ah)", str(sensor_state.capacity/1000.0))]) diag.status.append(stat) #charging source stat = DiagnosticStatus(name="Charging Sources", level=DiagnosticStatus.OK) try: stat.message = self.charging_source[sensor_state.charging_sources_available] except KeyError as ex: stat.message = "Invalid Charging Source %s, actual value: %i"%(ex,sensor_state.charging_sources_available) rospy.logwarn("Invalid Charging Source %s, actual value: %i"%(ex,sensor_state.charging_sources_available)) diag.status.append(stat) #cliff sensors stat = DiagnosticStatus(name="Cliff Sensor", level=DiagnosticStatus.OK, message="OK") if sensor_state.cliff_left or sensor_state.cliff_front_left or sensor_state.cliff_right or sensor_state.cliff_front_right: stat.level = DiagnosticStatus.WARN if (sensor_state.current/1000.0)>0: stat.message = "Near Cliff: While the irobot create is charging, the create thinks it's near a cliff." else: stat.message = "Near Cliff" stat.values = [KeyValue("Left", str(sensor_state.cliff_left)), KeyValue("Left Signal", str(sensor_state.cliff_left_signal)), KeyValue("Front Left", str(sensor_state.cliff_front_left)), KeyValue("Front Left Signal", str(sensor_state.cliff_front_left_signal)), KeyValue("Front Right", str(sensor_state.cliff_right)), KeyValue("Front Right Signal", str(sensor_state.cliff_right_signal)), KeyValue("Right", str(sensor_state.cliff_front_right)), KeyValue("Right Signal", str(sensor_state.cliff_front_right_signal))] diag.status.append(stat) #Wall sensors stat = DiagnosticStatus(name="Wall Sensor", level=DiagnosticStatus.OK, message="OK") #wall always seems to be false??? if sensor_state.wall: stat.level = DiagnosticStatus.ERROR stat.message = "Near Wall" stat.values = [KeyValue("Wall", str(sensor_state.wall)), KeyValue("Wall Signal", str(sensor_state.wall_signal)), KeyValue("Virtual Wall", str(sensor_state.virtual_wall))] diag.status.append(stat) #Gyro stat = DiagnosticStatus(name="Gyro Sensor", level = DiagnosticStatus.OK, message = "OK") if gyro is None: stat.level = DiagnosticStatus.WARN stat.message = "Gyro Not Enabled: To enable the gyro set the has_gyro param in the turtlebot_node." elif gyro.cal_offset < 100: stat.level = DiagnosticStatus.ERROR stat.message = "Gyro Power Problem: For more information visit http://answers.ros.org/question/2091/turtlebot-bad-gyro-calibration-also." elif gyro.cal_offset > 575.0 or gyro.cal_offset < 425.0: stat.level = DiagnosticStatus.ERROR stat.message = "Bad Gyro Calibration Offset: The gyro average is outside the standard deviation." if gyro is not None: stat.values = [KeyValue("Gyro Enabled", str(gyro is not None)), KeyValue("Raw Gyro Rate", str(sensor_state.user_analog_input)), KeyValue("Calibration Offset", str(gyro.cal_offset)), KeyValue("Calibration Buffer", str(gyro.cal_buffer))] diag.status.append(stat) #Digital IO stat = DiagnosticStatus(name="Digital Outputs", level = DiagnosticStatus.OK, message = "OK") out_byte = sensor_state.user_digital_outputs stat.values = [KeyValue("Raw Byte", str(out_byte)), KeyValue("Digital Out 2", self.digital_outputs[out_byte%2]), KeyValue("Digital Out 1", self.digital_outputs[(out_byte >>1)%2]), KeyValue("Digital Out 0", self.digital_outputs[(out_byte >>2)%2])] diag.status.append(stat) #publish self.diag_pub.publish(diag)
def publish(self, sensor_state): curr_time = sensor_state.header.stamp # limit to 5hz if (curr_time - self.last_diagnostics_time).to_sec() < 0.2: return self.last_diagnostics_time = curr_time diag = DiagnosticArray() diag.header.stamp = curr_time stat = DiagnosticStatus() #node status stat = DiagnosticStatus(name="Robo50 Node", level=DiagnosticStatus.OK, message="RUNNING") diag.status.append(stat) #battery info # stat = DiagnosticStatus(name="Battery", level=DiagnosticStatus.OK, message="OK") # values = stat.values # if sensor_state.charging_state == 5: # stat.level = DiagnosticStatus.ERROR # stat.message = "Charging Fault Condition" # values.append(KeyValue("Charging State", self.charging_state[sensor_state.charging_state])) # values.extend([KeyValue("Voltage (V)", str(sensor_state.voltage/1000.0)), # KeyValue("Current (A)", str(sensor_state.current/1000.0)), # KeyValue("Temperature (C)",str(sensor_state.temperature)), # KeyValue("Charge (Ah)", str(sensor_state.charge/1000.0)), # KeyValue("Capacity (Ah)", str(sensor_state.capacity/1000.0))]) # diag.status.append(stat) # #cliff sensors # stat = DiagnosticStatus(name="Cliff Sensor", level=DiagnosticStatus.OK, message="OK") # if sensor_state.cliff_left or sensor_state.cliff_front_left or sensor_state.cliff_right or sensor_state.cliff_front_right: # stat.level = DiagnosticStatus.WARN # if (sensor_state.current/1000.0)>0: # stat.message = "Near Cliff: While the irobot create is charging, the create thinks it's near a cliff." # stat.level = DiagnosticStatus.OK # We're OK here # else: # stat.message = "Near Cliff" # stat.values = [KeyValue("Left", str(sensor_state.cliff_left)), # KeyValue("Left Signal", str(sensor_state.cliff_left_signal)), # KeyValue("Front Left", str(sensor_state.cliff_front_left)), # KeyValue("Front Left Signal", str(sensor_state.cliff_front_left_signal)), # KeyValue("Front Right", str(sensor_state.cliff_right)), # KeyValue("Front Right Signal", str(sensor_state.cliff_right_signal)), # KeyValue("Right", str(sensor_state.cliff_front_right)), # KeyValue("Right Signal", str(sensor_state.cliff_front_right_signal))] # diag.status.append(stat) # #Wall sensors # stat = DiagnosticStatus(name="Wall Sensor", level=DiagnosticStatus.OK, message="OK") # #wall always seems to be false??? # if sensor_state.wall: # stat.level = DiagnosticStatus.ERROR # stat.message = "Near Wall" # stat.values = [KeyValue("Wall", str(sensor_state.wall)), # KeyValue("Wall Signal", str(sensor_state.wall_signal)), # KeyValue("Virtual Wall", str(sensor_state.virtual_wall))] # diag.status.append(stat) #Gyro # stat = DiagnosticStatus(name="Gyro Sensor", level = DiagnosticStatus.OK, message = "OK") # if gyro is None: # stat.level = DiagnosticStatus.WARN # stat.message = "Gyro Not Enabled: To enable the gyro set the has_gyro param in the Robo50_node." # elif gyro.cal_offset < 100: # stat.level = DiagnosticStatus.ERROR # stat.message = "Gyro Power Problem: For more information visit http://answers.ros.org/question/2091/Robo50-bad-gyro-calibration-also." # elif gyro.cal_offset > 575.0 or gyro.cal_offset < 425.0: # stat.level = DiagnosticStatus.ERROR # stat.message = "Bad Gyro Calibration Offset: The gyro average is outside the standard deviation." # # if gyro is not None: # stat.values = [KeyValue("Gyro Enabled", str(gyro is not None)), # KeyValue("Raw Gyro Rate", str(sensor_state.user_analog_input)), # KeyValue("Calibration Offset", str(gyro.cal_offset)), # KeyValue("Calibration Buffer", str(gyro.cal_buffer))] # diag.status.append(stat) #IMU stat = DiagnosticStatus(name="IMU Sensor", level = DiagnosticStatus.OK, message = "OK") stat.values = [KeyValue("Compass Heading", str(sensor_state.imu.heading)), KeyValue("Gyro", str(sensor_state.imu.gyro)), KeyValue("Accelero", str(sensor_state.imu.accelero))] diag.status.append(stat) #Encoder stat = DiagnosticStatus(name="Encoder Counts", level = DiagnosticStatus.OK, message = "OK") stat.values = [KeyValue("Left Encoder", str(sensor_state.encoder_counts_left)), KeyValue("Right Encoder", str(sensor_state.encoder_counts_right)), KeyValue("Distance Left", str(sensor_state.distance_left)), KeyValue("Distance Right", str(sensor_state.distance_right)), KeyValue("Distance", str(sensor_state.distance)), KeyValue("Angle", str(sensor_state.angle))] diag.status.append(stat) #publish self.diag_pub.publish(diag)
hostname = socket.gethostname() rospy.init_node('chronyStatus_'+hostname) pub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=1, latch=True) array = DiagnosticArray() status = DiagnosticStatus(name='ChronyStatus',\ level=0,\ hardware_id=hostname) array.status = [status] rate = rospy.Rate(0.2) # query and publish chrony information once every 5 seconds chronyVersion = checkChronyVersion() #chronyMinVersion = 1.29 #publish error and exit if chronyMinVersion is not satisfied #if chronyVersion < chronyMinVersion: # rospy.logerr('ChronyStatus requires chrony version ' + str(chronyMinVersion) + \ # ' or greater, version ' + str(chronyVersion) + ' detected, exiting') #else: while not rospy.is_shutdown(): status.values = [] status.values.append(KeyValue(key='chrony version', value=chronyVersion) ) getTracking(status) getSources(status) pub.publish(array) rate.sleep()
pub = rospy.Publisher("/diagnostics", DiagnosticArray, queue_size=10) rospy.Subscriber("hw_monitor/diagnostics/motor_stall_l", Bool, motor_stall_l_callback) rospy.Subscriber("hw_monitor/diagnostics/motor_stall_r", Bool, motor_stall_r_callback) rospy.Subscriber("hw_monitor/diagnostics/batt_low", Bool, batt_low_callback) rospy.Subscriber("hw_monitor/diagnostics/batt_high", Bool, batt_high_callback) rospy.Subscriber("hw_monitor/diagnostics/controller", Bool, controller_callback) rospy.Subscriber("hw_monitor/diagnostics/aux_lights", Bool, aux_lights_callback) array = DiagnosticArray() my_rate = rospy.Rate(1.0) while not rospy.is_shutdown(): motor_l_stat = DiagnosticStatus(name="Motor Left", level=0, message="Running") motor_l_stat.values = [KeyValue(key="Stall", value="False"), KeyValue(key="Controller Halt", value="False")] motor_r_stat = DiagnosticStatus(name="Motor Right", level=0, message="Running") motor_r_stat.values = [KeyValue(key="Stall", value="False"), KeyValue(key="Controller Halt", value="False")] if controller == True: motor_l_stat = DiagnosticStatus(name="Motor Left", level=0, message="Halted") motor_l_stat.values = [KeyValue(key="Stall", value="False"), KeyValue(key="Controller Halt", value="True")] motor_r_stat = DiagnosticStatus(name="Motor Right", level=0, message="Halted") motor_r_stat.values = [KeyValue(key="Stall", value="False"), KeyValue(key="Controller Halt", value="True")] if stall_l == True: motor_l_stat = DiagnosticStatus(name="Motor Left", level=0, message="Stalled") motor_l_stat.values = [KeyValue(key="Stall", value="True"), KeyValue(key="Controller Halt", value="False")] if stall_l == True & controller == True:
from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue from time import sleep def callback(msg): print("Got msg") return EmptyResponse() if __name__ == '__main__': rospy.init_node("test_node") rospy.Service('self_test', Empty, callback) pub = rospy.Publisher('/diagnostics', DiagnosticArray) # Publish diagnostics to check runtime monitor, rxconsole msg = DiagnosticArray() stat = DiagnosticStatus() stat.level = 0 stat.name = 'Test Node' stat.message = 'OK' stat.hardware_id = 'HW ID' stat.values = [ KeyValue('Node Status', 'OK') ] msg.status.append(stat) while not rospy.is_shutdown(): msg.header.stamp = rospy.get_rostime() pub.publish(msg) rospy.loginfo('Test node is printing things') sleep(1.0)
def ntp_monitor(namespace, offset=500, self_offset=500, diag_hostname=None, error_offset=5000000): rospy.init_node(NAME, anonymous=True) diag_updater = DiagnosticUpdater( name=namespace + 'ntp', display_name=diag_hostname + ' NTP', ) hostname = socket.gethostname() if diag_hostname is None: diag_hostname = hostname ntp_hostname = rospy.get_param('~reference_host', 'ntp.ubuntu.com') offset = rospy.get_param('~offset_tolerance', 500) error_offset = rospy.get_param('~error_offset_tolerance', 5000000) stat = DiagnosticStatus() stat.level = 0 stat.name = "NTP Offset" stat.message = "OK" stat.hardware_id = hostname stat.values = [] stat_diagnostic = GenericDiagnostic('/offset') stat_diagnostic.add_to_updater(diag_updater) # self_stat = DiagnosticStatus() # self_stat.level = DiagnosticStatus.OK # self_stat.name = "NTP self-offset for "+ diag_hostname # self_stat.message = "OK" # self_stat.hardware_id = hostname # self_stat.values = [] while not rospy.is_shutdown(): for st, host, off in [(stat, ntp_hostname, offset)]: try: p = Popen(["ntpdate", "-q", host], stdout=PIPE, stdin=PIPE, stderr=PIPE) res = p.wait() (o, e) = p.communicate() except OSError, (errno, msg): if errno == 4: break #ctrl-c interrupt else: raise if (res == 0): measured_offset = float(re.search("offset (.*),", o).group(1)) * 1000000 st.level = DiagnosticStatus.OK st.message = "OK" st.values = [ KeyValue("Offset (us)", str(measured_offset)), KeyValue("Offset tolerance (us)", str(off)), KeyValue("Offset tolerance (us) for Error", str(error_offset)) ] if (abs(measured_offset) > off): st.level = DiagnosticStatus.WARN st.message = "NTP offset too high" if (abs(measured_offset) > error_offset): st.level = DiagnosticStatus.ERROR st.message = "NTP offset too high" else: # Warning (not error), since this is a non-critical failure. st.level = DiagnosticStatus.WARN st.message = "Error running ntpdate (returned %d)" % res st.values = [ KeyValue("Offset (us)", "N/A"), KeyValue("Offset tolerance (us)", str(off)), KeyValue("Offset tolerance (us) for Error", str(error_offset)), KeyValue("Output", o), KeyValue("Errors", e) ] # Convert from ROS diagnostics to mbot_diagnostics for publishing. stat_diagnostic.set_status(Status(stat.level), stat.message) for diag_val in stat.values: stat_diagnostic.set_metric(diag_val.key, diag_val.value) time.sleep(1)
def cb_diagnostics_update(self): """ Callback periodically called to update the diagnostics status of the TouchOscInterface. """ msg = DiagnosticArray() msg.header.stamp = rospy.Time.now() msg.status = [] # If the callbacks DiagnosticStatus message hasn't been populated, # do that now. if not self._diagnostic_status_callbacks: callback_status = DiagnosticStatus() callback_status.level = callback_status.OK callback_status.name = " ".join([self.ros_name, "Registered Callbacks"]) callback_status.hardware_id = self.ros_name callback_status.message = "OK" callback_status.values = [] diags = [(k, v) for k, v in walk_node(self._osc_receiver)] rospy.logdebug("Registered Callbacks:") for (k, v) in diags: rospy.logdebug('{0:<30}{1:<30}'.format(k, v)) callback_status.values.append(KeyValue(key=k, value=v)) self._diagnostic_status_callbacks = callback_status msg.status.append(self._diagnostic_status_callbacks) # Populate the clients DiagnosticStatus message diagnostic_status_clients = DiagnosticStatus() diagnostic_status_clients.level = diagnostic_status_clients.OK diagnostic_status_clients.name = " ".join([self.ros_name, "Client Status"]) diagnostic_status_clients.hardware_id = self.ros_name diagnostic_status_clients.message = "Listening on %d" % self.osc_port diagnostic_status_clients.values = [] #TODO: The clients property accessor is not thread-safe, as far as I can #tell, but using a lock tends to make bonjour hang. clients = self.clients for client in clients.itervalues(): diagnostic_status_clients.values.append(KeyValue( key=client.address, value=client.servicename.split("[")[0])) diagnostic_status_clients.values.append(KeyValue( key=client.address + " Type", value=client.client_type)) diagnostic_status_clients.values.append(KeyValue( key=client.address + " Current", value=client.active_tabpage)) diagnostic_status_clients.values.append(KeyValue( key=client.address + " Tabpages", value=", ".join(client.tabpages))) if len(self.clients) == 0: diagnostic_status_clients.message = "No clients detected" msg.status.append(diagnostic_status_clients) # For each registered tabpage handler, get a DiagnosticStatus message. for tabpage in self.tabpage_handlers.itervalues(): msg.status.append(tabpage.cb_diagnostics_update()) # Publish self.diagnostics_pub.publish(msg) reactor.callLater(1.0, self.cb_diagnostics_update)
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)
def ntp_monitor(ntp_hostname, offset=500, self_offset=500, diag_hostname = None, error_offset = 5000000): pub = rospy.Publisher("/diagnostics", DiagnosticArray) rospy.init_node(NAME, anonymous=True) hostname = socket.gethostname() if diag_hostname is None: diag_hostname = hostname stat = DiagnosticStatus() stat.level = 0 stat.name = "NTP offset from "+ diag_hostname + " to " + ntp_hostname stat.message = "OK" stat.hardware_id = hostname stat.values = [] self_stat = DiagnosticStatus() self_stat.level = DiagnosticStatus.OK self_stat.name = "NTP self-offset for "+ diag_hostname self_stat.message = "OK" self_stat.hardware_id = hostname self_stat.values = [] while not rospy.is_shutdown(): for st,host,off in [(stat,ntp_hostname,offset), (self_stat, hostname,self_offset)]: try: p = Popen(["ntpdate", "-q", host], stdout=PIPE, stdin=PIPE, stderr=PIPE) res = p.wait() (o,e) = p.communicate() except OSError, (errno, msg): if errno == 4: break #ctrl-c interrupt else: raise if (res == 0): measured_offset = float(re.search("offset (.*),", o).group(1))*1000000 st.level = DiagnosticStatus.OK st.message = "OK" st.values = [ KeyValue("Offset (us)", str(measured_offset)), KeyValue("Offset tolerance (us)", str(off)), KeyValue("Offset tolerance (us) for Error", str(error_offset)) ] if (abs(measured_offset) > off): st.level = DiagnosticStatus.WARN st.message = "NTP Offset Too High" if (abs(measured_offset) > error_offset): st.level = DiagnosticStatus.ERROR st.message = "NTP Offset Too High" else: st.level = DiagnosticStatus.ERROR st.message = "Error Running ntpdate. Returned %d" % res st.values = [ KeyValue("Offset (us)", "N/A"), KeyValue("Offset tolerance (us)", str(off)), KeyValue("Offset tolerance (us) for Error", str(error_offset)), KeyValue("Output", o), KeyValue("Errors", e) ] msg = DiagnosticArray() msg.header.stamp = rospy.get_rostime() msg.status = [stat, self_stat] pub.publish(msg) time.sleep(1)
def ntp_monitor(ntp_hostname, offset=500, self_offset=500, diag_hostname=None, error_offset=5000000): pub = rospy.Publisher("/diagnostics", DiagnosticArray) rospy.init_node(NAME, anonymous=True) hostname = socket.gethostname() if diag_hostname is None: diag_hostname = hostname stat = DiagnosticStatus() stat.level = 0 stat.name = "NTP offset from " + diag_hostname + " to " + ntp_hostname stat.message = "OK" stat.hardware_id = hostname stat.values = [] self_stat = DiagnosticStatus() self_stat.level = DiagnosticStatus.OK self_stat.name = "NTP self-offset for " + diag_hostname self_stat.message = "OK" self_stat.hardware_id = hostname self_stat.values = [] while not rospy.is_shutdown(): for st, host, off in [(stat, ntp_hostname, offset), (self_stat, hostname, self_offset)]: try: p = Popen(["ntpdate", "-q", host], stdout=PIPE, stdin=PIPE, stderr=PIPE) res = p.wait() (o, e) = p.communicate() except OSError, (errno, msg): if errno == 4: break #ctrl-c interrupt else: raise if (res == 0): measured_offset = float(re.search("offset (.*),", o).group(1)) * 1000000 st.level = DiagnosticStatus.OK st.message = "OK" st.values = [ KeyValue("Offset (us)", str(measured_offset)), KeyValue("Offset tolerance (us)", str(off)), KeyValue("Offset tolerance (us) for Error", str(error_offset)) ] if (abs(measured_offset) > off): st.level = DiagnosticStatus.WARN st.message = "NTP Offset Too High" if (abs(measured_offset) > error_offset): st.level = DiagnosticStatus.ERROR st.message = "NTP Offset Too High" else: st.level = DiagnosticStatus.ERROR st.message = "Error Running ntpdate. Returned %d" % res st.values = [ KeyValue("Offset (us)", "N/A"), KeyValue("Offset tolerance (us)", str(off)), KeyValue("Offset tolerance (us) for Error", str(error_offset)), KeyValue("Output", o), KeyValue("Errors", e) ] msg = DiagnosticArray() msg.header.stamp = rospy.get_rostime() msg.status = [stat, self_stat] pub.publish(msg) time.sleep(1)
def publish(self, sensor_state, gyro): curr_time = sensor_state.header.stamp # limit to 5hz if (curr_time - self.last_diagnostics_time).to_sec() < 0.2: return self.last_diagnostics_time = curr_time diag = DiagnosticArray() diag.header.stamp = curr_time stat = DiagnosticStatus() #node status stat = DiagnosticStatus(name="WheeledRobin Node", level=DiagnosticStatus.OK, message="RUNNING") diag.status.append(stat) #mode info """ stat = DiagnosticStatus(name="Operating Mode", level=DiagnosticStatus.OK) try: stat.message = self.mode[sensor_state.mode] except KeyError as ex: stat.level=DiagnosticStatus.ERROR stat.message = "Invalid Mode Reported %s"%ex rospy.logwarn(stat.message) diag.status.append(stat) """ #cliff sensors """ stat = DiagnosticStatus(name="Cliff Sensor", level=DiagnosticStatus.OK, message="OK") if sensor_state.cliff_left or sensor_state.cliff_front_left or sensor_state.cliff_right or sensor_state.cliff_front_right: stat.level = DiagnosticStatus.WARN if (sensor_state.current/1000.0)>0: stat.message = "Near Cliff: While the irobot create is charging, the create thinks it's near a cliff." stat.level = DiagnosticStatus.OK # We're OK here else: stat.message = "Near Cliff" stat.values = [KeyValue("Left", str(sensor_state.cliff_left)), KeyValue("Left Signal", str(sensor_state.cliff_left_signal)), KeyValue("Front Left", str(sensor_state.cliff_front_left)), KeyValue("Front Left Signal", str(sensor_state.cliff_front_left_signal)), KeyValue("Front Right", str(sensor_state.cliff_right)), KeyValue("Front Right Signal", str(sensor_state.cliff_right_signal)), KeyValue("Right", str(sensor_state.cliff_front_right)), KeyValue("Right Signal", str(sensor_state.cliff_front_right_signal))] diag.status.append(stat) #Wall sensors stat = DiagnosticStatus(name="Wall Sensor", level=DiagnosticStatus.OK, message="OK") #wall always seems to be false??? if sensor_state.wall: stat.level = DiagnosticStatus.ERROR stat.message = "Near Wall" stat.values = [KeyValue("Wall", str(sensor_state.wall)), KeyValue("Wall Signal", str(sensor_state.wall_signal)), KeyValue("Virtual Wall", str(sensor_state.virtual_wall))] diag.status.append(stat) #Gyro stat = DiagnosticStatus(name="Gyro Sensor", level = DiagnosticStatus.OK, message = "OK") if gyro is None: stat.level = DiagnosticStatus.WARN stat.message = "Gyro Not Enabled: To enable the gyro set the has_gyro param in the wheeled_robin_node." elif gyro.cal_offset < 100: stat.level = DiagnosticStatus.ERROR stat.message = "Gyro Power Problem: For more information visit http://answers.ros.org/question/2091/turtlebot-bad-gyro-calibration-also." elif gyro.cal_offset > 575.0 or gyro.cal_offset < 425.0: stat.level = DiagnosticStatus.ERROR stat.message = "Bad Gyro Calibration Offset: The gyro average is outside the standard deviation." if gyro is not None: stat.values = [KeyValue("Gyro Enabled", str(gyro is not None)), KeyValue("Raw Gyro Rate", str(sensor_state.user_analog_input)), KeyValue("Calibration Offset", str(gyro.cal_offset)), KeyValue("Calibration Buffer", str(gyro.cal_buffer))] diag.status.append(stat) """ #Digital Outputs stat = DiagnosticStatus(name="Digital Outputs", level = DiagnosticStatus.OK, message = "OK") out_byte = sensor_state.digital_outputs stat.values = [KeyValue("Raw Byte", str(out_byte)), KeyValue("Digital Out 0", self.digital_outputs[(out_byte >>0)%2]), KeyValue("Digital Out 1", self.digital_outputs[(out_byte >>1)%2]), KeyValue("Digital Out 2", self.digital_outputs[(out_byte >>2)%2]), KeyValue("Digital Out 3", self.digital_outputs[(out_byte >>3)%2]), KeyValue("Digital Out 4", self.digital_outputs[(out_byte >>4)%2]), KeyValue("Digital Out 5", self.digital_outputs[(out_byte >>5)%2]), KeyValue("Digital Out 6", self.digital_outputs[(out_byte >>6)%2]), KeyValue("Digital Out 7", self.digital_outputs[(out_byte >>7)%2])] diag.status.append(stat) #Digital Inputs stat = DiagnosticStatus(name="Digital Inputs", level = DiagnosticStatus.OK, message = "OK") in_byte = sensor_state.digital_inputs stat.values = [KeyValue("Raw Byte", str(in_byte)), KeyValue("Digital Input 0", self.digital_outputs[(in_byte >>0)%2]), KeyValue("Digital Input 1", self.digital_outputs[(in_byte >>1)%2]), KeyValue("Digital Input 2", self.digital_outputs[(in_byte >>2)%2]), KeyValue("Digital Input 3", self.digital_outputs[(in_byte >>3)%2]), KeyValue("Digital Input 4", self.digital_outputs[(in_byte >>4)%2]), KeyValue("Digital Input 5", self.digital_outputs[(in_byte >>5)%2]), KeyValue("Digital Input 6", self.digital_outputs[(in_byte >>6)%2]), KeyValue("Digital Input 7", self.digital_outputs[(in_byte >>7)%2])] diag.status.append(stat) #publish self.diag_pub.publish(diag)