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 publish_diagnostic(self): try: da = DiagnosticArray() ds = DiagnosticStatus() ds.name = rospy.get_caller_id().lstrip('/') + ': Status' ds.hardware_id = self.ifname if not self.iwconfig.is_enabled(): ds.level = DiagnosticStatus.STALE ds.message = "Device not found" elif not self.iwconfig.is_connected(): ds.level = DiagnosticStatus.ERROR ds.message = "No connection" else: if extract_number(self.iwconfig.status["Link Quality"]) < self.warn_quality: ds.level = DiagnosticStatus.WARN ds.message = "Connected, but bad quality" else: ds.level = DiagnosticStatus.OK ds.message = "Connected" for key, val in self.iwconfig.status.items(): ds.values.append(KeyValue(key, val)) da.status.append(ds) da.header.stamp = rospy.Time.now() self.diagnostic_pub.publish(da) except Exception as e: rospy.logerr('Failed to publish diagnostic: %s' % str(e)) rospy.logerr(traceback.format_exc())
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 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 _publish_diagnostic(self): """Publish diagnostics.""" diagnostic = DiagnosticArray() diagnostic.header.stamp = rospy.Time.now() status = DiagnosticStatus() status.name = "LocalXY Origin" status.hardware_id = "origin_publisher" if self.origin is None: status.level = DiagnosticStatus.ERROR status.message = "No Origin" else: if self.origin_source == "gpsfix": status.level = DiagnosticStatus.OK status.message = "Has Origin (GPSFix)" elif self.origin_source == "navsat": status.level = DiagnosticStatus.OK status.message = "Has Origin (NavSatFix)" else: status.level = DiagnosticStatus.WARN status.message = "Origin Was Set Manually" frame_id = self.origin.header.frame_id status.values.append(KeyValue(key="Origin Frame ID", value=frame_id)) latitude = "%f" % self.origin.pose.position.y status.values.append(KeyValue(key="Latitude", value=latitude)) longitude = "%f" % self.origin.pose.position.x status.values.append(KeyValue(key="Longitude", value=longitude)) altitude = "%f" % self.origin.pose.position.z status.values.append(KeyValue(key="Altitude", value=altitude)) diagnostic.status.append(status) self.diagnostic_pub.publish(diagnostic)
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 _laptop_charge_to_diag(laptop_msg): rv = DiagnosticStatus() rv.level = DiagnosticStatus.OK rv.message = 'OK' rv.name = 'Laptop Battery' rv.hardware_id = socket.gethostname() if not laptop_msg.present: rv.level = DiagnosticStatus.ERROR rv.message = 'Laptop battery missing' charging_state = { SmartBatteryStatus.DISCHARGING: 'Not Charging', SmartBatteryStatus.CHARGING: 'Charging', SmartBatteryStatus.CHARGED: 'Trickle Charging' }.get(laptop_msg.charge_state, 'Not Charging') rv.values.append(KeyValue('Voltage (V)', str(laptop_msg.voltage))) rv.values.append(KeyValue('Current (A)', str(laptop_msg.rate))) rv.values.append(KeyValue('Charge (Ah)', str(laptop_msg.charge))) rv.values.append(KeyValue('Capacity (Ah)', str(laptop_msg.capacity))) rv.values.append(KeyValue('Design Capacity (Ah)', str(laptop_msg.design_capacity))) rv.values.append(KeyValue('Charging State', str(charging_state))) return rv
def publish_diagnostics(self, time): diag = DiagnosticArray() diag.header.stamp = time stat = DiagnosticStatus(name="Battery Status", level=DiagnosticStatus.OK, message=self.power_state_msg) if self.power_state == 3: stat.level=DiagnosticStatus.WARN if self.power_state == 4: stat.level=DiagnosticStatus.ERROR diag.status.append(stat) self.diag_pub.publish(diag)
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 mark_diag_stale(diag_stat = None, error = False): if not diag_stat: diag_stat = DiagnosticStatus() diag_stat.message = 'No Updates' diag_stat.name = DIAG_NAME else: diag_stat.message = 'Updates Stale' diag_stat.level = DiagnosticStatus.WARN if error: diag_stat.level = DiagnosticStatus.ERROR return diag_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 publish_diagnostics(self, time): diag = DiagnosticArray() diag.header.stamp = time for sphero_name in self.sphero_dict: sphero = self.sphero_dict[sphero_name] stat = DiagnosticStatus(name=sphero_name, level=DiagnosticStatus.OK, message=sphero.power_state_msg) #stat.message="Battery Status" if sphero.power_state == 3: stat.level=DiagnosticStatus.WARN if sphero.power_state == 4: stat.level=DiagnosticStatus.ERROR diag.status.append(stat) self.diag_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 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 spin(self): r = rospy.Rate(self._diagnostic_freq) while not rospy.is_shutdown(): diag = DiagnosticArray() diag.header.stamp = rospy.get_rostime() for mon in self._monitors: d = DiagnosticStatus() d.name=mon.get_name() d.hardware_id="PC0" d.message=mon.get_field_value("status") d.level=mon.get_field_value("status_level") for key in mon.get_fields(): p = KeyValue() p.key = key p.value = str(mon.get_field_value(key)) d.values.append(p) diag.status.append(d) self._diagnostic_pub.publish(diag) r.sleep() self._cpu_temp_mon.stop() self._cpu_usage_mon.stop() self._wlan_mon.stop() self._bandwidth_mon.stop()
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 _laptop_charge_to_diag(laptop_msg): rv = DiagnosticStatus() rv.level = DiagnosticStatus.OK rv.message = 'OK' rv.name = 'Laptop Battery' if not laptop_msg.present: rv.level = DiagnosticStatus.ERROR rv.message = 'Laptop battery missing' rv.values.append(KeyValue('Voltage (V)', str(laptop_msg.voltage))) rv.values.append(KeyValue('Current (A)', str(laptop_msg.rate))) rv.values.append(KeyValue('Charge (Ah)', str(laptop_msg.charge))) rv.values.append(KeyValue('Capacity (Ah)', str(laptop_msg.capacity))) rv.values.append(KeyValue('Design Capacity (Ah)', str(laptop_msg.design_capacity))) return rv
def getEtherbotixDiagnostics(self, etherbotix): msg = DiagnosticStatus() msg.name = "etherbotix" msg.level = DiagnosticStatus.OK msg.message = "OK" msg.hardware_id = etherbotix.getUniqueId() # System Voltage if etherbotix.system_voltage < 10.0: msg.level = DiagnosticStatus.ERROR msg.message = "Battery depleted!" msg.values.append(KeyValue("Voltage", str(etherbotix.system_voltage)+"V")) # Currents msg.values.append(KeyValue("Servo Current", str(etherbotix.servo_current)+"A")) msg.values.append(KeyValue("Aux. Current", str(etherbotix.aux_current)+"A")) msg.values.append(KeyValue("Packets", str(etherbotix.packets_recv))) msg.values.append(KeyValue("Packets Bad", str(etherbotix.packets_bad))) return msg
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 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 publish_diagnostics(self, time): diag = DiagnosticArray() diag.header.stamp = time values = [] values.append(KeyValue(key="voltage", value=str(self.batt_voltage))) values.append(KeyValue(key="numCharges", value=str(self.num_charges))) values.append(KeyValue(key="timeSinceCharge", value=str(self.time_since_chg))) stat = DiagnosticStatus(name="Battery Status", level=DiagnosticStatus.OK, message=self.power_state_msg, values=values) if self.power_state == 3: stat.level=DiagnosticStatus.WARN if self.power_state == 4: stat.level=DiagnosticStatus.ERROR diag.status.append(stat) self.diag_pub.publish(diag)
def _publish_diag(self): ok = False with self._mutex: ok = self._ok msg = DiagnosticArray() stat = DiagnosticStatus() msg.status.append(stat) # Simulate pr2_etherCAT diagnostics stat.level = 0 if not ok: stat.level = 2 stat.name = 'EtherCAT Master' stat.message = 'OK' stat.values.append(KeyValue('Dropped Packets', '0')) stat.values.append(KeyValue('RX Late Packet', '0')) # Check for encoder errors mcb_stat = DiagnosticStatus() mcb_stat.name = 'EtherCAT Device (my_motor)' mcb_stat.level = 0 mcb_stat.values.append(KeyValue('Num encoder_errors', '0')) # Test camera listener stat_cam = DiagnosticStatus() stat_cam.level = 0 stat_cam.name = 'wge100: Frequency Status' stat_cam.message = 'OK' # Test HK listener stat_hk = DiagnosticStatus() stat_hk.level = 0 stat_hk.name = 'tilt_hokuyo_node: Frequency Status' stat_hk.message = 'OK' msg.status.append(stat_cam) msg.status.append(mcb_stat) msg.status.append(stat_hk) msg.header.stamp = rospy.get_rostime() self.diag_pub.publish(msg) self._last_diag_pub = rospy.get_time()
def _ecat_diag(enc_errors = 0): array = DiagnosticArray() stat = DiagnosticStatus() stat.name = 'EtherCAT Master' stat.level = 0 stat.message = 'OK' stat.values.append(KeyValue('Dropped Packets', '0')) stat.values.append(KeyValue('RX Late Packet', '0')) mcb_stat = DiagnosticStatus() mcb_stat.name = 'EtherCAT Device (my_motor)' mcb_stat.level = 0 mcb_stat.values.append(KeyValue('Num encoder_errors', str(enc_errors))) array.header.stamp = rospy.get_rostime() array.status.append(stat) array.status.append(mcb_stat) return array
def publish_diagnostics(self, err=None): ''' Publishes current kill board state in ROS diagnostics package, making it easy to use in GUIs and other ROS tools ''' msg = DiagnosticArray() msg.header.stamp = rospy.Time.now() status = DiagnosticStatus() status.name = 'kill_board' status.hardware_id = self.port if not self.connected: status.level = DiagnosticStatus.ERROR status.message = 'Cannot connect to kill board. Retrying every second.' status.values.append(KeyValue("Exception", str(err))) else: status.level = DiagnosticStatus.OK for key, value in self.board_status.items(): status.values.append(KeyValue(key, str(value))) msg.status.append(status) self.diagnostics_pub.publish(msg)
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 _camera_diag(level = 0): array = DiagnosticArray() stat = DiagnosticStatus() stat.name = 'wge100: Driver Status' stat.level = level stat.message = 'OK' array.header.stamp = rospy.get_rostime() array.status.append(stat) return array
def gpu_status_to_diag(gpu_stat): stat = DiagnosticStatus() stat.name = socket.gethostname() + ' GPU Status' stat.message = '' stat.level = DiagnosticStatus.OK stat.hardware_id = socket.gethostname() + "/" + gpu_stat.product_name stat.values.append(KeyValue(key='Product Name', value = gpu_stat.product_name)) #stat.values.append(KeyValue(key='PCI Device/Vendor ID', value = gpu_stat.pci_device_id)) #stat.values.append(KeyValue(key='PCI Location ID', value = gpu_stat.pci_location)) #stat.values.append(KeyValue(key='Display', value = gpu_stat.display)) stat.values.append(KeyValue(key='Driver Version', value = gpu_stat.driver_version)) stat.values.append(KeyValue(key='Temperature (C)', value = '%.0f' % gpu_stat.temperature)) #stat.values.append(KeyValue(key='Fan Speed (RPM)', value = '%.0f' % _rads_to_rpm(gpu_stat.fan_speed))) #stat.values.append(KeyValue(key='Usage (%)', value = '%.0f' % gpu_stat.gpu_usage)) stat.values.append(KeyValue(key='Memory (%)', value = '%.0f' % gpu_stat.memory_usage)) errors = [] # Check for valid data if not gpu_stat.product_name: stat.level = DiagnosticStatus.ERROR errors.append('No Device Data') else: # Check load if gpu_stat.gpu_usage > 95: stat.level = max(stat.level, DiagnosticStatus.WARN) errors.append('High Load') # Check thresholds if gpu_stat.temperature > 95: stat.level = max(stat.level, DiagnosticStatus.ERROR) errors.append('Temperature Alarm') elif gpu_stat.temperature > 90: stat.level = max(stat.level, DiagnosticStatus.WARN) errors.append('High Temperature') # Check memory usage if gpu_stat.memory_usage > 95: stat.level = max(stat.level, DiagnosticStatus.ERROR) errors.append('Memory critical') elif gpu_stat.memory_usage > 90: stat.level = max(stat.level, DiagnosticStatus.WARN) errors.append('Low Memory') # Check fan #if gpu_stat.fan_speed == 0: # stat.level = max(stat.level, DiagnosticStatus.ERROR) # stat.message = 'No Fan Speed' if not errors: stat.message = 'OK' else: stat.message = ', '.join(errors) return stat
def get_status(self): stat = 0 if not self._ok: stat = 2 if rospy.get_time() - self._update_time > 3: stat = 3 diag = DiagnosticStatus() diag.level = stat diag.name = 'Caster Slip Listener' diag.message = 'OK' if diag.level == 2: diag.message = 'Caster Slipping' if diag.level == 3: diag.message = 'Caster Stale' diag.level = 2 diag.values.append(KeyValue("Turret", str(TURRET_NAME))) diag.values.append(KeyValue("R Wheel", str(R_WHEEL_NAME))) diag.values.append(KeyValue("L Wheel", str(L_WHEEL_NAME))) diag.values.append(KeyValue("Turret Position", str(self.last_position.turret))) diag.values.append(KeyValue("R Wheel Position", str(self.last_position.r_wheel))) diag.values.append(KeyValue("L Wheel Position", str(self.last_position.l_wheel))) diag.values.append(KeyValue("Max Pos. Left Slip", str(self._max_l_err_pos))) diag.values.append(KeyValue("Max Neg. Left Slip", str(self._max_l_err_neg))) diag.values.append(KeyValue("Max Pos. Right Slip", str(self._max_r_err_pos))) diag.values.append(KeyValue("Max Neg. Right Slip", str(self._max_r_err_neg))) diag.values.append(KeyValue("Max Pos. Left Slip (Reset)", str(self._max_l_err_pos_reset))) diag.values.append(KeyValue("Max Neg. Left Slip (Reset)", str(self._max_l_err_neg_reset))) diag.values.append(KeyValue("Max Pos. Right Slip (Reset)", str(self._max_r_err_pos_reset))) diag.values.append(KeyValue("Max Neg. Right Slip (Reset)", str(self._max_r_err_neg_reset))) diag.values.append(KeyValue("Wheel Offset", str(WHEEL_OFFSET))) diag.values.append(KeyValue("Wheel Diameter", str(WHEEL_RADIUS))) diag.values.append(KeyValue("Allowed Slip", str(ALLOWED_SLIP))) diag.values.append(KeyValue("Update Interval", str(UPDATE_INTERVAL))) diag.values.append(KeyValue("Total Errors", str(self._num_errors))) diag.values.append(KeyValue("Errors Since Reset", str(self._num_errors_since_reset))) return diag
def sensor_status_to_diag(sensorList, hostname='localhost', ignore_fans=False): stat = DiagnosticStatus() stat.name = '%s Sensor Status'%hostname stat.message = 'OK' stat.level = DiagnosticStatus.OK stat.hardware_id = hostname for sensor in sensorList: if sensor.getType() == "Temperature": if sensor.getInput() > sensor.getCrit(): stat.level = max(stat.level, DiagnosticStatus.ERROR) stat.message = "Critical Temperature" elif sensor.getInput() > sensor.getHigh(): stat.level = max(stat.level, DiagnosticStatus.WARN) stat.message = "High Temperature" stat.values.append(KeyValue(key=" ".join([sensor.getName(), sensor.getType()]), value=str(sensor.getInput()))) elif sensor.getType() == "Voltage": if sensor.getInput() < sensor.getMin(): stat.level = max(stat.level, DiagnosticStatus.ERROR) stat.message = "Low Voltage" elif sensor.getInput() > sensor.getMax(): stat.level = max(stat.level, DiagnosticStatus.ERROR) stat.message = "High Voltage" stat.values.append(KeyValue(key=" ".join([sensor.getName(), sensor.getType()]), value=str(sensor.getInput()))) elif sensor.getType() == "Speed": if not ignore_fans: if sensor.getInput() < sensor.getMin(): stat.level = max(stat.level, DiagnosticStatus.ERROR) stat.message = "No Fan Speed" stat.values.append(KeyValue(key=" ".join([sensor.getName(), sensor.getType()]), value=str(sensor.getInput()))) return stat
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
pub_diagnostics = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=1) rospy.loginfo("fake diagnostics for %s running listening to %s", diagnostics_name, topic_name) rate = rospy.Rate(1) while not rospy.is_shutdown(): # if no topic_name is set, we assume that we received a if topic_name == None: last_received_ = rospy.Time.now() # only publish ok if message received recently if rospy.Time.now() - last_received_ <= rospy.Duration(10.0): status = DiagnosticStatus() status.level = 0 status.name = diagnostics_name status.message = diagnostics_name + " running" diagnostics = DiagnosticArray() diagnostics.header.stamp = rospy.Time.now() diagnostics.status.append(status) else: status = DiagnosticStatus() status.level = 2 status.name = diagnostics_name status.message = "no message received on " + topic_name diagnostics = DiagnosticArray() diagnostics.header.stamp = rospy.Time.now() diagnostics.status.append(status) pub_diagnostics.publish(diagnostics) try:
def hwboard(self): # initialize local variables send_channel = 0 read_channel = 0 send_specifier = 0 read_specifier = 0 read_status = 0 read_data = 0 read_id = 0 read_crc = 0 # init ros-node pub = rospy.Publisher('diagnostics', DiagnosticArray) while not rospy.is_shutdown(): # init publisher message pub_message = DiagnosticArray() # init array for storing data status_array = [] # init local variable for error detection error_while_reading = 0 for send_specifier in range(0, 7, 3): if send_specifier == 0: count_range = range(6) elif send_specifier == 3: count_range = [0, 1, 2, 3, 6, 7] else: count_range = [1, 2, 3, 6, 7] for send_channel in count_range: # init message and local variables send_buff_array = [ send_channel, send_specifier, 0x00, 0x00, 0x00 ] message = "" preamble_bytes = 4 preamble_error = 1 crc_error = 1 retry = 0 # calculate crc crc = 0x00 for i in range(4): data = send_buff_array[i] for k in range(8): feedback_bit = (crc ^ data) & 0x80 feedback_bit = (feedback_bit >> 7) & 0xFF if feedback_bit == 1: crc = (crc << 1) & 0xFF crc = crc ^ 0x31 else: crc = (crc << 1) & 0xFF data = (data << 1) & 0xFF send_buff_array[4] = crc # send message while (preamble_error == 1 or crc_error == 1) and retry < 8: message = "" for i in range(preamble_bytes): message += chr(0x55) for i in send_buff_array: message += chr(i) self.s.write(message) # receive message # check for first preamble byte of reveiced message read_buff_array = [] buff = self.s.read(1) preamble_count = 0 for i in buff: read_buff_array.append(ord(i)) if read_buff_array[0] == 0x55: # check for following preamble bytes while read_buff_array[ 0] == 0x55 and preamble_count < 10: read_buff_array = [] buff = self.s.read(1) for i in buff: read_buff_array.append(ord(i)) preamble_count = preamble_count + 1 buff = self.s.read(13) # check preamble length if preamble_count > 6: preamble_error = 1 preamble_bytes = preamble_bytes + 1 retry = retry + 1 if preamble_bytes == 7: preamble_bytes = 2 elif preamble_count < 2: preamble_error = 1 preamble_bytes = preamble_bytes + 1 retry = retry + 1 if preamble_bytes == 7: preamble_bytes = 2 else: # preamble ok. evaluate message preamble_error = 0 # get remaining message for i in buff: read_buff_array.append(ord(i)) #check crc crc = 0x00 for i in range(14): data = read_buff_array[i] for k in range(8): feedback_bit = (crc ^ data) & 0x80 feedback_bit = ( feedback_bit >> 7) & 0xFF if feedback_bit == 1: crc = (crc << 1) & 0xFF crc = crc ^ 0x31 else: crc = (crc << 1) & 0xFF data = (data << 1) & 0xFF if crc != 0: crc_error = 1 preamble_bytes = preamble_bytes + 1 retry = retry + 1 if preamble_bytes == 7: preamble_bytes = 2 else: crc_error = 0 # no preamble detected else: buff = s.read(14) preamble_error = 1 preamble_bytes = preamble_bytes + 1 retry = retry + 1 if preamble_bytes == 7: preamble_bytes = 2 # get channel byte read_channel = int(read_buff_array[0]) # get specifier byte read_specifier = int(read_buff_array[1]) # get status byte read_status = int(read_buff_array[2]) # get data bytes read_data = 256 * int(read_buff_array[3]) read_data = read_data + int(read_buff_array[4]) # get id bytes read_id = read_buff_array[5] << 8 read_id = (read_id | read_buff_array[6]) << 8 read_id = (read_id | read_buff_array[7]) << 8 read_id = read_id | read_buff_array[8] # evaluate recieved message if read_channel == send_channel: if read_specifier == send_specifier: if read_status == 0 or read_status == 8: if send_specifier == 0: read_data = read_data / 10.0 else: read_data = read_data / 1000.0 erro_while_reading = 0 else: read_data = 0 error_while_reading = 1 else: read_data = 0 error_while_reading = 1 else: read_data = 0 error_while_reading = 1 #prepare status object for publishing # init sensor object status_object = DiagnosticStatus() # init local variable for data key_value = KeyValue() # set values for temperature parameters if send_specifier == 0: if read_data == 85: level = 1 status_object.message = "sensor damaged" elif read_data > 50: level = 2 status_object.message = "temperature critical" elif read_data > 40: level = 1 status_object.message = "temperature high" elif read_data > 10: level = 0 status_object.message = "temperature ok" elif read_data > -1: level = 1 status_object.message = "temperature low" else: level = 2 status_object.message = "temperature critical" # mapping for temperature sensors if read_id == self.head_sensor_param: status_object.name = "Head Temperature" status_object.hardware_id = "hwboard_channel " + str( send_channel) elif read_id == self.eye_sensor_param: status_object.name = "Eye Camera Temperature" status_object.hardware_id = "hwboard_channel = " + str( send_channel) elif read_id == self.torso_module_sensor_param: status_object.name = "Torso Module Temperature" status_object.hardware_id = "hwboard_channel =" + str( send_channel) elif read_id == self.torso_sensor_param: status_object.name = "Torso Temperature" status_object.hardware_id = "hwboard_channel =" + str( send_channel) elif read_id == self.pc_sensor_param: status_object.name = "PC Temperature" status_object.hardware_id = "hwboard_channel =" + str( send_channel) elif read_id == self.engine_sensor_param: status_object.name = "Engine Temperature" status_object.hardware_id = "hwboard_channel = " + str( send_channel) else: level = 1 status_object.message = "cannot map if from yaml file to temperature sensor" # set values for voltage parameters elif send_specifier == 3: if send_channel == 0: if read_data > 58: level = 2 status_object.message = "voltage critical" elif read_data > 56: level = 1 status_object.message = "voltage high" elif read_data > 44: level = 0 status_object.message = "voltage ok" elif read_data > 42: level = 1 status_object.message = "voltage low" else: level = 2 status_object.message = "voltage critical" else: if read_data > 27: level = 2 status_object.message = "voltage critical" elif read_data > 25: level = 1 status_object.message = "voltage_high" elif read_data > 23: level = 0 status_object.message = "voltage ok" elif read_data > 19: level = 1 status_object.message = "voltage low" else: level = 2 status_object.message = "voltage critical" if send_channel == 0: status_object.name = "Akku Voltage" status_object.hardware_id = "hwboard_channel = 0" elif send_channel == 1: status_object.name = "Torso Engine Voltage" status_object.hardware_id = "hwboard_channel = 1" elif send_channel == 2: status_object.name = "Torso Logic Voltage" status_object.hardware_id = "hwboard_channel = 2" elif send_channel == 3: status_object.name = "Tray Logic Voltage" status_object.hardware_id = "hwboard_channel = 3" elif send_channel == 6: status_object.name = "Arm Engine Voltage" status_object.hardware_id = "hwboard_channel = 6" elif send_channel == 7: status_object.name = "Tray Engine Voltage" status_object.hardware_id = "hwboard_channel = 7" # set values for current parameters else: if read_data > 15: level = 2 status_object.message = "current critical" elif read_data > 10: level = 1 status_object.message = "current high" elif read_data < 0: level = 2 status_object.message = "current critical" else: level = 0 status_object.message = "current ok" if send_channel == 1: status_object.name = "Torso Engine Current" status_object.hardware_id = "hwboard_channel = 1" elif send_channel == 2: status_object.name = "Torso Logic Current" status_object.hardware_id = "hwboard_channel = 2" elif send_channel == 3: status_object.name = "Tray Logic Current" status_object.hardware_id = "hwboard_channel = 3" elif send_channel == 6: status_object.name = "Arm Engine Current" status_object.hardware_id = "hwboard_channel = 6" elif send_channel == 7: status_object.name = "Tray Engine Current" status_object.hardware_id = "hwboard_channel = 7" # evaluate error detection if error_while_reading == 1: level = 1 status_object.message = "detected error while receiving answer from hardware" # append status object to publishing message status_object.level = level key_value.value = str(read_data) status_object.values.append(key_value) pub_message.status.append(status_object) # publish message pub.publish(pub_message) rospy.sleep(1.0)
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()
#!/usr/bin/env python import os import rospy from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue if __name__ == '__main__': node_name = os.path.splitext(os.path.basename(__file__))[0] rospy.init_node(node_name) publish_rate = rospy.get_param('~publish_rate', 1.0) frame_id = rospy.get_param('~frame_id', 'base_link') ip_address = rospy.get_param('~ip_address', '192.168.0.12') pub = rospy.Publisher('/ft_sensor/diagnostics', DiagnosticArray, queue_size=3) msg = DiagnosticArray() msg.header.frame_id = frame_id status = DiagnosticStatus() status.level = DiagnosticStatus.OK status.name = 'NetFT RDT Driver' status.message = 'OK' status.hardware_id = 'ATI Gamma' status.values.append(KeyValue('IP Address', ip_address)) msg.status.append(status) rate = rospy.Rate(publish_rate) while not rospy.is_shutdown(): msg.header.stamp = rospy.Time.now() pub.publish(msg) rate.sleep()
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)
proxy = Connection(("ant", 1234)) proxy.login("Master", "robox") data = proxy.inspect(StringArray(["Safety:batteryPower,batteryVoltage"])) diagnosticArray = DiagnosticArray() diagnosticArray.header.stamp = rospy.Time.now() batteryStatus = DiagnosticStatus() batteryStatus.name = "battery" batteryStatus.hardware_id = socket.gethostname() batteryStatus.message = "%.3fV %.1f%%" % (data.Safety.batteryVoltage, data.Safety.batteryPower) if data.Safety.batteryPower < 10: batteryStatus.message += "; Critically low power, recharge NOW!" batteryStatus.level = DiagnosticStatus.ERROR elif data.Safety.batteryPower < 20: batteryStatus.message += "; Low power, recharge soon!" batteryStatus.level = DiagnosticStatus.WARN else: batteryStatus.message += "; Power level good" batteryStatus.level = DiagnosticStatus.OK diagnosticArray.status.append(batteryStatus) publisher.publish(diagnosticArray) batteryVoltagePublisher.publish(data.Safety.batteryVoltage) batteryPercentagePublisher.publish(data.Safety.batteryPower) rate.sleep() print "Closing battery monitor" proxy.close()
def initialize_origin(): rospy.init_node('initialize_origin', anonymous=True) global _origin_pub global _local_xy_frame _origin_pub = rospy.Publisher('/local_xy_origin', GPSFix, latch=True) diagnostic_pub = rospy.Publisher('/diagnostics', DiagnosticArray) local_xy_origin = rospy.get_param('~local_xy_origin', 'auto') _local_xy_frame = rospy.get_param('~local_xy_frame', 'map') if local_xy_origin == "auto": global _sub _sub = rospy.Subscriber("gps", GPSFix, gps_callback) else: parse_origin(local_xy_origin) hw_id = rospy.get_param('~hw_id', 'none') while not rospy.is_shutdown(): if _gps_fix == None: diagnostic = DiagnosticArray() diagnostic.header.stamp = rospy.Time.now() status = DiagnosticStatus() status.name = "LocalXY Origin" status.hardware_id = hw_id status.level = DiagnosticStatus.ERROR status.message = "No Origin" diagnostic.status.append(status) diagnostic_pub.publish(diagnostic) else: _origin_pub.publish( _gps_fix) # Publish this at 1Hz for bag convenience diagnostic = DiagnosticArray() diagnostic.header.stamp = rospy.Time.now() status = DiagnosticStatus() status.name = "LocalXY Origin" status.hardware_id = hw_id if _gps_fix.status.header.frame_id == 'auto': status.level = DiagnosticStatus.OK status.message = "Has Origin (auto)" else: status.level = DiagnosticStatus.WARN status.message = "Origin is static (non-auto)" value0 = KeyValue() value0.key = "Origin" value0.value = _gps_fix.status.header.frame_id status.values.append(value0) value1 = KeyValue() value1.key = "Latitude" value1.value = "%f" % _gps_fix.latitude status.values.append(value1) value2 = KeyValue() value2.key = "Longitude" value2.value = "%f" % _gps_fix.longitude status.values.append(value2) value3 = KeyValue() value3.key = "Altitude" value3.value = "%f" % _gps_fix.altitude status.values.append(value3) diagnostic.status.append(status) diagnostic_pub.publish(diagnostic) rospy.sleep(1.0)
mag_msg.magnetic_field.x = float(st.unpack('h', st.pack('BB', buf[6], buf[7]))[0]) / mag_fact mag_msg.magnetic_field.y = float(st.unpack('h', st.pack('BB', buf[8], buf[9]))[0]) / mag_fact mag_msg.magnetic_field.z = float(st.unpack('h', st.pack('BB', buf[10], buf[11]))[0]) / mag_fact pub_mag.publish(mag_msg) # Publish temperature if len(buf) > 45: temperature_msg.header.stamp = rospy.Time.now() temperature_msg.header.frame_id = frame_id temperature_msg.header.seq = seq temperature_msg.temperature = buf[44] pub_temp.publish(temperature_msg) # Publish diagnostic status if len(buf) > 51: status_msg.level = 0 status_msg.name = "BNO055" status_msg.message = "" calib_stat = KeyValue(key='calib_stat',value=str(buf[45])) selftest_result = KeyValue(key='selftest_result',value=str(buf[46])) intr_stat = KeyValue(key='intr_stat',value=str(buf[47])) sys_clk_stat = KeyValue(key='sys_clk_stat',value=str(buf[48])) sys_stat = KeyValue(key='sys_stat',value=str(buf[49])) sys_err = KeyValue(key='sys_err',value=str(buf[50])) status_msg.values = [calib_stat,selftest_result,intr_stat,sys_clk_stat,sys_stat,sys_err] pub_status.publish(status_msg) seq = seq + 1 rate.sleep()
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 initialize_origin(): global _origin rospy.init_node('initialize_origin', anonymous=True) ros_distro = os.environ.get('ROS_DISTRO') if not ros_distro: rospy.logerror('ROS_DISTRO environment variable was not set.') exit(1) if ros_distro == 'indigo': # ROS Indigo uses the GPSFix message global _origin_pub global _local_xy_frame _origin_pub = rospy.Publisher('/local_xy_origin', GPSFix, latch=True, queue_size=2) diagnostic_pub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=2) local_xy_origin = rospy.get_param('~local_xy_origin', 'auto') _local_xy_frame = rospy.get_param('~local_xy_frame', 'map') _local_xy_frame_identity = rospy.get_param('~local_xy_frame_identity', _local_xy_frame + "__identity") if local_xy_origin == "auto": global _sub _sub = rospy.Subscriber("gps", GPSFix, gps_callback) else: parse_origin(local_xy_origin) if len(_local_xy_frame): tf_broadcaster = tf.TransformBroadcaster() else: tf_broadcaster = None hw_id = rospy.get_param('~hw_id', 'none') while not rospy.is_shutdown(): if tf_broadcaster: # Publish transform involving map (to an anonymous unused # frame) so that TransformManager can support /tf<->/wgs84 # conversions without requiring additional nodes. tf_broadcaster.sendTransform( (0, 0, 0), (0, 0, 0, 1), rospy.Time.now(), _local_xy_frame_identity, _local_xy_frame) if _gps_fix == None: diagnostic = DiagnosticArray() diagnostic.header.stamp = rospy.Time.now() status = DiagnosticStatus() status.name = "LocalXY Origin" status.hardware_id = hw_id status.level = DiagnosticStatus.ERROR status.message = "No Origin" diagnostic.status.append(status) diagnostic_pub.publish(diagnostic) else: _origin_pub.publish(_gps_fix) # Publish this at 1Hz for bag convenience diagnostic = DiagnosticArray() diagnostic.header.stamp = rospy.Time.now() status = DiagnosticStatus() status.name = "LocalXY Origin" status.hardware_id = hw_id if local_xy_origin == 'auto': status.level = DiagnosticStatus.OK status.message = "Has Origin (auto)" else: status.level = DiagnosticStatus.WARN status.message = "Origin is static (non-auto)" value0 = KeyValue() value0.key = "Origin" value0.value = _gps_fix.status.header.frame_id status.values.append(value0) value1 = KeyValue() value1.key = "Latitude" value1.value = "%f" % _gps_fix.latitude status.values.append(value1) value2 = KeyValue() value2.key = "Longitude" value2.value = "%f" % _gps_fix.longitude status.values.append(value2) value3 = KeyValue() value3.key = "Altitude" value3.value = "%f" % _gps_fix.altitude status.values.append(value3) diagnostic.status.append(status) diagnostic_pub.publish(diagnostic) rospy.sleep(1.0) else: # ROS distros later than Indigo use NavSatFix origin_pub = rospy.Publisher('/local_xy_origin', PoseStamped, latch=True, queue_size=2) diagnostic_pub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=2) origin_name = rospy.get_param('~local_xy_origin', 'auto') rospy.loginfo('Local XY origin is "' + origin_name + '"') origin_frame_id = rospy.get_param(rospy.search_param('local_xy_frame'), 'map') origin_frame_identity = rospy.get_param('~local_xy_frame_identity', origin_frame_id + "__identity") rospy.loginfo('Local XY frame ID is "' + origin_frame_id + '"') if len(origin_frame_id): tf_broadcaster = tf.TransformBroadcaster() else: tf_broadcaster = None if origin_name != "auto": origin_list = rospy.get_param('~local_xy_origins', []) _origin = make_origin_from_list(origin_frame_id, origin_name, origin_list) if _origin is not None: origin_pub.publish(_origin) else: origin_name = "auto" if origin_name == "auto": sub = rospy.Subscriber("gps", NavSatFix) sub.impl.add_callback(navsatfix_callback, (origin_frame_id, origin_pub, sub)) rospy.loginfo('Subscribed to NavSat on ' + sub.resolved_name) while not rospy.is_shutdown(): if tf_broadcaster: # Publish transform involving map (to an anonymous unused # frame) so that TransformManager can support /tf<->/wgs84 # conversions without requiring additional nodes. tf_broadcaster.sendTransform( (0, 0, 0), (0, 0, 0, 1), rospy.Time.now(), origin_frame_identity, origin_frame_id) diagnostic_pub.publish(make_diagnostic(_origin, origin_name != "auto")) rospy.sleep(1.0)
def ntp_monitor(ntp_hostname, offset=500, self_offset=500, diag_hostname=None, error_offset=5000000, ignore_self=False): pub = rospy.Publisher("/diagnostics", DiagnosticArray, queue_size=10) rospy.init_node(NAME, anonymous=True) hostname = socket.gethostname() if diag_hostname is None: diag_hostname = hostname ntp_checks = [] stat = DiagnosticStatus() stat.level = 0 stat.name = "NTP offset from " + diag_hostname + " to " + ntp_hostname stat.message = "OK" stat.hardware_id = hostname stat.values = [] ntp_checks.append((stat, ntp_hostname, offset)) if not ignore_self: 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 = [] ntp_checks.append((self_stat, hostname, self_offset)) while not rospy.is_shutdown(): msg = DiagnosticArray() for st, host, off in ntp_checks: 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.status.append(st) msg.header.stamp = rospy.get_rostime() pub.publish(msg) time.sleep(1)
def publish_diagnostics(self, rt_HZ_store = []): # set desired rates if self.hzerror: if isinstance(self.hzerror, float) or isinstance(self.hzerror, int): min_rate = self.hz - self.hzerror max_rate = self.hz + self.hzerror else: rospy.logerr("hzerror not float or int") sys.exit(1) else: min_rate = None max_rate = None # create diagnostic message array = DiagnosticArray() array.header.stamp = rospy.Time.now() hz_status = DiagnosticStatus() hz_status.name = self.diagnostics_name hz_status.values.append(KeyValue("topics", str(self.topics))) hz_status.values.append(KeyValue("desired_rate", str(self.hz))) hz_status.values.append(KeyValue("min_rate", str(min_rate))) hz_status.values.append(KeyValue("max_rate", str(max_rate))) hz_status.values.append(KeyValue("window_size", str(self.window_size))) publishing_rate_error = False rates = [] consolidated_error_messages = {} ## store and display consolidated erros messages for all the topics consolidated_error_messages.setdefault("never received message for topics", []) consolidated_error_messages.setdefault("no messages anymore for topics", []) consolidated_error_messages.setdefault("publishing rate is too low for topics", []) consolidated_error_messages.setdefault("publishing rate is too high for topics", []) if len(rt_HZ_store) != len(self.topics): hz_status.level = DiagnosticStatus.WARN hz_status.message = "could not determine type for topics %s. Probably never published on that topics."%self.missing_topics array.status.append(hz_status) self.pub_diagnostics.publish(array) return # calculate actual rates for rt, topic in zip(rt_HZ_store, self.topics): if not rt or not rt.times: hz_status.level = DiagnosticStatus.ERROR publishing_rate_error = True rates.append(0.0) consolidated_error_messages["never received message for topics"].append(topic) elif rt.msg_tn == rt.last_printed_tn: hz_status.level = DiagnosticStatus.ERROR publishing_rate_error = True rates.append(0.0) consolidated_error_messages["no messages anymore for topics"].append(topic) else: with rt.lock: # calculation taken from /opt/ros/indigo/lib/python2.7/dist-packages/rostopic/__init__.py n = len(rt.times) mean = sum(rt.times) / n rate = 1./mean if mean > 0. else 0 rt.last_printed_tn = rt.msg_tn rates.append(round(rate, 2)) if min_rate and rate < min_rate: hz_status.level = DiagnosticStatus.WARN publishing_rate_error = True consolidated_error_messages["publishing rate is too low for topics"].append(topic) elif max_rate and rate > max_rate: hz_status.level = DiagnosticStatus.WARN publishing_rate_error = True consolidated_error_messages["publishing rate is too high for topics"].append(topic) else: if not publishing_rate_error: hz_status.level = DiagnosticStatus.OK hz_status.message = 'all publishing rates are ok' if publishing_rate_error: message = "" key = "never received message for topics" if len(consolidated_error_messages[key]) > 0: message += key + " " + str(consolidated_error_messages[key]) + ", " key = "no messages anymore for topics" if len(consolidated_error_messages[key]) > 0: message += key + " " + str(consolidated_error_messages[key]) + ", " key = "publishing rate is too low for topics" if len(consolidated_error_messages[key]) > 0: message += key + " " + str(consolidated_error_messages[key]) + ", " key = "publishing rate is too high for topics" if len(consolidated_error_messages[key]) > 0: message += key + " " + str(consolidated_error_messages[key]) hz_status.message = message hz_status.values.append(KeyValue("rates", str(rates))) array.status.append(hz_status) self.pub_diagnostics.publish(array)
if previousTimestamp is not None: interval = (receivedTimestamp - previousTimestamp).to_sec() averageInterval += interval maxInterval = max(interval, maxInterval) previousTimestamp = receivedTimestamp # Prepare diagnostics message diagnosticArray = DiagnosticArray() diagnosticArray.header.stamp = rospy.Time.now() topicStatus = DiagnosticStatus() topicStatus.name = "Topic status: " + topic topicStatus.hardware_id = hostname topicStatus.level = DiagnosticStatus.WARN if lastMessageAt == rospy.Time( 0 ) else DiagnosticStatus.ERROR if hasTimeout else DiagnosticStatus.OK averageRate = 0.0 if len(timestampBuffer) > 1: # Finish averaging averageDelay /= float(len(timestampBuffer)) averageInterval /= float(len(timestampBuffer) - 1) averageRate = 1.0 / max(averageInterval, 1.0 / 10000) topicStatus.values.append( KeyValue('Last received at', str(lastMessageAt.to_sec()))) topicStatus.values.append( KeyValue('Observation window size:', "%d messages" % len(timestampBuffer))) topicStatus.values.append(
imuMsg.orientation.y = q[1] imuMsg.orientation.z = q[2] imuMsg.orientation.w = q[3] imuMsg.header.stamp = rospy.Time.now() imuMsg.header.frame_id = 'base_imu_link' imuMsg.header.seq = seq seq = seq + 1 pub.publish(imuMsg) if (diag_pub_time < rospy.get_time()): diag_pub_time += 1 diag_arr = DiagnosticArray() diag_arr.header.stamp = rospy.get_rostime() diag_arr.header.frame_id = '1' diag_msg = DiagnosticStatus() diag_msg.name = 'Razor_Imu' diag_msg.level = DiagnosticStatus.OK diag_msg.message = 'Received AHRS measurement' diag_msg.values.append( KeyValue('roll (deg)', str(roll * (180.0 / math.pi)))) diag_msg.values.append( KeyValue('pitch (deg)', str(pitch * (180.0 / math.pi)))) diag_msg.values.append( KeyValue('yaw (deg)', str(yaw * (180.0 / math.pi)))) diag_msg.values.append(KeyValue('sequence number', str(seq))) diag_arr.status.append(diag_msg) diag_pub.publish(diag_arr) ser.close #f.close
maxDelay = max(delay, maxDelay) #if previousTimestamp is not None: # interval = (receivedTimestamp - previousTimestamp).to_sec() # averageInterval += interval # maxInterval = max(interval, maxInterval) previousTimestamp = receivedTimestamp # Prepare diagnostics message diagnosticArray = DiagnosticArray() diagnosticArray.header.stamp = rospy.Time.now() tfStatus = DiagnosticStatus() tfStatus.name = "TF link status: %s --> %s" % (source_frame, target_frame) tfStatus.hardware_id = hostname tfStatus.level = DiagnosticStatus.WARN if lastTransformAt == rospy.Time(0) else DiagnosticStatus.ERROR if hasTimeout else DiagnosticStatus.OK averageRate = 0.0 if len(timestampBuffer) > 1: # Finish averaging averageDelay /= float(len(timestampBuffer)) # averageInterval /= float(len(timestampBuffer) - 1) # averageRate = 1.0 / averageInterval tfStatus.values.append(KeyValue('Last received at', str(lastTransformAt.to_sec()))) tfStatus.values.append(KeyValue('Observation window size:', "%d messages" % len(timestampBuffer))) #tfStatus.values.append(KeyValue('Average interval between messages', "%.3f sec" % averageInterval)) tfStatus.values.append(KeyValue('Maximum delay wrt. current ROS time', "%.3f sec" % maxDelay)) tfStatus.values.append(KeyValue('Average delay wrt. current ROS time', "%.3f sec" % averageDelay)) if lastTransformAt == rospy.Time(0):
def publish_diagnostics(self, event): # set desired rates if isinstance(self.hzerror, float) or isinstance(self.hzerror, int): if self.hzerror < 0: rospy.logerr("hzerror cannot be negative") sys.exit(1) min_rate = self.hz - self.hzerror max_rate = self.hz + self.hzerror if min_rate < 0 or max_rate < 0: rospy.logerr("min_rate/max_rate cannot be negative") sys.exit(1) min_duration = 1 / min_rate if min_rate > 0 else float('inf') max_duration = 1 / max_rate if max_rate > 0 else float('inf') else: rospy.logerr("hzerror not float or int") sys.exit(1) # create diagnostic message array = DiagnosticArray() array.header.stamp = rospy.Time.now() hz_status = DiagnosticStatus() hz_status.name = self.diagnostics_name hz_status.level = DiagnosticStatus.OK hz_status.message = 'all publishing rates are ok' hz_status.values.append(KeyValue("topics", str(self.topics))) hz_status.values.append(KeyValue("desired_rate", str(self.hz))) hz_status.values.append(KeyValue("min_rate", str(min_rate))) hz_status.values.append(KeyValue("max_rate", str(max_rate))) hz_status.values.append(KeyValue("window_size", str(self.window_size))) rates = [] consolidated_error_messages = { } ## store and display consolidated erros messages for all the topics consolidated_error_messages.setdefault( "never received message for topics", []) consolidated_error_messages.setdefault( "no messages anymore for topics", []) consolidated_error_messages.setdefault( "publishing rate is too low for topics", []) consolidated_error_messages.setdefault( "publishing rate is too high for topics", []) if not all(k in self.rt_HZ_store for k in self.topics): hz_status.level = DiagnosticStatus.WARN hz_status.message = "could not determine type for topics {}. Probably never published on that topics.".format( self.missing_topics) array.status.append(hz_status) self.pub_diagnostics.publish(array) return # calculate actual rates for topic, rt in self.rt_HZ_store.items(): #print("rt.times {}".format(rt.times)) #print("rt.msg_tn {}".format(rt.msg_tn)) #print("rt.last_printed_tn {}".format(rt.last_printed_tn)) #print("rt.delta: {}".format(rt.msg_tn - rt.last_printed_tn)) #print("event.current_real: {}".format(event.current_real.to_sec())) #print("event.last_real: {}".format(event.last_real.to_sec())) #print("event.delta: {}".format((event.current_real - event.last_real).to_sec())) if not rt or not rt.times: hz_status.level = DiagnosticStatus.ERROR rates.append(0.0) consolidated_error_messages[ "never received message for topics"].append(topic) elif rt.msg_tn == rt.last_printed_tn \ and not (event.current_real.to_sec() < rt.msg_tn + min_duration): # condition to prevent WARN for topics with hz<diagnostics_rate, allow 1/min_rte to pass before reporting error hz_status.level = DiagnosticStatus.ERROR rates.append(0.0) consolidated_error_messages[ "no messages anymore for topics"].append(topic) else: with rt.lock: # calculation taken from /opt/ros/indigo/lib/python2.7/dist-packages/rostopic/__init__.py n = len(rt.times) mean = sum(rt.times) / n rate = 1. / mean if mean > 0. else 0 rt.last_printed_tn = rt.msg_tn rates.append(round(rate, 2)) if min_rate and rate < min_rate: hz_status.level = DiagnosticStatus.WARN consolidated_error_messages[ "publishing rate is too low for topics"].append(topic) if max_rate and rate > max_rate: hz_status.level = DiagnosticStatus.WARN consolidated_error_messages[ "publishing rate is too high for topics"].append(topic) if hz_status.level != DiagnosticStatus.OK: message = "" key = "never received message for topics" if len(consolidated_error_messages[key]) > 0: message += key + " " + str( consolidated_error_messages[key]) + ", " key = "no messages anymore for topics" if len(consolidated_error_messages[key]) > 0: message += key + " " + str( consolidated_error_messages[key]) + ", " key = "publishing rate is too low for topics" if len(consolidated_error_messages[key]) > 0: message += key + " " + str( consolidated_error_messages[key]) + ", " key = "publishing rate is too high for topics" if len(consolidated_error_messages[key]) > 0: message += key + " " + str(consolidated_error_messages[key]) hz_status.message = message hz_status.values.append(KeyValue("rates", str(rates))) array.status.append(hz_status) self.pub_diagnostics.publish(array)
except ValueError: rospy.loginfo("Waiting for laptop battery state to become available...") nextQueryAt = time.time() + 15.0 continue diagnosticArray = DiagnosticArray() diagnosticArray.header.stamp = rospy.Time.now() batteryStatus = DiagnosticStatus() batteryStatus.name = hostname + " laptop battery power" batteryStatus.hardware_id = hostname batteryStatus.message="%.1f%% (%.3fV)" % (batteryPercentage, batteryVoltage) if batteryPercentage < 20: batteryStatus.message += "; Critically low power, recharge NOW!" batteryStatus.level = DiagnosticStatus.ERROR elif batteryPercentage < 40: batteryStatus.message += "; Low power, recharge soon!" batteryStatus.level = DiagnosticStatus.WARN else: batteryStatus.message += "; Power level good" batteryStatus.level = DiagnosticStatus.OK diagnosticArray.status.append(batteryStatus) laptopPluggedStatus = DiagnosticStatus() laptopPluggedStatus.name = hostname + " power supply" laptopPluggedStatus.hardware_id = hostname laptopPluggedStatus.message = "Plugged in" if laptopPluggedIn else "Disconnected!" laptopPluggedStatus.level = DiagnosticStatus.OK if laptopPluggedIn else DiagnosticStatus.WARN
def ntp_monitor(offset=500, self_offset=500, diag_hostname = None, error_offset = 5000000): pub = rospy.Publisher("/diagnostics", DiagnosticArray, queue_size = 100) rospy.init_node(NAME, anonymous=True) 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 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)]: try: p = Popen(["ntpdate", "-q", host], stdout=PIPE, stdin=PIPE, stderr=PIPE, universal_newlines=True) res = p.wait() (o,e) = p.communicate() except OSError as e: (errno, msg) = e.args 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] pub.publish(msg) time.sleep(1)
def run(self): """This function recieves data from the socket and published a IMU message to ROS""" while not rospy.is_shutdown(): line = self.recv_all() if line.startswith("#YPRAG="): line = line.replace("#YPRAG=", "") # Delete "#YPRAG=" else: continue #f.write(line) # Write to the output log file words = string.split(line, ",") # Fields split #Try to convert words from string to float, if a value doesn't work, continue to the next loop try: words = [float(word) for word in words] except ValueError: print "Failed to parse data into 9 values.. No big concern" continue if len(words) == 9: #in AHRS firmware z axis points down, in ROS z axis points up (see REP 103) yaw_deg = -words[0] yaw_deg = yaw_deg + imu_yaw_calibration if yaw_deg > 180.0: yaw_deg = yaw_deg - 360.0 if yaw_deg < -180.0: yaw_deg = yaw_deg + 360.0 self.yaw = yaw_deg * degrees2rad #in AHRS firmware y axis points right, in ROS y axis points left (see REP 103) self.pitch = -words[1] * degrees2rad self.roll = words[2] * degrees2rad # AHRS firmware accelerations are negated # This means y and z are correct for ROS, but x needs reversing self.imuMsg.linear_acceleration.x = -words[ 3] * self.accel_factor self.imuMsg.linear_acceleration.y = words[4] * self.accel_factor self.imuMsg.linear_acceleration.z = words[5] * self.accel_factor self.imuMsg.angular_velocity.x = words[6] #in AHRS firmware y axis points right, in ROS y axis points left (see REP 103) self.imuMsg.angular_velocity.y = -words[7] #in AHRS firmware z axis points down, in ROS z axis points up (see REP 103) self.imuMsg.angular_velocity.z = -words[8] q = quaternion_from_euler(self.roll, self.pitch, self.yaw) self.imuMsg.orientation.x = q[0] self.imuMsg.orientation.y = q[1] self.imuMsg.orientation.z = q[2] self.imuMsg.orientation.w = q[3] self.imuMsg.header.stamp = rospy.Time.now() self.imuMsg.header.frame_id = 'base_imu_link' self.imuMsg.header.seq = self.seq self.seq = self.seq + 1 #Publish message self.pub.publish(self.imuMsg) else: print "Recieved less than 9 values from socket, not publishing" if (self.diag_pub_time < rospy.get_time()): self.diag_pub_time += 1 diag_arr = DiagnosticArray() diag_arr.header.stamp = rospy.get_rostime() diag_arr.header.frame_id = '1' diag_msg = DiagnosticStatus() diag_msg.name = 'Razor_Imu' diag_msg.level = DiagnosticStatus.OK diag_msg.message = 'Received AHRS measurement' diag_msg.values.append( KeyValue('roll (deg)', str(self.roll * (180.0 / math.pi)))) diag_msg.values.append( KeyValue('pitch (deg)', str(self.pitch * (180.0 / math.pi)))) diag_msg.values.append( KeyValue('yaw (deg)', str(self.yaw * (180.0 / math.pi)))) diag_msg.values.append( KeyValue('sequence number', str(self.seq))) diag_arr.status.append(diag_msg) self.diag_pub.publish(diag_arr)
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)
NAME = 'ntp_monitor' def ntp_monitor(offset=500, self_offset=500, diag_hostname = None as error_offset = 5000000): pub = rospy.Publisher("/diagnostics", DiagnosticArray, queue_size = 100) rospy.init_node(NAME, anonymous=True) 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 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)]: try:
def bno08x_node(): # Initialize ROS node raw_pub = rospy.Publisher('bno08x/raw', Imu, queue_size=10) mag_pub = rospy.Publisher('bno08x/mag', MagneticField, queue_size=10) status_pub = rospy.Publisher('bno08x/status', DiagnosticStatus, queue_size=10) rospy.init_node('bno08x') rate = rospy.Rate(100) # frequency in Hz rospy.loginfo(rospy.get_caller_id() + " bno08x node launched.") i2c = busio.I2C(board.SCL, board.SDA, frequency=800000) bno = BNO08X_I2C(i2c, address=0x4b) # BNO080 (0x4b) BNO085 (0x4a) bno.enable_feature(BNO_REPORT_ACCELEROMETER) bno.enable_feature(BNO_REPORT_GYROSCOPE) bno.enable_feature(BNO_REPORT_MAGNETOMETER) bno.enable_feature(BNO_REPORT_ROTATION_VECTOR) time.sleep(0.5) # ensure IMU is initialized while True: raw_msg = Imu() raw_msg.header.stamp = rospy.Time.now() accel_x, accel_y, accel_z = bno.acceleration raw_msg.linear_acceleration.x = accel_x raw_msg.linear_acceleration.y = accel_y raw_msg.linear_acceleration.z = accel_z gyro_x, gyro_y, gyro_z = bno.gyro raw_msg.angular_velocity.x = gyro_x raw_msg.angular_velocity.y = gyro_y raw_msg.angular_velocity.z = gyro_z quat_i, quat_j, quat_k, quat_real = bno.quaternion raw_msg.orientation.w = quat_i raw_msg.orientation.x = quat_j raw_msg.orientation.y = quat_k raw_msg.orientation.z = quat_real raw_msg.orientation_covariance[0] = -1 raw_msg.linear_acceleration_covariance[0] = -1 raw_msg.angular_velocity_covariance[0] = -1 raw_pub.publish(raw_msg) mag_msg = MagneticField() mag_x, mag_y, mag_z = bno.magnetic mag_msg.header.stamp = rospy.Time.now() mag_msg.magnetic_field.x = mag_x mag_msg.magnetic_field.y = mag_y mag_msg.magnetic_field.z = mag_z mag_msg.magnetic_field_covariance[0] = -1 mag_pub.publish(mag_msg) status_msg = DiagnosticStatus() status_msg.level = 0 status_msg.name = "bno08x IMU" status_msg.message = "" status_pub.publish(status_msg) rate.sleep() rospy.loginfo(rospy.get_caller_id() + " bno08x node finished")
def _update_diagnostics_state(self): md5_warnings = {} ttype_warnings = {} for mname, mth in self.masters.items(): warnings = mth.get_md5warnigs() if warnings: md5_warnings[mname] = warnings twarnings = mth.get_topic_type_warnings() if twarnings: ttype_warnings[mname] = twarnings level = 0 if md5_warnings or ttype_warnings: level = 1 if self._current_diagnistic_level != level: da = DiagnosticArray() if md5_warnings or ttype_warnings: # add warnings for all hosts with topic types with different md5sum for mname, warnings in md5_warnings.items(): diag_state = DiagnosticStatus() diag_state.level = level diag_state.name = rospy.get_name() diag_state.message = 'Wrong topic md5sum @ %s and %s' % ( mname, self._localname) diag_state.hardware_id = self.hostname for (topicname, _node, _nodeuri), tmtype in warnings.items(): if isinstance(tmtype, tuple): (md5sum, ttype) = tmtype if md5sum is not None: key = KeyValue() key.key = topicname key.value = str(ttype) diag_state.values.append(key) da.status.append(diag_state) # add warnings if a topic with different type is synchrinozied to local host for mname, warnings in ttype_warnings.items(): diag_state = DiagnosticStatus() diag_state.level = level diag_state.name = rospy.get_name() diag_state.message = 'Wrong topics type @ %s and %s' % ( mname, self._localname) diag_state.hardware_id = self.hostname for (topicname, _node, _nodeuri), tmtype in warnings.items(): ttype = tmtype if isinstance(tmtype, tuple): (md5sum, ttype) = tmtype key = KeyValue() key.key = topicname key.value = str(ttype) diag_state.values.append(key) da.status.append(diag_state) else: # clear all warnings diag_state = DiagnosticStatus() diag_state.level = 0 diag_state.name = rospy.get_name() diag_state.message = "" diag_state.hardware_id = self.hostname da.status.append(diag_state) da.header.stamp = rospy.Time.now() self.pub_diag.publish(da) self._current_diagnistic_level = level