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 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 _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 fill_diags(name, summary, hid, diags): ds = DiagnosticStatus() ds.values = [KeyValue(k, str(v)) for (k, v) in diags] ds.hardware_id = hid ds.name = rospy.get_caller_id().lstrip('/') + ": " + name ds.message = summary return ds
def 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_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 _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 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 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 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 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 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 update(self): msg = DiagnosticArray() msg.header.stamp = rospy.Time.now() msg.status = [] it = 0 while it < 20: status = DiagnosticStatus() status.level = random.randint(0,2) status.name = "Test %i"%it status.hardware_id = "Dummy Diagnostics" if status.level == 0: message = "OK" elif status.level == 1: message = "WARN" elif status.level == 2: message = "ERROR" status.message = message status.values = [] ii = 0 while ii < 20: status.values.append(KeyValue("Key %i"%ii,str(random.randint(0,50)))) ii += 1 it += 1 msg.status.append(status) self.publisher.publish(msg) msg = DiagnosticArray() msg.header.stamp = rospy.Time.now() msg.status = [] status = DiagnosticStatus() status.level = status.WARN status.name = "Test Warn" status.hardware_id = "Dummy Diagnostics" status.message = "Warning - This is a test" status.values = [] msg.status.append(status) self.publisher.publish(msg)
def cb_diagnostics_update(self): """ Callback periodically called to update the diagnostics status of the tabpage handler. @return: A status message for the tabpage handler @rtype: C{diagnostic_msgs/DiagnosticStatus} """ tabpage_status = DiagnosticStatus() tabpage_status.level = tabpage_status.OK tabpage_status.name = " ".join([self.handler_name, "Handler"]) tabpage_status.hardware_id = self.parent.ros_name tabpage_status.message = "OK" tabpage_status.values = [] return tabpage_status
def 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 _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' 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 gpu_status_to_diag(gpu_stat): stat = DiagnosticStatus() stat.name = 'GPU Status' stat.message = 'OK' stat.level = DiagnosticStatus.OK stat.hardware_id = gpu_stat.pci_device_id 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)) # Check for valid data if not gpu_stat.product_name or not gpu_stat.pci_device_id: stat.level = DiagnosticStatus.ERROR stat.message = 'No Device Data' return stat # Check load if gpu_stat.gpu_usage > 98: stat.level = max(stat.level, DiagnosticStatus.WARN) stat.message = 'High Load' # Check thresholds if gpu_stat.temperature > 90: stat.level = max(stat.level, DiagnosticStatus.WARN) stat.message = 'High Temperature' if gpu_stat.temperature > 95: stat.level = max(stat.level, DiagnosticStatus.ERROR) stat.message = 'Temperature Alarm' # Check fan if gpu_stat.fan_speed == 0: stat.level = max(stat.level, DiagnosticStatus.ERROR) stat.message = 'No Fan Speed' return stat
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 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 create_nuc_status(self): diag_status = DiagnosticStatus() diag_status.name = "BHG: NUC Status" diag_status.message = "BHG: Normal" diag_status.hardware_id = socket.gethostname() temperature_case_kv = KeyValue() temperature_case_kv.key = 'temperature_case' temperature_case_kv.value = str(self.temperature_case) diag_status.values.append(temperature_case_kv) disk_usage_kv = KeyValue() self.get_disk_usage() disk_usage_kv.key = 'disk_usage' disk_usage_kv.value = str(self.disk_usage) diag_status.values.append(disk_usage_kv) is_recording_kv = KeyValue() is_recording_kv.key = 'is_recording' is_recording_kv.value = str(self.is_recording) diag_status.values.append(is_recording_kv) return diag_status
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)" elif self.origin_source == "custom": status.level = DiagnosticStatus.OK status.message = "Has Origin (Custom Topic)" 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 _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' rv.values.append(KeyValue('Voltage (V)', str(laptop_msg.voltage))) rv.values.append(KeyValue('Current (A)', str(laptop_msg.current))) 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('Percentage (%)', str(laptop_msg.percentage))) rv.values.append( KeyValue('Charging state', val_to_state[laptop_msg.power_supply_status])) return rv
def __init__(self, argv=sys.argv): rospy.init_node("ntp_monitor") self.parse_args(argv) stat = DiagnosticStatus() stat.level = DiagnosticStatus.WARN stat.name = '%s NTP Offset' % self.diag_hostname stat.message = 'No Data' stat.hardware_id = self.diag_hostname stat.values = [] self.msg = DiagnosticArray() self.msg.header.stamp = rospy.get_rostime() self.msg.status = [stat] self.update_diagnostics() self.diag_pub = rospy.Publisher("/diagnostics", DiagnosticArray, queue_size=1) self.diag_timer = rospy.Timer(rospy.Duration(1.0), self.publish_diagnostics) self.monitor_timer = rospy.Timer(rospy.Duration(60.0), self.update_diagnostics)
def create_fcu_status(self): diag_status = DiagnosticStatus() diag_status.name = "BHG: FCU Status" diag_status.message = "BHG: Normal" diag_status.hardware_id = "Pixhawk" gps_fix_type_kv = KeyValue() gps_fix_type_kv.key = 'gps_fix_type' gps_fix_type_kv.value = self.fixtype_convert(self.gps_fix_type) diag_status.values.append(gps_fix_type_kv) gps_num_sv_kv = KeyValue() gps_num_sv_kv.key = 'gps_num_sv' gps_num_sv_kv.value = str(self.gps_num_sv) diag_status.values.append(gps_num_sv_kv) fcu_mode_kv = KeyValue() fcu_mode_kv.key = 'fcu_mode' fcu_mode_kv.value = str(self.fcu_mode) diag_status.values.append(fcu_mode_kv) fcu_voltage_kv = KeyValue() fcu_voltage_kv.key = 'fcu_voltage' fcu_voltage_kv.value = str(self.fcu_voltage) diag_status.values.append(fcu_voltage_kv) fcu_amps_kv = KeyValue() fcu_amps_kv.key = 'fcu_amps' fcu_amps_kv.value = str(self.fcu_amps) diag_status.values.append(fcu_amps_kv) fcu_cpu_load_kv = KeyValue() fcu_cpu_load_kv.key = 'fcu_cpu_load' fcu_cpu_load_kv.value = str(self.fcu_cpu_load) diag_status.values.append(fcu_cpu_load_kv) return diag_status
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 publish_diagnostics_info(self): """ Publishes at a rate of 1Hz ( because thats the rate real kobuki does it ) For this first version we only publish battery status. :return: """ battery_charge = self.get_simulated_battery_status() msg = DiagnosticArray() info_msg = DiagnosticStatus() header = Header() header.frame_id = "" header.stamp = rospy.Time.now() msg.header = header msg.status.append(info_msg) values = [] percent_value = KeyValue() percent_value.key = "Percent" percent_value.value = str(battery_charge) # TODO: Add all the other values voltage_value = KeyValue() voltage_value.key = "Voltage (V)" charge_value = KeyValue() charge_value.key = "Charge (Ah)" capacity_value = KeyValue() capacity_value.key = "Capacity (Ah)" source_value = KeyValue() source_value.key = "Source" charging_state_value = KeyValue() charging_state_value.key = "Charging State" current_value = KeyValue() current_value.key = "Current (A)" if battery_charge <= 10.0: level = DiagnosticStatus.ERROR message = "Empty Battery" elif 10.0 < battery_charge <= 20.0: level = DiagnosticStatus.WARN message = "Warning Low Battery" elif battery_charge >= 100.0: level = DiagnosticStatus.OK message = "Maximum" else: level = DiagnosticStatus.OK message = "NormalLevel" info_msg.name = "mobile_base_nodelet_manager: Battery" info_msg.hardware_id = "Kobuki" info_msg.message = message info_msg.level = level values.append(percent_value) values.append(voltage_value) values.append(charge_value) values.append(capacity_value) values.append(source_value) values.append(charging_state_value) values.append(current_value) info_msg.values = values msg.status.append(info_msg) self._pub.publish(msg)
def diagnostics(self): ds = DiagnosticStatus() ds.name = rospy.get_caller_id().lstrip('/') + ": " + "Network Watchdog" ds.hardware_id = "none" with self.mutex: now = rospy.get_time() time_to_timeout = self.timeout - (now - self.interruption_time) elapsed_orig = now - self.interruption_time_orig # Parameters ds.values.append(KeyValue("Timeout (s)", "%.1f" % self.timeout)) ds.values.append( KeyValue("Latency threshold (ms)", str(int(self.max_latency * 1000)))) ds.values.append( KeyValue("Loss threshold (%)", "%.1f" % (self.max_loss * 100))) ds.values.append(KeyValue("Timeout command", str(self.action_name))) # Network outage stats ds.values.append( KeyValue("Network is down", str(self.outage_in_progress))) ds.values.append(KeyValue("Outage count", str(self.outage_count))) if self.outage_in_progress: ds.values.append( KeyValue("Time since outage start (s)", "%.1f" % elapsed_orig)) if self.outage_length: ds.values.append( KeyValue("Latest outage length", "%.1f" % self.outage_length)) ds.values.append( KeyValue("Longest outage length (s)", "%.1f" % self.longest_outage)) # Command stats ds.values.append( KeyValue("Timeout command in progress", str(self.action_running))) ds.values.append(KeyValue("Trigger count", str(self.trigger_count))) if self.outage_in_progress: if not self.action_running: ds.values.append( KeyValue("Time to timeout (s)", "%.1f" % (time_to_timeout))) if self.action_running: ds.values.append( KeyValue("Time since trigger", str(now - self.latest_trigger_time))) if self.longest_action_time: ds.values.append( KeyValue("Longest timeout command run time", "%.1f" % self.longest_action_duration)) # Stats ds.values.append( KeyValue("Node start time", time.ctime(self.start_time))) if self.interruption_time_orig != inf: ds.values.append( KeyValue("Latest outage start time", time.ctime(self.interruption_time_orig))) if self.longest_outage_time: ds.values.append( KeyValue("Longest outage start time", time.ctime(self.longest_outage_time))) if self.latest_trigger_time: ds.values.append( KeyValue("Latest trigger time", time.ctime(self.latest_trigger_time))) if self.longest_action_time: ds.values.append( KeyValue("Time of longest timeout command", time.ctime(self.longest_action_time))) # Summary if self.interruption_time == inf: ds.message = "Network is up" ds.level = DiagnosticStatus.OK if self.action_running: ds.message += "; Timeout command in progress (%.0f s)" % ( now - self.latest_trigger_time) else: if self.action_running: ds.message = "Timeout command in progress (%.0f s; %.0f s)" % ( elapsed_orig, now - self.latest_trigger_time) ds.level = DiagnosticStatus.ERROR else: ds.message = "Network is down (%.0f s; %.0f s)" % ( elapsed_orig, time_to_timeout) ds.level = DiagnosticStatus.WARN da = DiagnosticArray() da.header.stamp = rospy.get_rostime() da.status.append(ds) self.diag_pub.publish(da)
def main(): rclpy.init(args=None) node = Node('system_monitor') pub = node.create_publisher(WorkloadMsg, 'system_workload', 1) diagnostic_pub = node.create_publisher(DiagnosticArray, 'diagnostics', 1) hostname = socket.gethostname() diag_array = DiagnosticArray() diag_cpu = DiagnosticStatus() # start all names with "SYSTEM" for diagnostic analysesr diag_cpu.name = "SYSTEMCPU" diag_cpu.hardware_id = "CPU" diag_mem = DiagnosticStatus() diag_mem.name = "SYSTEMMemory" diag_cpu.hardware_id = "Memory" node.declare_parameter('update_frequency', 10.0) node.declare_parameter('do_memory', True) node.declare_parameter('do_cpu', True) node.declare_parameter('cpu_load_percentage', 80.0) node.declare_parameter('memoroy_load_percentage', 80.0) node.declare_parameter('network_rate_received_errors', 10.0) node.declare_parameter('network_rate_send_errors', 10.0) rate = node.get_parameter('update_frequency').get_parameter_value().double_value do_memory = node.get_parameter('do_memory').get_parameter_value().bool_value do_cpu = node.get_parameter('do_cpu').get_parameter_value().bool_value cpu_load_percentage = node.get_parameter('cpu_load_percentage').get_parameter_value().double_value memoroy_load_percentage = node.get_parameter('memoroy_load_percentage').get_parameter_value().double_value network_rate_received_errors = node.get_parameter( 'network_rate_received_errors').get_parameter_value().double_value network_rate_send_errors = node.get_parameter('network_rate_send_errors').get_parameter_value().double_value while rclpy.ok(): last_send_time = time.time() running_processes, cpu_usages, overall_usage_percentage = cpus.collect_all() if do_cpu else ( -1, [], 0) memory_available, memory_used, memory_total = memory.collect_all() if do_memory else (-1, -1, -1) interfaces = network_interfaces.collect_all(node.get_clock()) msg = WorkloadMsg( hostname=hostname, cpus=cpu_usages, running_processes=running_processes, cpu_usage_overall=overall_usage_percentage, memory_available=memory_available, memory_used=memory_used, memory_total=memory_total, filesystems=[], network_interfaces=interfaces ) pub.publish(msg) diag_array.status = [] # publish diagnostic message to show this in the rqt diagnostic viewer diag_cpu.message = str(overall_usage_percentage) + "%" if overall_usage_percentage >= cpu_load_percentage: diag_cpu.level = DiagnosticStatus.WARN else: diag_cpu.level = DiagnosticStatus.OK diag_array.status.append(diag_cpu) memory_usage = round((memory_used / memory_total) * 100, 2) diag_mem.message = str(memory_usage) + "%" if memory_usage >= memoroy_load_percentage: diag_mem.level = DiagnosticStatus.WARN else: diag_mem.level = DiagnosticStatus.OK diag_array.status.append(diag_mem) for interface in interfaces: diag_net = DiagnosticStatus() diag_net.name = "SYSTEM" + interface.name diag_net.hardware_id = interface.name if interface.rate_received_packets_errors >= network_rate_received_errors \ or interface.rate_sent_packets_errors >= network_rate_send_errors: diag_net.level = DiagnosticStatus.WARN else: diag_net.level = DiagnosticStatus.OK diag_array.status.append(diag_net) diag_array.header.stamp = node.get_clock().now().to_msg() diagnostic_pub.publish(diag_array) # sleep to have correct rate. we dont use ROS time since we are interested in system things dt = time.time() - last_send_time time.sleep(max(0, (1 / rate) - dt))
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)
batteryVoltagePublisher = rospy.Publisher("/spencer/sensors/battery/voltage", Float32, latch=True, queue_size=1) batteryPercentagePublisher = rospy.Publisher("/spencer/sensors/battery/percentage", Float32, latch=True, queue_size=1) rospy.loginfo("Starting robot battery monitor...") while not rospy.is_shutdown(): 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)
def ntp_monitor(namespace, offset=500, self_offset=500, diag_hostname=None, error_offset=5000000): rospy.init_node(NAME, anonymous=True) diag_updater = DiagnosticUpdater( name=namespace + 'ntp', display_name=diag_hostname + ' NTP', ) hostname = socket.gethostname() if diag_hostname is None: diag_hostname = hostname ntp_hostname = rospy.get_param('~reference_host', 'ntp.ubuntu.com') offset = rospy.get_param('~offset_tolerance', 500) error_offset = rospy.get_param('~error_offset_tolerance', 5000000) stat = DiagnosticStatus() stat.level = 0 stat.name = "NTP Offset" stat.message = "OK" stat.hardware_id = hostname stat.values = [] stat_diagnostic = GenericDiagnostic('/offset') stat_diagnostic.add_to_updater(diag_updater) # self_stat = DiagnosticStatus() # self_stat.level = DiagnosticStatus.OK # self_stat.name = "NTP self-offset for "+ diag_hostname # self_stat.message = "OK" # self_stat.hardware_id = hostname # self_stat.values = [] while not rospy.is_shutdown(): for st, host, off in [(stat, ntp_hostname, offset)]: try: p = Popen(["ntpdate", "-q", host], stdout=PIPE, stdin=PIPE, stderr=PIPE) res = p.wait() (o, e) = p.communicate() except OSError, (errno, msg): if errno == 4: break #ctrl-c interrupt else: raise if (res == 0): measured_offset = float(re.search("offset (.*),", o).group(1)) * 1000000 st.level = DiagnosticStatus.OK st.message = "OK" st.values = [ KeyValue("Offset (us)", str(measured_offset)), KeyValue("Offset tolerance (us)", str(off)), KeyValue("Offset tolerance (us) for Error", str(error_offset)) ] if (abs(measured_offset) > off): st.level = DiagnosticStatus.WARN st.message = "NTP offset too high" if (abs(measured_offset) > error_offset): st.level = DiagnosticStatus.ERROR st.message = "NTP offset too high" else: # Warning (not error), since this is a non-critical failure. st.level = DiagnosticStatus.WARN st.message = "Error running ntpdate (returned %d)" % res st.values = [ KeyValue("Offset (us)", "N/A"), KeyValue("Offset tolerance (us)", str(off)), KeyValue("Offset tolerance (us) for Error", str(error_offset)), KeyValue("Output", o), KeyValue("Errors", e) ] # Convert from ROS diagnostics to mbot_diagnostics for publishing. stat_diagnostic.set_status(Status(stat.level), stat.message) for diag_val in stat.values: stat_diagnostic.set_metric(diag_val.key, diag_val.value) time.sleep(1)
def cb_diagnostics_update(self): """ Callback periodically called to update the diagnostics status of the TouchOscInterface. """ msg = DiagnosticArray() msg.header.stamp = rospy.Time.now() msg.status = [] # If the callbacks DiagnosticStatus message hasn't been populated, # do that now. if not self._diagnostic_status_callbacks: callback_status = DiagnosticStatus() callback_status.level = callback_status.OK callback_status.name = " ".join([self.ros_name, "Registered Callbacks"]) callback_status.hardware_id = self.ros_name callback_status.message = "OK" callback_status.values = [] diags = [(k, v) for k, v in walk_node(self._osc_receiver)] rospy.logdebug("Registered Callbacks:") for (k, v) in diags: rospy.logdebug('{0:<30}{1:<30}'.format(k, v)) callback_status.values.append(KeyValue(key=k, value=v)) self._diagnostic_status_callbacks = callback_status msg.status.append(self._diagnostic_status_callbacks) # Populate the clients DiagnosticStatus message diagnostic_status_clients = DiagnosticStatus() diagnostic_status_clients.level = diagnostic_status_clients.OK diagnostic_status_clients.name = " ".join([self.ros_name, "Client Status"]) diagnostic_status_clients.hardware_id = self.ros_name diagnostic_status_clients.message = "Listening on %d" % self.osc_port diagnostic_status_clients.values = [] #TODO: The clients property accessor is not thread-safe, as far as I can #tell, but using a lock tends to make bonjour hang. clients = self.clients for client in clients.itervalues(): diagnostic_status_clients.values.append(KeyValue( key=client.address, value=client.servicename.split("[")[0])) diagnostic_status_clients.values.append(KeyValue( key=client.address + " Type", value=client.client_type)) diagnostic_status_clients.values.append(KeyValue( key=client.address + " Current", value=client.active_tabpage)) diagnostic_status_clients.values.append(KeyValue( key=client.address + " Tabpages", value=", ".join(client.tabpages))) if len(self.clients) == 0: diagnostic_status_clients.message = "No clients detected" msg.status.append(diagnostic_status_clients) # For each registered tabpage handler, get a DiagnosticStatus message. for tabpage in self.tabpage_handlers.itervalues(): msg.status.append(tabpage.cb_diagnostics_update()) # Publish self.diagnostics_pub.publish(msg) reactor.callLater(1.0, self.cb_diagnostics_update)
batteryPercentagePublisher = rospy.Publisher( "/spencer/sensors/battery/percentage", Float32, latch=True, queue_size=1) rospy.loginfo("Starting robot battery monitor...") while not rospy.is_shutdown(): 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)
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)
def __init__(self, coz, node_name): """ :type coz: cozmo.Robot :param coz: The cozmo SDK robot handle (object). :type node_name: String :param node_name: Name for the node """ # initialize node super().__init__(node_name) # node timer self._timer_rate = 10 # Hz self.timer = self.create_timer(1 / self._timer_rate, self.timer_callback) # vars self._cozmo = coz self._lin_vel = .0 self._ang_vel = .0 self._cmd_lin_vel = .0 self._cmd_ang_vel = .0 self._last_pose = self._cozmo.pose self._wheel_vel = (0, 0) self._optical_frame_orientation = quaternion_from_euler( -np.pi / 2., .0, -np.pi / 2.) self._camera_info_manager = CameraInfoManager( self, 'cozmo_camera', namespace='/cozmo_camera') # tf self._tfb = TransformBroadcaster(self) # params ## TODO: use rosparam when rclpy supports it #self._odom_frame = rospy.get_param('~odom_frame', 'odom') self._odom_frame = 'odom' #self._footprint_frame = rospy.get_param('~footprint_frame', 'base_footprint') self._footprint_frame = 'base_footprint' #self._base_frame = rospy.get_param('~base_frame', 'base_link') self._base_frame = 'base_link' #self._head_frame = rospy.get_param('~head_frame', 'head_link') self._head_frame = 'head_link' #self._camera_frame = rospy.get_param('~camera_frame', 'camera_link') self._camera_frame = 'camera_link' #self._camera_optical_frame = rospy.get_param('~camera_optical_frame', 'cozmo_camera') self._camera_optical_frame = 'cozmo_camera' #camera_info_url = rospy.get_param('~camera_info_url', '') camera_info_url = 'file://' + os.path.dirname( os.path.abspath(__file__)) + '/config/cozmo_camera.yaml' # pubs #self._joint_state_pub = rospy.Publisher('joint_states', JointState, queue_size=1) self._joint_state_pub = self.create_publisher(JointState, 'joint_states') #self._odom_pub = rospy.Publisher('odom', Odometry, queue_size=1) self._odom_pub = self.create_publisher(Odometry, 'odom') #self._imu_pub = rospy.Publisher('imu', Imu, queue_size=1) self._imu_pub = self.create_publisher(Imu, 'imu') #self._battery_pub = rospy.Publisher('battery', BatteryState, queue_size=1) self._battery_pub = self.create_publisher(BatteryState, 'battery') # Note: camera is published under global topic (preceding "/") #self._image_pub = rospy.Publisher('/cozmo_camera/image', Image, queue_size=10) self._image_pub = self.create_publisher(Image, '/cozmo_camera/image') #self._camera_info_pub = rospy.Publisher('/cozmo_camera/camera_info', CameraInfo, queue_size=10) self._camera_info_pub = self.create_publisher( CameraInfo, '/cozmo_camera/camera_info') # subs #self._backpack_led_sub = rospy.Subscriber( # 'backpack_led', ColorRGBA, self._set_backpack_led, queue_size=1) self._backpack_led_sub = self.create_subscription( ColorRGBA, 'backpack_led', self._set_backpack_led) #self._twist_sub = rospy.Subscriber('cmd_vel', Twist, self._twist_callback, queue_size=1) self._twist_sub = self.create_subscription(Twist, 'cmd_vel', self._twist_callback) #self._say_sub = rospy.Subscriber('say', String, self._say_callback, queue_size=1) self._say_sub = self.create_subscription(String, 'say', self._say_callback) #self._head_sub = rospy.Subscriber('head_angle', Float64, self._move_head, queue_size=1) self._head_sub = self.create_subscription(Float64, 'head_angle', self._move_head) #self._lift_sub = rospy.Subscriber('lift_height', Float64, self._move_lift, queue_size=1) self._lift_sub = self.create_subscription(Float64, 'lift_height', self._move_lift) # diagnostics self._diag_array = DiagnosticArray() self._diag_array.header.frame_id = self._base_frame diag_status = DiagnosticStatus() diag_status.hardware_id = 'Cozmo Robot' diag_status.name = 'Cozmo Status' diag_status.values.append(KeyValue(key='Battery Voltage', value='')) diag_status.values.append(KeyValue(key='Head Angle', value='')) diag_status.values.append(KeyValue(key='Lift Height', value='')) self._diag_array.status.append(diag_status) #self._diag_pub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=1) self._diag_pub = self.create_publisher(DiagnosticArray, '/diagnostics') # camera info manager self._camera_info_manager.setURL(camera_info_url) self._camera_info_manager.loadCameraInfo()
def diagnostics(self): ds = DiagnosticStatus() ds.name = rospy.get_caller_id().lstrip('/') + ": " + "Network Watchdog" ds.hardware_id = "none" with self.mutex: now = rospy.get_time() time_to_timeout = self.timeout - (now - self.interruption_time) elapsed_orig = now - self.interruption_time_orig # Parameters ds.values.append(KeyValue("Timeout (s)", "%.1f"%self.timeout)) ds.values.append(KeyValue("Latency threshold (ms)", str(int(self.max_latency * 1000)))) ds.values.append(KeyValue("Loss threshold (%)", "%.1f"%(self.max_loss * 100))) ds.values.append(KeyValue("Timeout command", str(self.action_name))) # Network outage stats ds.values.append(KeyValue("Network is down", str(self.outage_in_progress))) ds.values.append(KeyValue("Outage count", str(self.outage_count))) if self.outage_in_progress: ds.values.append(KeyValue("Time since outage start (s)", "%.1f"%elapsed_orig)) if self.outage_length: ds.values.append(KeyValue("Latest outage length", "%.1f"%self.outage_length)) ds.values.append(KeyValue("Longest outage length (s)", "%.1f"%self.longest_outage)) # Command stats ds.values.append(KeyValue("Timeout command in progress", str(self.action_running))) ds.values.append(KeyValue("Trigger count", str(self.trigger_count))) if self.outage_in_progress: if not self.action_running: ds.values.append(KeyValue("Time to timeout (s)", "%.1f"%(time_to_timeout))) if self.action_running: ds.values.append(KeyValue("Time since trigger", str(now - self.latest_trigger_time))) if self.longest_action_time: ds.values.append(KeyValue("Longest timeout command run time", "%.1f"%self.longest_action_duration)) # Stats ds.values.append(KeyValue("Node start time", time.ctime(self.start_time))) if self.interruption_time_orig != inf: ds.values.append(KeyValue("Latest outage start time", time.ctime(self.interruption_time_orig))) if self.longest_outage_time: ds.values.append(KeyValue("Longest outage start time", time.ctime(self.longest_outage_time))) if self.latest_trigger_time: ds.values.append(KeyValue("Latest trigger time", time.ctime(self.latest_trigger_time))) if self.longest_action_time: ds.values.append(KeyValue("Time of longest timeout command", time.ctime(self.longest_action_time))) # Summary if self.interruption_time == inf: ds.message = "Network is up" ds.level = DiagnosticStatus.OK if self.action_running: ds.message += "; Timeout command in progress (%.0f s)"%(now - self.latest_trigger_time) else: if self.action_running: ds.message = "Timeout command in progress (%.0f s; %.0f s)"%(elapsed_orig, now - self.latest_trigger_time) ds.level = DiagnosticStatus.ERROR else: ds.message = "Network is down (%.0f s; %.0f s)"%(elapsed_orig, time_to_timeout) ds.level = DiagnosticStatus.WARN da = DiagnosticArray() da.header.stamp = rospy.get_rostime() da.status.append(ds) self.diag_pub.publish(da)
averageDelay += delay 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))
while not rospy.is_shutdown(): if time.time() > nextQueryAt: try: batteryVoltage, batteryPercentage, laptopPluggedIn = getBatteryState() nextQueryAt = time.time() + 15.0 # query interval 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()
import time from water_sampler_ros.srv import * # Initialize pumps object pumps = Sampler() # Diagnostic status for services fill_pump_status = DiagnosticStatus() check_pump_status = DiagnosticStatus() empty_pump_status = DiagnosticStatus() stop_pump_status = DiagnosticStatus() fill_pump_status.name = 'sampler fill pump service' check_pump_status.name = 'sampler check pump service' empty_pump_status.name = 'sampler empty pump service' stop_pump_status.name = 'sampler stop pump service' fill_pump_status.hardware_id = 'sampler' check_pump_status.hardware_id = 'sampler' empty_pump_status.hardware_id = 'sampler' stop_pump_status.hardware_id = 'sampler' def check_status(status, srv): # Check if service exists in ROS services if any(srv in s for s in rosservice.get_service_list()): status.level = DiagnosticStatus.OK status.message = "OK" status.values = [KeyValue(key="Update Status", value="OK")] else: status.level = DiagnosticStatus.ERROR status.message = "Error" status.values = [
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() 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(KeyValue('Average interval between messages', "%.3f sec" % averageInterval)) if hasHeader: topicStatus.values.append(KeyValue('Maximum delay wrt. current ROS time', "%.3f sec" % maxDelay))
def add_sentence(self, nmea_string, frame_id, timestamp=None): if not check_nmea_checksum(nmea_string): rospy.logwarn("Received a sentence with an invalid checksum. " + "Sentence was: %s" % repr(nmea_string)) return False parsed_sentence = libnmea_navsat_driver.parser.parse_nmea_sentence( nmea_string) if not parsed_sentence: rospy.logdebug("Failed to parse NMEA sentence. Sentence was: %s" % nmea_string) return False if timestamp: current_time = timestamp else: current_time = rospy.get_rostime() current_fix = NavSatFix() current_fix.header.stamp = current_time current_fix.header.frame_id = frame_id current_pose_utm = GpsLocal() current_pose_utm.header.stamp = current_time current_pose_utm.header.frame_id = frame_id current_time_ref = TimeReference() current_time_ref.header.stamp = current_time current_time_ref.header.frame_id = frame_id if self.time_ref_source: current_time_ref.source = self.time_ref_source else: current_time_ref.source = frame_id if not self.use_RMC and 'GGA' in parsed_sentence: self.seq = self.seq + 1 current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_APPROXIMATED data = parsed_sentence['GGA'] gps_qual = data['fix_type'] try: # Unpack the fix params for this quality value current_fix.status.status, self.default_epe, self.fix_type = self.fix_mappings[ gps_qual] except KeyError: current_fix.status.status = NavSatStatus.STATUS_NO_FIX self.fix_type = 'Unknown' if gps_qual == 0: current_fix.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN self.using_receiver_epe = False self.invalid_cnt += 1 current_fix.status.service = NavSatStatus.SERVICE_GPS current_fix.header.stamp = current_time latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude # use default epe std_dev unless we've received a GST sentence with epes if not self.using_receiver_epe or math.isnan(self.lon_std_dev): self.lon_std_dev = self.default_epe if not self.using_receiver_epe or math.isnan(self.lat_std_dev): self.lat_std_dev = self.default_epe if not self.using_receiver_epe or math.isnan(self.alt_std_dev): self.alt_std_dev = self.default_epe * 2 hdop = data['hdop'] current_fix.position_covariance[0] = (hdop * self.lon_std_dev)**2 current_fix.position_covariance[4] = (hdop * self.lat_std_dev)**2 current_fix.position_covariance[8] = (hdop * self.alt_std_dev)**2 # FIXME # Altitude is above ellipsoid, so adjust for mean-sea-level altitude = data['altitude'] + data['mean_sea_level'] current_fix.altitude = altitude self.fix_pub.publish(current_fix) if not math.isnan(latitude) and not math.isnan(longitude) and ( gps_qual == 5 or gps_qual == 4): UTMNorthing, UTMEasting = LLtoUTM(latitude, longitude)[0:2] current_pose_utm.position.x = UTMEasting current_pose_utm.position.y = UTMNorthing current_pose_utm.position.z = altitude # Pose x/y/z covariance is whatever we decided h & v covariance is. # Here is it the same as for ECEF coordinates current_pose_utm.covariance[0] = (hdop * self.lon_std_dev)**2 current_pose_utm.covariance[4] = (hdop * self.lat_std_dev)**2 current_pose_utm.covariance[8] = (hdop * self.alt_std_dev)**2 current_pose_utm.rtk_fix = True if gps_qual == 4 else False self.local_pub.publish(current_pose_utm) if not math.isnan(data['utc_time']): current_time_ref.time_ref = rospy.Time.from_sec( data['utc_time']) self.time_ref_pub.publish(current_time_ref) 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 = frame_id diag_msg = DiagnosticStatus() diag_msg.name = 'GPS_status' diag_msg.hardware_id = 'GPS' diag_msg.level = DiagnosticStatus.OK diag_msg.message = 'Received GGA Fix' diag_msg.values.append( KeyValue('Sequence number', str(self.seq))) diag_msg.values.append( KeyValue('Invalid fix count', str(self.invalid_cnt))) diag_msg.values.append(KeyValue('Latitude', str(latitude))) diag_msg.values.append(KeyValue('Longitude', str(longitude))) diag_msg.values.append(KeyValue('Altitude', str(altitude))) diag_msg.values.append(KeyValue('GPS quality', str(gps_qual))) diag_msg.values.append(KeyValue('Fix type', self.fix_type)) diag_msg.values.append( KeyValue('Number of satellites', str(data['num_satellites']))) diag_msg.values.append( KeyValue('Receiver providing accuracy', str(self.using_receiver_epe))) diag_msg.values.append(KeyValue('Hdop', str(hdop))) diag_msg.values.append( KeyValue('Latitude std dev', str(hdop * self.lat_std_dev))) diag_msg.values.append( KeyValue('Longitude std dev', str(hdop * self.lon_std_dev))) diag_msg.values.append( KeyValue('Altitude std dev', str(hdop * self.alt_std_dev))) diag_arr.status.append(diag_msg) self.diag_pub.publish(diag_arr) elif 'RMC' in parsed_sentence: data = parsed_sentence['RMC'] # Only publish a fix from RMC if the use_RMC flag is set. if self.use_RMC: if data['fix_valid']: current_fix.status.status = NavSatStatus.STATUS_FIX else: current_fix.status.status = NavSatStatus.STATUS_NO_FIX current_fix.status.service = NavSatStatus.SERVICE_GPS latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude current_fix.altitude = float('NaN') current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_UNKNOWN self.fix_pub.publish(current_fix) if not math.isnan(data['utc_time']): current_time_ref.time_ref = rospy.Time.from_sec( data['utc_time']) self.time_ref_pub.publish(current_time_ref) # Publish velocity from RMC regardless, since GGA doesn't provide it. if data['fix_valid']: current_vel = TwistStamped() current_vel.header.stamp = current_time current_vel.header.frame_id = frame_id current_vel.twist.linear.x = data['speed'] * \ math.sin(data['true_course']) current_vel.twist.linear.y = data['speed'] * \ math.cos(data['true_course']) self.vel_pub.publish(current_vel) elif 'GST' in parsed_sentence: data = parsed_sentence['GST'] # Use receiver-provided error estimate if available self.using_receiver_epe = True self.lon_std_dev = data['lon_std_dev'] self.lat_std_dev = data['lat_std_dev'] self.alt_std_dev = data['alt_std_dev'] else: return False
#!/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 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 __init__(self, coz): """ :type coz: cozmo.Robot :param coz: The cozmo SDK robot handle (object). """ # vars self._cozmo = coz self._lin_vel = .0 self._ang_vel = .0 self._cmd_lin_vel = .0 self._cmd_ang_vel = .0 self._last_pose = self._cozmo.pose self._wheel_vel = (0, 0) self._optical_frame_orientation = quaternion_from_euler(-np.pi/2., .0, -np.pi/2.) self._camera_info_manager = CameraInfoManager('cozmo_camera', namespace='/cozmo_camera') self.loc1Found=False self.loc2Found=False self.loc3Found=False self.rampFound=False self.allLocFound=False # tf self._tfb = TransformBroadcaster() # params self._odom_frame = rospy.get_param('~odom_frame', 'odom') self._footprint_frame = rospy.get_param('~footprint_frame', 'base_footprint') self._base_frame = rospy.get_param('~base_frame', 'base_link') self._head_frame = rospy.get_param('~head_frame', 'head_link') self._camera_frame = rospy.get_param('~camera_frame', 'camera_link') self._camera_optical_frame = rospy.get_param('~camera_optical_frame', 'cozmo_camera') camera_info_url = rospy.get_param('~camera_info_url', '') # pubs self._joint_state_pub = rospy.Publisher('joint_states', JointState, queue_size=1) self._odom_pub = rospy.Publisher('odom', Odometry, queue_size=1) self._imu_pub = rospy.Publisher('imu', Imu, queue_size=1) self._battery_pub = rospy.Publisher('battery', BatteryState, queue_size=1) # Note: camera is published under global topic (preceding "/") self._image_pub = rospy.Publisher('/cozmo_camera/image', Image, queue_size=10) self._camera_info_pub = rospy.Publisher('/cozmo_camera/camera_info', CameraInfo, queue_size=10) # subs self._backpack_led_sub = rospy.Subscriber( 'backpack_led', ColorRGBA, self._set_backpack_led, queue_size=1) self._twist_sub = rospy.Subscriber('cmd_vel', Twist, self._twist_callback, queue_size=1) self._say_sub = rospy.Subscriber('say', String, self._say_callback, queue_size=1) self._head_sub = rospy.Subscriber('head_angle', Float64, self._move_head, queue_size=1) self._lift_sub = rospy.Subscriber('lift_height', Float64, self._move_lift, queue_size=1) # diagnostics self._diag_array = DiagnosticArray() self._diag_array.header.frame_id = self._base_frame diag_status = DiagnosticStatus() diag_status.hardware_id = 'Cozmo Robot' diag_status.name = 'Cozmo Status' diag_status.values.append(KeyValue(key='Battery Voltage', value='')) diag_status.values.append(KeyValue(key='Head Angle', value='')) diag_status.values.append(KeyValue(key='Lift Height', value='')) self._diag_array.status.append(diag_status) self._diag_pub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=1) # camera info manager self._camera_info_manager.setURL(camera_info_url) self._camera_info_manager.loadCameraInfo() self.twist = Twist() custom_objects(self._cozmo)
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) res = p.wait() (o,e) = p.communicate()
insock.bind(('',11111)) rospy.init_node('mbr_logging') pubs = {} diagnostic_pub = rospy.Publisher('/diagnostics',DiagnosticArray,queue_size=10) while not rospy.is_shutdown(): data = insock.recv(1024) values = data.decode('utf8').split(',') sn = values[0] if len(values) >= 24: diag_array = DiagnosticArray() diag_array.header.stamp = rospy.Time.now() ds = DiagnosticStatus() ds.name = 'mbr' ds.hardware_id = values[0] for i in range(len(fields)): ds.values.append(KeyValue(fields[i],values[i])) for i in (21,22): if not sn+'/'+fields[i] in pubs: pubs[sn+'/'+fields[i]] = rospy.Publisher('mbr/'+sn+'/'+fields[i],Float32, queue_size=10) pubs[sn+'/'+fields[i]].publish(float(values[i])) site_count = int(values[23]) for i in range(site_count): base_index = 24+i*21 if len(values) >= base_index+21: topic = 'mbr/'+sn+'/'+values[base_index+0]+'/mean_margin' if not topic in pubs: pubs[topic] = rospy.Publisher(topic, Float32, queue_size=10)
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, 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)
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 __init__(self, vec): """ :type vec: anki_vector.Robot :param vec: The vector SDK robot handle (object). """ # vars self._vector = vec self._lin_vel = .0 self._ang_vel = .0 self._cmd_lin_vel = .0 self._cmd_ang_vel = .0 self._last_pose = self._vector.pose self._wheel_vel = (0, 0) self._optical_frame_orientation = quaternion_from_euler(-np.pi/2., .0, -np.pi/2.) # self._camera_info_manager = CameraInfoManager('vector_camera', namespace='/vector_camera') # tf self._tfb = TransformBroadcaster() # params self._odom_frame = rospy.get_param('~odom_frame', 'odom') self._footprint_frame = rospy.get_param('~footprint_frame', 'base_footprint') self._base_frame = rospy.get_param('~base_frame', 'base_link') self._head_frame = rospy.get_param('~head_frame', 'head_link') self._camera_frame = rospy.get_param('~camera_frame', 'camera_link') self._camera_optical_frame = rospy.get_param('~camera_optical_frame', 'vector_camera') # camera_info_url = rospy.get_param('~camera_info_url', '') # pubs self._joint_state_pub = rospy.Publisher('joint_states', JointState, queue_size=1) self._odom_pub = rospy.Publisher('odom', Odometry, queue_size=1) self._imu_pub = rospy.Publisher('imu', Imu, queue_size=1) self._battery_pub = rospy.Publisher('battery', BatteryState, queue_size=1) self._touch_pub = rospy.Publisher('touch', Int16, queue_size=1) self._laser_pub = rospy.Publisher('laser', Range, queue_size=50) # Note: camera is published under global topic (preceding "/") self._image_pub = rospy.Publisher('/vector_camera/image', Image, queue_size=10) # self._camera_info_pub = rospy.Publisher('/vector_camera/camera_info', CameraInfo, queue_size=10) # subs # self._backpack_led_sub = rospy.Subscriber( # 'backpack_led', ColorRGBA, self._set_backpack_led, queue_size=1) self._twist_sub = rospy.Subscriber('cmd_vel', Twist, self._twist_callback, queue_size=1) self._say_sub = rospy.Subscriber('say', String, self._say_callback, queue_size=1) self._head_sub = rospy.Subscriber('head_angle', Float64, self._move_head, queue_size=1) self._lift_sub = rospy.Subscriber('lift_height', Float64, self._move_lift, queue_size=1) # diagnostics self._diag_array = DiagnosticArray() self._diag_array.header.frame_id = self._base_frame diag_status = DiagnosticStatus() diag_status.hardware_id = 'Vector Robot' diag_status.name = 'Vector Status' diag_status.values.append(KeyValue(key='Battery Voltage', value='')) diag_status.values.append(KeyValue(key='Head Angle', value='')) diag_status.values.append(KeyValue(key='Lift Height', value='')) self._diag_array.status.append(diag_status) self._diag_pub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=1)
def __init__(self, coz): """ :type coz: cozmo.Robot :param coz: The cozmo SDK robot handle (object). """ # vars self._cozmo = coz self._lin_vel = .0 self._ang_vel = .0 self._cmd_lin_vel = .0 self._cmd_ang_vel = .0 self._last_pose = self._cozmo.pose self._wheel_vel = (0, 0) self._optical_frame_orientation = quaternion_from_euler( -np.pi / 2., .0, -np.pi / 2.) self._camera_info_manager = CameraInfoManager( 'cozmo_camera', namespace='/cozmo_camera') # tf self._tfb = TransformBroadcaster() # params self._odom_frame = rospy.get_param('~odom_frame', 'odom') self._footprint_frame = rospy.get_param('~footprint_frame', 'base_footprint') self._base_frame = rospy.get_param('~base_frame', 'base_link') self._head_frame = rospy.get_param('~head_frame', 'head_link') self._camera_frame = rospy.get_param('~camera_frame', 'camera_link') self._camera_optical_frame = rospy.get_param('~camera_optical_frame', 'cozmo_camera') camera_info_url = rospy.get_param('~camera_info_url', '') # pubs self._joint_state_pub = rospy.Publisher('joint_states', JointState, queue_size=1) self._odom_pub = rospy.Publisher('odom', Odometry, queue_size=1) self._imu_pub = rospy.Publisher('imu', Imu, queue_size=1) self._battery_pub = rospy.Publisher('battery', BatteryState, queue_size=1) # Note: camera is published under global topic (preceding "/") self._image_pub = rospy.Publisher('/cozmo_camera/image', Image, queue_size=10) self._camera_info_pub = rospy.Publisher('/cozmo_camera/camera_info', CameraInfo, queue_size=10) # subs self._backpack_led_sub = rospy.Subscriber('backpack_led', ColorRGBA, self._set_backpack_led, queue_size=1) self._twist_sub = rospy.Subscriber('cmd_vel', Twist, self._twist_callback, queue_size=1) self._say_sub = rospy.Subscriber('say', String, self._say_callback, queue_size=1) self._head_sub = rospy.Subscriber('head_angle', Float64, self._move_head, queue_size=1) self._lift_sub = rospy.Subscriber('lift_height', Float64, self._move_lift, queue_size=1) # diagnostics self._diag_array = DiagnosticArray() self._diag_array.header.frame_id = self._base_frame diag_status = DiagnosticStatus() diag_status.hardware_id = 'Cozmo Robot' diag_status.name = 'Cozmo Status' diag_status.values.append(KeyValue(key='Battery Voltage', value='')) diag_status.values.append(KeyValue(key='Head Angle', value='')) diag_status.values.append(KeyValue(key='Lift Height', value='')) self._diag_array.status.append(diag_status) self._diag_pub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=1) # camera info manager self._camera_info_manager.setURL(camera_info_url) self._camera_info_manager.loadCameraInfo() # Raise the head self.move_head(10) # level the head self.move_lift(0) # raise the lift # Start the tasks self.task = 0 self.goal_pose = self._cozmo.pose self.action = None self._cozmo.add_event_handler(cozmo.objects.EvtObjectAppeared, self.handle_object_appeared) self._cozmo.add_event_handler(cozmo.objects.EvtObjectDisappeared, self.handle_object_disappeared) self.front_wall = self._cozmo.world.define_custom_wall( CustomObjectTypes.CustomType00, CustomObjectMarkers.Hexagons2, 300, 44, 63, 63, True) self.ramp_bottom = self._cozmo.world.define_custom_wall( CustomObjectTypes.CustomType01, CustomObjectMarkers.Triangles5, 100, 100, 63, 63, True) self.ramp_top = self._cozmo.world.define_custom_wall( CustomObjectTypes.CustomType02, CustomObjectMarkers.Diamonds2, 5, 5, 44, 44, True) self.drop_spot = self._cozmo.world.define_custom_wall( CustomObjectTypes.CustomType03, CustomObjectMarkers.Circles4, 70, 70, 63, 63, True) self.back_wall = self._cozmo.world.define_custom_wall( CustomObjectTypes.CustomType04, CustomObjectMarkers.Diamonds4, 300, 50, 63, 63, True) self.drop_target = self._cozmo.world.define_custom_wall( CustomObjectTypes.CustomType05, CustomObjectMarkers.Hexagons5, 5, 5, 32, 32, True) self.drop_clue = self._cozmo.world.define_custom_wall( CustomObjectTypes.CustomType06, CustomObjectMarkers.Triangles3, 5, 5, 32, 32, True) self.front_wall_pose = None self.ramp_bottom_pose = None self.drop_spot_pose = None self.back_wall_pose = None self.drop_target_pose = None self.drop_clue_pose = None self.cube_found = False self.target_cube = None self.is_up_top = False # Is Cozmo on the platform
from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue from time import sleep def callback(msg): print("Got msg") return EmptyResponse() if __name__ == '__main__': rospy.init_node("test_node") rospy.Service('self_test', Empty, callback) pub = rospy.Publisher('/diagnostics', DiagnosticArray) # Publish diagnostics to check runtime monitor, rxconsole msg = DiagnosticArray() stat = DiagnosticStatus() stat.level = 0 stat.name = 'Test Node' stat.message = 'OK' stat.hardware_id = 'HW ID' stat.values = [ KeyValue('Node Status', 'OK') ] msg.status.append(stat) while not rospy.is_shutdown(): msg.header.stamp = rospy.get_rostime() pub.publish(msg) rospy.loginfo('Test node is printing things') sleep(1.0)
def 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): 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 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) rospy.init_node('hwboard') 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 = "hcboard_channel " + str(send_channel) elif read_id == self.eye_sensor_param: status_object.name = "Eye Camera Temperature" status_object.hardware_id = "hcboard_channel = " + str(send_channel) elif read_id == self.torso_module_sensor_param: status_object.name = "Torso Module Temperature" status_object.hardware_id = "hcboard_channel =" + str(send_channel) elif read_id == self.torso_sensor_param: status_object.name = "Torso Temperature" status_object.hardware_id = "hcboard_channel =" + str(send_channel) elif read_id == self.pc_sensor_param: status_object.name = "PC Temperature" status_object.hardware_id = "hcboard_channel =" + str(send_channel) elif read_id == self.engine_sensor_param: status_object.name = "Engine Temperature" status_object.hardware_id = "hcboard_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 = "hcboard_channel = 0" elif send_channel == 1: status_object.name = "Torso Engine Voltage" status_object.hardware_id = "hcboard_channel = 1" elif send_channel == 2: status_object.name = "Torso Logic Voltage" status_object.hardware_id = "hcboard_channel = 2" elif send_channel == 3: status_object.name = "Tray Logic Voltage" status_object.hardware_id = "hcboard_channel = 3" elif send_channel == 6: status_object.name = "Arm Engine Voltage" status_object.hardware_id = "hcboard_channel = 6" elif send_channel == 7: status_object.name = "Tray Engine Voltage" status_object.hardware_id = "hcboard_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 = "hcboard_channel = 1" elif send_channel == 2: status_object.name = "Torso Logic Current" status_object.hardware_id = "hcboard_channel = 2" elif send_channel == 3: status_object.name = "Tray Logic Current" status_object.hardware_id = "hcboard_channel = 3" elif send_channel == 6: status_object.name = "Arm Engine Current" status_object.hardware_id = "hcboard_channel = 6" elif send_channel == 7: status_object.name = "Tray Engine Current" status_object.hardware_id = "hcboard_channel = 7" # evaluate error detection if error_while_reading == 1: level = 1 status_object.message = "detected error while receiving answer from hardware control board" # 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()
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() 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:',