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 publish_diagnostic_msg(self): """Helper to imu_callback. Publishes a DiagnosticArray containing calibration information. """ diag_msg = DiagnosticArray() diag_msg.header.stamp = rospy.Time.now() diag_msg.status = [ DiagnosticStatus(level=DiagnosticStatus.OK, name='IMU Calibration Info', values=[ KeyValue(key='Gyro Bias', value=str(self.gyro_bias)), KeyValue(key='Gyro Scale', value=str(self.gyro_scale)), KeyValue(key='Accel Offsets', value=str(self.acc_offsets)), KeyValue(key='Accel Transform', value=str(self.acc_transform)), KeyValue(key='Mag Offsets', value=str(self.mag_offsets)), KeyValue(key='Mag Transform', value=str(self.mag_transform)), KeyValue(key='Misalignment', value=str(self.misalignment)) ]) ] self.imu_diag_pub.publish(diag_msg) return
def send_diags(): # See diagnostics with: rosrun rqt_runtime_monitor rqt_runtime_monitor msg = DiagnosticArray() msg.status = [] msg.header.stamp = rospy.Time.now() for gripper in grippers: for servo in gripper.servos: status = DiagnosticStatus() status.name = "Gripper '%s' servo %d"%(gripper.name, servo.servo_id) status.hardware_id = '%s'%servo.servo_id temperature = servo.read_temperature() status.values.append(KeyValue('Temperature', str(temperature))) status.values.append(KeyValue('Voltage', str(servo.read_voltage()))) if temperature >= 70: status.level = DiagnosticStatus.ERROR status.message = 'OVERHEATING' elif temperature >= 65: status.level = DiagnosticStatus.WARN status.message = 'HOT' else: status.level = DiagnosticStatus.OK status.message = 'OK' msg.status.append(status) diagnostics_pub.publish(msg)
def diagnostics_processor(self): diag_msg = DiagnosticArray() rate = rospy.Rate(self.diagnostics_rate) while not rospy.is_shutdown(): diag_msg.status = [] diag_msg.header.stamp = rospy.Time.now() for controller in self.controllers.values(): try: joint_state = controller.joint_state temps = joint_state.motor_temps max_temp = max(temps) status = DiagnosticStatus() status.name = 'Joint Controller (%s)' % controller.joint_name status.hardware_id = 'Robotis Dynamixel %s on port %s' % (str(joint_state.motor_ids), controller.port_namespace) status.values.append(KeyValue('Goal', str(joint_state.goal_pos))) status.values.append(KeyValue('Position', str(joint_state.current_pos))) status.values.append(KeyValue('Error', str(joint_state.error))) status.values.append(KeyValue('Velocity', str(joint_state.velocity))) status.values.append(KeyValue('Load', str(joint_state.load))) status.values.append(KeyValue('Moving', str(joint_state.is_moving))) status.values.append(KeyValue('Temperature', str(max_temp))) status.level = DiagnosticStatus.OK status.message = 'OK' diag_msg.status.append(status) except: pass self.diagnostics_pub.publish(diag_msg) rate.sleep()
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 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 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 send_diags(): # See diagnostics with: rosrun rqt_runtime_monitor rqt_runtime_monitor msg = DiagnosticArray() msg.status = [] msg.header.stamp = rospy.Time.now() for gripper in grippers: for servo in gripper.servos: status = DiagnosticStatus() status.name = "Gripper '%s' servo %d" % (gripper.name, servo.servo_id) status.hardware_id = '%s' % servo.servo_id temperature = servo.read_temperature() status.values.append(KeyValue('Temperature', str(temperature))) status.values.append(KeyValue('Voltage', str(servo.read_voltage()))) if temperature >= 70: status.level = DiagnosticStatus.ERROR status.message = 'OVERHEATING' elif temperature >= 65: status.level = DiagnosticStatus.WARN status.message = 'HOT' else: status.level = DiagnosticStatus.OK status.message = 'OK' msg.status.append(status) diagnostics_pub.publish(msg)
def state_cb(msg): global last_publish_time now = rospy.get_rostime() if (now - last_publish_time).to_seconds() > 1.0: d = DiagnosticArray() d.header.stamp = msg.header.stamp d.status = [joint_to_diag(js) for js in msg.joint_states] pub_diag.publish(d) last_publish_time = now
def wrapper(jetson): # Load level options level_options = { rospy.get_param("~level/error", 60): DiagnosticStatus.ERROR, rospy.get_param("~level/warning", 40): DiagnosticStatus.WARN, rospy.get_param("~level/ok", 20): DiagnosticStatus.OK, } rospy.loginfo(level_options) # Initialization ros pubblisher pub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=1) # Set default rate jetson stats 2hz rate_node = rospy.get_param("~rate", 2) rate = rospy.Rate(rate_node) # Extract board information board = jetson.board # Define hardware name hardware = board["info"]["Machine"] # Define Diagnostic array message # http://docs.ros.org/api/diagnostic_msgs/html/msg/DiagnosticStatus.html arr = DiagnosticArray() # Initialization Tegrastats while not rospy.is_shutdown(): # Save all stats stats = jetson.stats # Add timestamp arr.header.stamp = rospy.Time.now() # Status board and board info arr.status = [other_status(hardware, jetson)] # Make diagnostic message for each cpu arr.status += [cpu_status(hardware, cpu) for cpu in stats['CPU']] # Merge all other diagnostics if 'GR3D' in stats: arr.status += [gpu_status(hardware, stats['GR3D'])] if 'RAM' in stats: arr.status += [ram_status(hardware, stats['RAM'], 'mem')] if 'SWAP' in stats: arr.status += [swap_status(hardware, stats['SWAP'], 'mem')] if 'EMC' in stats: arr.status += [emc_status(hardware, stats['EMC'], 'mem')] if 'FAN' in stats: arr.status += [fan_status(hardware, stats['FAN'], 'board')] if 'WATT' in stats: arr.status += [power_status(hardware, stats['WATT'])] if 'TEMP' in stats: arr.status += [temp_status(hardware, stats['TEMP'], level_options)] # Status board and board info arr.status += [board_status(hardware, board, 'board')] # Add disk status arr.status += [disk_status(hardware, jetson.disk, 'board')] # Update status jtop rospy.logdebug("jtop message %s" % rospy.get_time()) pub.publish(arr) rate.sleep()
def _publish_diag_agg(self): msg = DiagnosticArray() msg.status = [ DiagnosticStatus(level = 0, name='/Cameras', message='OK'), DiagnosticStatus(level = 1, name='/Cameras/wge100', message='Uh Oh'), DiagnosticStatus(level = 0, name='/Lasers', message='OK'), DiagnosticStatus(level = 2, name='/Other', message='Error')] msg.header.stamp = rospy.get_rostime() self.diag_agg_pub.publish(msg) self._last_diag_agg_pub = rospy.get_time()
def publish(self, msg): """Publishes a single diagnostic status or a vector of diagnostic statuses.""" if not type(msg) is list: msg = [msg] for stat in msg: stat.name = rospy.get_name()[1:]+ ": " + stat.name da = DiagnosticArray() da.status = msg da.header.stamp = rospy.Time.now() # Add timestamp for ROS 0.10 self.publisher.publish(da)
def publish(self, msg): """Publishes a single diagnostic status or a vector of diagnostic statuses.""" if not type(msg) is list: msg = [msg] for stat in msg: stat.name = rospy.get_name()[1:] + ": " + stat.name da = DiagnosticArray() da.status = msg da.header.stamp = rospy.Time.now() # Add timestamp for ROS 0.10 self.publisher.publish(da)
def publish(self, event): """ Publish current diagnostics status once a second. Runs in a separate timer thread, so locking is required. :param event: rospy.TimerEvent for this call """ with self.lock: array = DiagnosticArray() array.header.stamp = event.current_real array.status = list(self.devices.values()) self.pub.publish(array)
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 test_add_agg(self): self.wait_for_agg() # confirm that the things we're going to add aren't there already with self._mutex: agg_paths = [msg.name for name, msg in self.agg_msgs.iteritems()] self.assert_(not any(expected in agg_paths for expected in self.expected)) # add the new groups self.add_analyzer() arr = DiagnosticArray() arr.header.stamp = rospy.get_rostime() arr.status = [ DiagnosticStatus(name='primary', message='hello-primary'), DiagnosticStatus(name='secondary', message='hello-secondary') ] self.pub.publish(arr) self.wait_for_agg() # the new aggregator data should contain the extra paths. At this point # the paths are probably still in the 'Other' group because the bond # hasn't been fully formed with self._mutex: agg_paths = [msg.name for name, msg in self.agg_msgs.iteritems()] self.assert_( all(expected in agg_paths for expected in self.expected)) rospy.sleep(rospy.Duration( 5)) # wait a bit for the new items to move to the right group arr.header.stamp = rospy.get_rostime() self.pub.publish( arr) # publish again to get the correct groups to show OK self.wait_for_agg() for name, msg in self.agg_msgs.iteritems(): if name in self.expected: # should have just received messages on the analyzer self.assert_(msg.message == 'OK') agg_paths = [msg.name for name, msg in self.agg_msgs.iteritems()] self.assert_( all(expected in agg_paths for expected in self.expected)) self.bond.shutdown() rospy.sleep( rospy.Duration(5)) # wait a bit for the analyzers to unload self.wait_for_agg() # the aggregator data should no longer contain the paths once the bond is shut down with self._mutex: agg_paths = [msg.name for name, msg in self.agg_msgs.iteritems()] self.assert_(not any(expected in agg_paths for expected in self.expected))
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 publish(pub, info): diag = DiagnosticArray() diag.header.stamp = rospy.Time.now() status = DiagnosticStatus() status.hardware_id = "wifi" status.name = 'wifi_scan' status.level = status.OK status.message = pformat(info) for k, v in info.items(): status.values.append(KeyValue(k, str(v)), ) diag.status = [status] pub.publish(diag)
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 transform(self, msg): subtopic = self.params statuses = msg.status for status in statuses: if not status.name.startswith('/'): status.name = '/' + status.name def device(status): return status.name.startswith(subtopic) filtered_statuses = filter(device, statuses) new_message = DiagnosticArray() new_message.status = filtered_statuses return new_message
def publish(pub, info): diag = DiagnosticArray() diag.header.stamp = rospy.Time.now() status = DiagnosticStatus() status.hardware_id = "wifi" status.name = 'wifi_scan' status.level = status.OK status.message = pformat(info) for k,v in info.items(): status.values.append( KeyValue(k,str(v)), ) diag.status = [status] pub.publish(diag)
def diagnostics(level, msg_short, msg_long): if level == 0: rospy.loginfo(msg_long) elif level == 1: rospy.logwarn(msg_long) elif level == 2: rospy.logerr(msg_long) d = DiagnosticArray() d.header.stamp = rospy.Time.now() ds = DiagnosticStatus() ds.level = level ds.message = msg_long ds.name = msg_short d.status = [ds] pub_diag.publish(d)
def callback(self, _msg): msg = DiagnosticArray() statuses = [] for _status in _msg.status: status = DiagnosticStatus() status.level = _status.level status.name = _status.name if _status.code == 0: status.message = "" else: status.message = self.status_codes_by_module[_status.name].get( _status.code, "Unknown error") statuses.append(status) msg.status = statuses self.pub.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(): sensor = modbus(method='rtu', port='/dev/ucfsub/depth', baudrate=115200) sensor.connect() rospy.init_node('Depth') tempPub = rospy.Publisher('ExternalTemperature', Temperature, queue_size=1) depthPub = rospy.Publisher('/depth', Float32, queue_size=1) posePub = rospy.Publisher('/depth/pose', PoseWithCovarianceStamped, queue_size=1) diagPub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=1) temp = Temperature() depth = Float32() diag = DiagnosticArray() updateRate = rospy.get_param("/updateRate", 30) freq = rospy.Rate(updateRate) loop = 0 pose = PoseWithCovarianceStamped() pose.header.frame_id = "odom" pose.pose.covariance = [0.0]*36 pose.pose.covariance[14] = 0.01 pose.pose.pose.orientation.x = 0.0 pose.pose.pose.orientation.y = 0.0 pose.pose.pose.orientation.z = 0.0 pose.pose.pose.orientation.w = 1.0 pose.pose.pose.position.x = 0.0 pose.pose.pose.position.y = 0.0 while not rospy.is_shutdown(): if loop >= updateRate: rr = sensor.read_holding_registers(address=8, count=2, unit=1) if type(rr) is not type(ModbusIOException): temp.temperature = struct.unpack('>f',struct.pack('>HH', *rr.registers))[0] tempPub.publish(temp) loop = 0 loop += 1 rr = sensor.read_holding_registers(address=2, count=2, unit=1) if type(rr) is not type(ModbusIOException): depth.data = 10.2*struct.unpack('>f',struct.pack('>HH', *rr.registers))[0] pose.pose.pose.position.z = depth.data pose.header.stamp = rospy.Time.now() posePub.publish(pose) depthPub.publish(depth) diag.status = [DiagnosticStatus(name='Depth', message=str(type(rr) is type(ModbusIOException)), level=int(rr == 'true')*2)] freq.sleep()
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 _publish(self, time): """ Publishes diagnostic data""" # Limit to 1 Hz if (time - self.last_diagnostics_time).to_sec() < 1.0: return self.last_diagnostics_time = time diag = DiagnosticArray() diag.header.stamp = time # E-Stop if self.stat_estop: diag.status.append(self.stat_estop) # Power if self.stat_power: diag.status.append(self.stat_power) # Voltage if self.stat_voltage: diag.status.append(self.stat_voltage) # Temperature if self.stat_temp: diag.status.append(self.stat_temp) # Uptime if self.stat_uptime: diag.status.append(self.stat_uptime) # Publish print diag self.diag_pub.publish(diag)
def diagnostics(self, state): try: da = DiagnosticArray() ds = DiagnosticStatus() ds.name = rospy.get_caller_id().lstrip('/') + ": Node State" if state == 0: ds.level = DiagnosticStatus.OK ds.message = "%i sounds playing" % self.active_sounds ds.values.append( KeyValue("Active sounds", str(self.active_sounds))) ds.values.append( KeyValue("Allocated sound channels", str(self.num_channels))) ds.values.append( KeyValue("Buffered builtin sounds", str(len(self.builtinsounds)))) ds.values.append( KeyValue("Buffered wave sounds", str(len(self.filesounds)))) ds.values.append( KeyValue("Buffered voice sounds", str(len(self.voicesounds)))) elif state == 1: ds.level = DiagnosticStatus.WARN ds.message = "Sound device not open yet." else: ds.level = DiagnosticStatus.ERROR ds.message = "Can't open sound device. See http://wiki.ros.org/sound_play/Troubleshooting" da.status.append(ds) da.header.stamp = rospy.get_rostime() self.diagnostic_pub.publish(da) except Exception, e: rospy.loginfo('Exception in diagnostics: %s' % str(e))
def update(self): with self._mutex: diag = DiagnosticArray() diag.header.stamp = rospy.get_rostime() info_update_ok = rospy.get_time() - self._last_info_update < 5.0 / self._batt_info_rate state_update_ok = rospy.get_time() - self._last_state_update < 5.0 / self._batt_state_rate if info_update_ok: self._msg.design_capacity = self._batt_design_capacity self._msg.capacity = self._batt_last_full_capacity else: self._msg.design_capacity = 0.0 self._msg.capacity = 0.0 if info_update_ok and state_update_ok and self._msg.capacity != 0: self._msg.percentage = int(self._msg.charge / self._msg.capacity * 100.0) diag_stat = _laptop_charge_to_diag(self._msg) if not info_update_ok or not state_update_ok: diag_stat.level = DiagnosticStatus.ERROR diag_stat.message = 'Laptop battery data stale' diag.status.append(diag_stat) self._diag_pub.publish(diag) self._power_pub.publish(self._msg)
def send_qa_value_msgs(self, timeout=5.0): ############################################################################## rospy.init_node(NAME, anonymous=True) self.message_pub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=1) rospy.Subscriber("/rosout", Log, self.log_callback) timeout_t = time.time() + timeout # Default timeout 5 sec. while not rospy.is_shutdown( ) and not self.success and time.time() < timeout_t: diag_msg = DiagnosticArray() diag_msg.header.stamp = rospy.get_rostime() status_msg = DiagnosticStatus() status_msg.level = DiagnosticStatus.OK status_msg.name = "fg_print" status_msg.values.append(KeyValue(str(QA_TYPE), str(QA_VALUE))) status_msg.message = "QA status" diag_msg.status.append(status_msg) self.message_pub.publish(diag_msg) time.sleep(MSG_DELAY)
def callback(self, _msg): msg = DiagnosticArray() statuses = [] for _status in _msg.status: status = DiagnosticStatus() status.level = _status.level status.name = _status.name if _status.code == 0: status.message = "" else: status.message = self.status_codes_by_module[_status.name].get( _status.code, "Unknown error" ) statuses.append(status) msg.status = statuses self.pub.publish(msg)
class NVidiaTempMonitor(object): def __init__(self, hostname): self._pub = rospy.Publisher('/diagnostics', DiagnosticArray) self._gpu_pub = rospy.Publisher('gpu_status', GPUStatus) self.xmlMode = rospy.get_param('~/xml', False) self.hostname = hostname def pub_status(self): gpu_stat = GPUStatus() stat = DiagnosticStatus() try: card_out = computer_monitor.get_gpu_status(xml=True) gpu_stat = computer_monitor.parse_smi_output(card_out) stat = computer_monitor.gpu_status_to_diag(gpu_stat, self.hostname) except Exception, e: import traceback rospy.logerr('Unable to process nVidia GPU data') rospy.logerr(traceback.format_exc()) stat.name = '%s GPU Status' % self.hostname stat.message = 'Could not get GPU information' stat.level = DiagnosticStatus.ERROR gpu_stat.header.stamp = rospy.get_rostime() array = DiagnosticArray() array.header.stamp = rospy.get_rostime() array.status = [ stat ] self._pub.publish(array) self._gpu_pub.publish(gpu_stat)
def publish(self, state): STATE_INDEX_CHARGING = 0 STATE_INDEX_BATTERY = 1 STATE_INDEX_CONNECTION = 2 ## timed gate: limit to 1 Hz curr_time = rospy.get_rostime() if (curr_time - self.last_diagnostics_time).to_sec() < 1.0: return self.last_diagnostics_time = curr_time ## compose diagnostics message diag = DiagnosticArray() diag.header.stamp = curr_time # battery info stat = DiagnosticStatus(name="Battery", level=DiagnosticStatus.OK, message="OK") try: battery_state_code = state[STATE_INDEX_BATTERY] stat.message = self.STATE_TEXTS_BATTERY[battery_state_code] if battery_state_code < 3: stat.level = DiagnosticStatus.WARN if battery_state_code < 1: stat.level = DiagnosticStatus.ERROR stat.message = "Please Recharge Battery (%s)." % self.STATE_TEXTS_BATTERY[ battery_state_code] except KeyError as ex: stat.message = "Invalid Battery State %s" % ex rospy.logwarn("Invalid Battery State %s" % ex) stat.level = DiagnosticStatus.ERROR diag.status.append(stat) # connection info stat = DiagnosticStatus(name='ps3joy' ": Connection Type", level=DiagnosticStatus.OK, message="OK") try: stat.message = self.STATE_TEXTS_CONNECTION[ state[STATE_INDEX_CONNECTION]] except KeyError as ex: stat.message = "Invalid Connection State %s" % ex rospy.logwarn("Invalid Connection State %s" % ex) stat.level = DiagnosticStatus.ERROR diag.status.append(stat) # charging info stat = DiagnosticStatus(name='ps3joy' ": Charging State", level=DiagnosticStatus.OK, message="OK") try: stat.message = self.STATE_TEXTS_CHARGING[ state[STATE_INDEX_CHARGING]] except KeyError as ex: stat.message = "Invalid Charging State %s" % ex rospy.logwarn("Invalid Charging State %s" % ex) stat.level = DiagnosticStatus.ERROR diag.status.append(stat) # publish message self.diag_pub.publish(diag)
def update_diagnostics(self): da = DiagnosticArray() ds = DiagnosticStatus() ds.name = rospy.get_caller_id().lstrip('/') + ": Tasks" in_progress = 0 longest_interval = 0 for updater in list(asynchronous_updaters): (name, interval) = updater.getStatus() if interval == 0: msg = "Idle" else: in_progress = in_progress + 1 msg = "Update in progress (%i s)" % interval longest_interval = max(interval, longest_interval) ds.values.append(KeyValue(name, msg)) if in_progress == 0: ds.message = "Idle" else: ds.message = "Updates in progress: %i" % in_progress if longest_interval > 10: ds.level = 1 ds.message = "Update is taking too long: %i" % in_progress ds.hardware_id = "none" da.status.append(ds) da.header.stamp = rospy.get_rostime() self.diagnostic_pub.publish(da)
def publish_status(self): status = 0 messages = [] array = DiagnosticArray() for listener in self._listeners: stat, msg, diags = listener.check_ok() status = max(status, stat) if msg != '': messages.append(msg) if diags: array.status.extend(diags) if len(self._listeners) == 0: status = 3 messages = ['No listeners'] if len(array.status) > 0: self._diag_pub.publish(array) test_stat = TestStatus() test_stat.test_ok = int(status) test_stat.message = ', '.join(messages) if test_stat.test_ok == 0: test_stat.message = 'OK' self._status_pub.publish(test_stat)
def __init__(self): super().__init__('jtop_publisher') self.publisher_ = self.create_publisher(DiagnosticArray, 'diagnostics', 1) # Create Services self.fan_srv = self.create_service(Fan, '/jtop/fan', self.fan_service) self.nvpmodel_srv = self.create_service(NVPModel, '/jtop/nvpmodel', self.nvpmodel_service) self.jetson_clocks_srv = self.create_service( JetsonClocks, '/jtop/jetson_clocks', self.jetson_clocks_service) self.declare_parameter('interval', 0.5) # Default interval 0.5 self.declare_parameter('level_error', 60) # Default interval 0.5 self.declare_parameter('level_warning', 40) # Default interval 0.5 self.declare_parameter('level_ok', 20) # Default interval 0.5 self.level_options = { self.get_parameter('level_error')._value: DiagnosticStatus.ERROR, self.get_parameter('level_warning')._value: DiagnosticStatus.WARN, self.get_parameter('level_ok')._value: DiagnosticStatus.OK, } timer_period = self.get_parameter('interval')._value self.timer = self.create_timer(timer_period, self.jetson_callback) self.i = 0 self.jetson = jtop.jtop(interval=0.5) self.arr = DiagnosticArray() self.get_logger().info( "Jetson Stats has started with interval : {}\n You can run following:\n 1. $ros2 run rqt_topic rqt_topic \n 2. Services for controlling fan_speed, power_mode, jetson_clocks\n" .format(timer_period))
def make_diagnostic(origin, static_origin): diagnostic = DiagnosticArray() diagnostic.header.stamp = rospy.Time.now() status = DiagnosticStatus() status.name = "LocalXY Origin" status.hardware_id = "origin_publisher" if origin == None: status.level = DiagnosticStatus.ERROR status.message = "No Origin" else: if not static_origin: status.level = DiagnosticStatus.OK status.message = "Has Origin (auto)" else: status.level = DiagnosticStatus.WARN status.message = "Origin is static (non-auto)" frame_id = origin.header.frame_id status.values.append(KeyValue(key="Origin Frame ID", value=frame_id)) latitude = "%f" % origin.pose.position.y status.values.append(KeyValue(key="Latitude", value=latitude)) longitude = "%f" % origin.pose.position.x status.values.append(KeyValue(key="Longitude", value=longitude)) altitude = "%f" % origin.pose.position.z status.values.append(KeyValue(key="Altitude", value=altitude)) diagnostic.status.append(status) return diagnostic
def test_add_agg(self): self.wait_for_agg() # confirm that the things we're going to add aren't there already with self._mutex: agg_paths = [msg.name for name, msg in self.agg_msgs.iteritems()] self.assert_(not any(expected in agg_paths for expected in self.expected)) # add the new groups self.add_analyzer() arr = DiagnosticArray() arr.header.stamp = rospy.get_rostime() arr.status = [ DiagnosticStatus(name='primary', message='hello-primary'), DiagnosticStatus(name='secondary', message='hello-secondary') ] self.pub.publish(arr) self.wait_for_agg() # the new aggregator data should contain the extra paths. At this point # the paths are probably still in the 'Other' group because the bond # hasn't been fully formed with self._mutex: agg_paths = [msg.name for name, msg in self.agg_msgs.iteritems()] self.assert_(all(expected in agg_paths for expected in self.expected)) rospy.sleep(rospy.Duration(5)) # wait a bit for the new items to move to the right group arr.header.stamp = rospy.get_rostime() self.pub.publish(arr) # publish again to get the correct groups to show OK self.wait_for_agg() for name, msg in self.agg_msgs.iteritems(): if name in self.expected: # should have just received messages on the analyzer self.assert_(msg.message == 'OK') agg_paths = [msg.name for name, msg in self.agg_msgs.iteritems()] self.assert_(all(expected in agg_paths for expected in self.expected)) self.bond.shutdown() self.wait_for_agg() # the aggregator data should no longer contain the paths once the bond is shut down with self._mutex: agg_paths = [msg.name for name, msg in self.agg_msgs.iteritems()] self.assert_(not any(expected in agg_paths for expected in self.expected))
def transform(self, msg): statuses = msg.status for status in statuses: if not status.name.startswith('/'): status.name = '/' + status.name def top_only(status): return status.name.count("/") < 2 filtered_statuses = filter(top_only, statuses) #rospy.loginfo("statuses: %s" % statuses) #rospy.loginfo("f_statuses: %s" % filtered_statuses) #for status in filtered_statuses: # rospy.loginfo("name: %s" % status.name) new_message = DiagnosticArray() new_message.status = filtered_statuses return new_message
def publish(): rospy.init_node('Leak') leakPub = rospy.Publisher('/leak', Bool, queue_size=1) diagPub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=1) #GPIO.setmode(GPIO.BCM) #GPIO.setup(CHANNEL, GPIO.IN) gpio.setup(CHANNEL, gpio.IN) freq = rospy.Rate(1) leak = Bool() diag = DiagnosticArray() while not rospy.is_shutdown(): levelData=0 leak.data = gpio.input(CHANNEL) if (leak.data): levelData=2 diag.status = [DiagnosticStatus(name='Leak', message=str(leak.data), level=levelData)] leakPub.publish(leak) diagPub.publish(diag) freq.sleep()
def publish_diags(self, strategy): now = rospy.get_rostime() ns = strategy.ns ds = self.fill_diags("synthetic interface", ns.diag_summary, self.hostname, strategy.ns.diags) ds.level = ns.diag_level statuses = [ds] for i in range(0, len(ns.interfaces)): iface = ns.interfaces[i] ds = self.fill_diags(iface.iface, iface.diag_summary, self.hostname, iface.diags) statuses.append(ds) if iface.__class__ == mir.WirelessInterface: msg = self.gen_accesspoint_msg(iface) msg.header.stamp = now self.wifi_sub_pub[iface.iface].publish(msg) if i == ns.active_iface: self.wifi_pub.publish(msg) da = DiagnosticArray() da.header.stamp = rospy.get_rostime() da.status = statuses self.diag_pub.publish(da)
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)
DiagnosticStatus(0, '/BASE/group1/item2', 'OK', '', []), DiagnosticStatus(0, '/BASE/group2', 'OK', '', []), DiagnosticStatus(0, '/BASE/group2/item1', 'OK', '', []), DiagnosticStatus(2, '/BASE2', 'OK', '', []), DiagnosticStatus(2, '/BASE2/group3', 'OK', '', []), DiagnosticStatus(2, '/BASE2/group3/item2', 'Error', '', []), #DiagnosticStatus(0, '/BASE3', 'OK', '', []), DiagnosticStatus(0, '/BASE3/group1', 'OK', '', []), DiagnosticStatus(0, '/BASE3/group1/item2', 'Error', '', []) ] ] array = DiagnosticArray() array.header.stamp = rospy.get_rostime() i = 0 ind = 0 while not rospy.is_shutdown(): old_ind = ind ind = 0 if (i % 10 > 5): ind = 1 i = i + 1 if (old_ind != ind): print "switched", ind
def __publish_diagnostic_information(self): diag_msg = DiagnosticArray() rate = rospy.Rate(self.diagnostics_rate) while not rospy.is_shutdown() and self.running: diag_msg.status = [] diag_msg.header.stamp = rospy.Time.now() status = DiagnosticStatus() status.name = 'Dynamixel Serial Bus (%s)' % self.port_namespace status.hardware_id = 'Dynamixel Serial Bus on port %s' % self.port_name status.values.append(KeyValue('Baud Rate', str(self.baud_rate))) status.values.append(KeyValue('Min Motor ID', str(self.min_motor_id))) status.values.append(KeyValue('Max Motor ID', str(self.max_motor_id))) status.values.append(KeyValue('Desired Update Rate', str(self.update_rate))) status.values.append(KeyValue('Actual Update Rate', str(self.actual_rate))) status.values.append(KeyValue('# Non Fatal Errors', str(self.error_counts['non_fatal']))) status.values.append(KeyValue('# Checksum Errors', str(self.error_counts['checksum']))) status.values.append(KeyValue('# Dropped Packet Errors', str(self.error_counts['dropped']))) status.level = DiagnosticStatus.OK status.message = 'OK' if self.actual_rate - self.update_rate < -5: status.level = DiagnosticStatus.WARN status.message = 'Actual update rate is lower than desired' diag_msg.status.append(status) for motor_state in self.current_state.motor_states: mid = motor_state.id status = DiagnosticStatus() status.name = 'Robotis Dynamixel Motor %d on port %s' % (mid, self.port_namespace) status.hardware_id = 'DXL-%d@%s' % (motor_state.id, self.port_namespace) status.values.append(KeyValue('Model Name', str(self.motor_static_info[mid]['model']))) status.values.append(KeyValue('Firmware Version', str(self.motor_static_info[mid]['firmware']))) status.values.append(KeyValue('Return Delay Time', str(self.motor_static_info[mid]['delay']))) status.values.append(KeyValue('Minimum Voltage', str(self.motor_static_info[mid]['min_voltage']))) status.values.append(KeyValue('Maximum Voltage', str(self.motor_static_info[mid]['max_voltage']))) status.values.append(KeyValue('Minimum Position (CW)', str(self.motor_static_info[mid]['min_angle']))) status.values.append(KeyValue('Maximum Position (CCW)', str(self.motor_static_info[mid]['max_angle']))) status.values.append(KeyValue('Goal', str(motor_state.goal))) status.values.append(KeyValue('Position', str(motor_state.position))) status.values.append(KeyValue('Error', str(motor_state.error))) status.values.append(KeyValue('Velocity', str(motor_state.speed))) status.values.append(KeyValue('Load', str(motor_state.load))) status.values.append(KeyValue('Voltage', str(motor_state.voltage))) status.values.append(KeyValue('Temperature', str(motor_state.temperature))) status.values.append(KeyValue('Moving', str(motor_state.moving))) if motor_state.temperature >= self.error_level_temp: status.level = DiagnosticStatus.ERROR status.message = 'OVERHEATING' elif motor_state.temperature >= self.warn_level_temp: status.level = DiagnosticStatus.WARN status.message = 'VERY HOT' else: status.level = DiagnosticStatus.OK status.message = 'OK' diag_msg.status.append(status) self.diagnostics_pub.publish(diag_msg) rate.sleep()
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_status(self): now = rospy.get_rostime() # current_iface_id ai = self.interface_selector.active_interfaces if not ai or ai[0] not in self._interfaces: index = -1 else: index = self._interfaces.index(ai[0]) self.iface_id_pub.publish(index) # accesspoint best_active = self.interface_selector.radio_manager.best_active for iface in self._wireless_interfaces: msg = self.gen_accesspoint_msg(iface) msg.header.stamp = now self.all_ap_pub[iface].publish(msg) if iface == best_active: self.ap_pub.publish(msg) if best_active is None: self.ap_pub.publish(AccessPoint()) # status msg = MultiInterfaceStatus() for iface in self._interfaces: msg.interfaces.append(self.gen_status_msg(iface)) self.status_pub.publish(msg) # diagnostics diags = [] diags.append(('Tunnel Interface', self.interface_selector.tunnel_interface)) if self.interface_selector.active_interfaces and \ self.interface_selector.active_interfaces[0].score != self.interface_selector.TERRIBLE_INTERFACE: act_iface = self.interface_selector.active_interfaces[0] diags.append(('Active Interface', act_iface.iface )) diags += act_iface.diags if act_iface.goodness > 95: diag_summary = "Active interface %s running strong"%act_iface.iface diag_level = 0 elif act_iface.goodness > 50: diag_summary = "Active interface %s is lossy"%act_iface.iface diag_level = 1 else: if act_iface.goodness > 0: diag_summary = "Active interface %s is very poor"%act_iface.iface else: diag_summary = "Active interface %s is failing to ping"%act_iface.iface diag_level = 2 else: diags.append(('Active Interface', "none")) diag_summary = 'No active interface' diag_level = 2 ds = self.fill_diags("synthetic interface", diag_summary, self.hostname, diags) ds.level = diag_level statuses = [ds] for iface in self._interfaces: status = iface.status if status == InterfaceStatus.STATE_ADDR and iface.ping_loss < 100: status = InterfaceStatus.STATE_PINGING diag_summary = "Connected (goodness %f, reliability %f)"%(iface.goodness, iface.reliability) else: diag_summary = STATUSES[status] ds = self.fill_diags(iface.iface, diag_summary, self.hostname, iface.diags) statuses.append(ds) da = DiagnosticArray() da.header.stamp = rospy.get_rostime() da.status = statuses self.diag_pub.publish(da)
#!/usr/bin/env python import roslib; roslib.load_manifest('maggie_eyelids_analyzer') import rospy from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue if __name__ == '__main__': rospy.init_node('eyelids_node_main') pub = rospy.Publisher('/diagnostics', DiagnosticArray) array = DiagnosticArray() # Right eyelid eye_right_stat = DiagnosticStatus(name = 'Right Eyelid', level = 0, message = 'Connected') #Left eyelid eye_left_stat = DiagnosticStatus(name='Left Eyelid', level = 0, message = 'Connected') array.status = [ eye_right_stat, eye_left_stat ] my_rate = rospy.Rate(1.0) while not rospy.is_shutdown(): pub.publish(array) my_rate.sleep()
import roslib; roslib.load_manifest(PKG) import rospy from time import sleep from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus if __name__ == '__main__': rospy.init_node('diag_pub') pub = rospy.Publisher('/diagnostics', DiagnosticArray) start_time = rospy.get_time() while not rospy.is_shutdown(): array = DiagnosticArray() array.status = [ DiagnosticStatus(0, 'EtherCAT Device (fl_caster_l_wheel_motor)', 'OK', '', []), DiagnosticStatus(0, 'EtherCAT Device (fl_caster_r_wheel_motor)', 'OK', '', []), DiagnosticStatus(0, 'EtherCAT Device (fl_caster_rotation_motor)', 'OK', '', []), DiagnosticStatus(2, 'EtherCAT Device (fr_caster_l_wheel_motor)', 'Motor model', '', []), DiagnosticStatus(1, 'EtherCAT Device (fr_caster_r_wheel_motor)', 'High temperature', '', []), DiagnosticStatus(0, 'EtherCAT Device (fr_caster_rotation_motor)', 'OK', '', []), DiagnosticStatus(0, 'tilt_hokuyo_node: Frequency Status', 'OK', '', []), DiagnosticStatus(0, 'tilt_hokuyo_node: Connection Status', 'OK', '', []), DiagnosticStatus(0, 'base_hokuyo_node: Frequency Status', 'OK', '', []), DiagnosticStatus(0, 'base_hokuyo_node: Connection Status', 'OK', '', []), DiagnosticStatus(0, 'Joint (fl_caster_l_wheel_joint)', 'OK', '', []), DiagnosticStatus(0, 'Joint (fl_caster_r_wheel_joint)', 'OK', '', []),
import roslib; roslib.load_manifest(PKG) import rospy from time import sleep from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue if __name__ == '__main__': nodeName = 'tasmania_ActCap1Func2' rospy.init_node('diag_'+nodeName+'_pub2') pub = rospy.Publisher('/diagnostics', DiagnosticArray) array = DiagnosticArray() array.status = [ # GenericAnalyzer prefix1 #DiagnosticStatus(0, nodeName, 'msg: all OK', '', [KeyValue('internal_state_valTEST', '4')]), #DiagnosticStatus(0, nodeName, 'msg: all OK', '', [KeyValue('CompNotifierType', 'CompHeartBeatInterval'), KeyValue('Interval', '700') ]), DiagnosticStatus(0, nodeName, 'msg: xxx', '', [KeyValue('CharacType', 'CompCpu'), KeyValue('Measurement', '300') ]), #DiagnosticStatus(0, nodeName, 'msg: all OK', '', [KeyValue('CharacType', 'CompMem'), KeyValue('Measurement', '3000') ]) ] #array.header.stamp = rospy.get_rostime() array.header.stamp = rospy.Time.now() #print rospy.Time.now() #print rospy.get_rostime() count = 0 while not rospy.is_shutdown(): array.header.stamp = rospy.get_rostime()
import roslib; roslib.load_manifest(PKG) import rospy from time import sleep from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus if __name__ == '__main__': rospy.init_node('diag_pub') pub = rospy.Publisher('/diagnostics', DiagnosticArray) start_time = rospy.get_time() while not rospy.is_shutdown(): array = DiagnosticArray() array.header.stamp = rospy.get_rostime() array.status = [ # GenericAnalyzer my_path DiagnosticStatus(0, 'multi', 'OK', '', []), DiagnosticStatus(1, 'Something', 'OK', '', []), DiagnosticStatus(2, 'Something Else', 'OK', '', []), # OtherAnalyzer for Other DiagnosticStatus(2, 'other2', 'OK', '', []), DiagnosticStatus(0, 'other3', 'OK', '', [])] pub.publish(array) sleep(1)
##\brief Publishes messages for aggregator testing of expected items. PKG = 'test_diagnostic_aggregator' import rospy from time import sleep from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus if __name__ == '__main__': rospy.init_node('diag_pub') pub = rospy.Publisher('/diagnostics', DiagnosticArray) while not rospy.is_shutdown(): array = DiagnosticArray() array.header.stamp = rospy.get_rostime() array.status = [ # Match no analyze analyzer DiagnosticStatus(0, 'Match Item', 'OK', '', []), # Generic for something else DiagnosticStatus(1, 'Something', 'OK', '', []), DiagnosticStatus(2, 'Something Else', 'OK', '', []), # OtherAnalyzer for Other DiagnosticStatus(2, 'other1', 'OK', '', []), DiagnosticStatus(1, 'other2', 'OK', '', []), DiagnosticStatus(0, 'other3', 'OK', '', [])] pub.publish(array)
#S = tok[0][1] #all is good if we are synchronizing to a source if tok[0][1] == '*': status.level = 0 #print M, S except subprocess.CalledProcessError as e: rospy.logerr(e.output) status.values.append(KeyValue(key=e.output, value=chr(2))) if __name__ == '__main__': 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')
import roslib; roslib.load_manifest(PKG) import rospy from time import sleep from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus if __name__ == '__main__': rospy.init_node('diag_pub') pub = rospy.Publisher('/diagnostics', DiagnosticArray) start_time = rospy.get_time() while not rospy.is_shutdown(): array = DiagnosticArray() array.status = [ # GenericAnalyzer my_path DiagnosticStatus(0, 'expected1', 'OK', '', []), DiagnosticStatus(1, 'expected2', 'OK', '', []), DiagnosticStatus(2, 'expected3', 'OK', '', []), DiagnosticStatus(0, 'startswith1', 'OK', '', []), DiagnosticStatus(0, 'startswith2', 'OK', '', []), DiagnosticStatus(1, 'startswith3', 'OK', '', []), # OtherAnalyzer for Other DiagnosticStatus(2, 'other2', 'OK', '', []), DiagnosticStatus(0, 'other3', 'OK', '', [])] array.header.stamp = rospy.get_rostime()