def battery_status(self):
stat = DiagnosticStatus(name="Battery", level=DiagnosticStatus.OK, message="OK")
stat.values = [
KeyValue("Voltage avg(Vin)", str(self.robot.get_avg_voltageIn())),
KeyValue("Voltage max(Vin)", str(self.robot.get_max_voltageIn())),
KeyValue("Voltage min(Vin)", str(self.robot.get_min_voltageIn())),
KeyValue("Current avg(A)", str(self.robot.get_avg_current())),
KeyValue("Current max(A)", str(self.robot.get_avg_current())),
KeyValue("Current min(A)", str(self.robot.get_avg_current())),
KeyValue("Voltage avg(V5V)", str(self.robot.get_avg_5voltage())),
KeyValue("Voltage max(V5V)", str(self.robot.get_max_5voltage())),
KeyValue("Voltage min(V5V)", str(self.robot.get_min_5voltage())),
KeyValue("Voltage avg(V3.3)", str(self.robot.get_avg_3voltage())),
KeyValue("Voltage max(V3.3)", str(self.robot.get_max_3voltage())),
KeyValue("Voltage min(V3.3)", str(self.robot.get_min_3voltage()))]
if self.robot.get_status().VoltageLow == True:
stat.level = DiagnosticStatus.WARN
stat.message = "Voltage too low"
if self.robot.get_status().CurrentError == True:
stat.level = DiagnosticStatus.WARN
stat.message = "Current error"
if self.robot.get_status().Voltage3v3Low == True:
stat.level = DiagnosticStatus.WARN
stat.message = "Voltage3.3 too low"
return stat
def publish_diagnostic(self):
try:
da = DiagnosticArray()
ds = DiagnosticStatus()
ds.name = rospy.get_caller_id().lstrip('/') + ': Status'
ds.hardware_id = self.ifname
if not self.iwconfig.is_enabled():
ds.level = DiagnosticStatus.STALE
ds.message = "Device not found"
elif not self.iwconfig.is_connected():
ds.level = DiagnosticStatus.ERROR
ds.message = "No connection"
else:
if extract_number(self.iwconfig.status["Link Quality"]) < self.warn_quality:
ds.level = DiagnosticStatus.WARN
ds.message = "Connected, but bad quality"
else:
ds.level = DiagnosticStatus.OK
ds.message = "Connected"
for key, val in self.iwconfig.status.items():
ds.values.append(KeyValue(key, val))
da.status.append(ds)
da.header.stamp = rospy.Time.now()
self.diagnostic_pub.publish(da)
except Exception as e:
rospy.logerr('Failed to publish diagnostic: %s' % str(e))
rospy.logerr(traceback.format_exc())
def update_diagnostics(self):
da = DiagnosticArray()
ds = DiagnosticStatus()
ds.name = rospy.get_caller_id().lstrip('/') + ": Tasks"
in_progress = 0;
longest_interval = 0;
for updater in list(asynchronous_updaters):
(name, interval) = updater.getStatus()
if interval == 0:
msg = "Idle"
else:
in_progress = in_progress + 1
msg = "Update in progress (%i s)"%interval
longest_interval = max(interval, longest_interval)
ds.values.append(KeyValue(name, msg))
if in_progress == 0:
ds.message = "Idle"
else:
ds.message = "Updates in progress: %i"%in_progress
if longest_interval > 10:
ds.level = 1
ds.message = "Update is taking too long: %i"%in_progress
ds.hardware_id = "none"
da.status.append(ds)
da.header.stamp = rospy.get_rostime()
self.diagnostic_pub.publish(da)
def _laptop_charge_to_diag(laptop_msg):
rv = DiagnosticStatus()
rv.level = DiagnosticStatus.OK
rv.message = 'OK'
rv.name = 'Laptop Battery'
rv.hardware_id = socket.gethostname()
if not laptop_msg.present:
rv.level = DiagnosticStatus.ERROR
rv.message = 'Laptop battery missing'
charging_state = {
SmartBatteryStatus.DISCHARGING: 'Not Charging',
SmartBatteryStatus.CHARGING: 'Charging',
SmartBatteryStatus.CHARGED: 'Trickle Charging'
}.get(laptop_msg.charge_state, 'Not Charging')
rv.values.append(KeyValue('Voltage (V)', str(laptop_msg.voltage)))
rv.values.append(KeyValue('Current (A)', str(laptop_msg.rate)))
rv.values.append(KeyValue('Charge (Ah)', str(laptop_msg.charge)))
rv.values.append(KeyValue('Capacity (Ah)', str(laptop_msg.capacity)))
rv.values.append(KeyValue('Design Capacity (Ah)', str(laptop_msg.design_capacity)))
rv.values.append(KeyValue('Charging State', str(charging_state)))
return rv
def _publish_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 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 diagnosticsCallback (event):
array = DiagnosticArray()
# Set the timestamp for diagnostics
array.header.stamp = rospy.Time.now()
motors_message = DiagnosticStatus(name = 'PhidgetMotors', level = 0,message = 'initialized', hardware_id='Phidget')
if (motorsError == 1):
motors_message.level = 2
motors_message.message = "Phidget Motor controller can't be initialized"
motors_message.values = [ KeyValue(key = 'Recommendation', value = motorControllerDisconnected)]
if (motorsError == 2):
motors_message.level = 2
motors_message.message = "Can't set up motor speed"
motors_message.values = [ KeyValue(key = 'Recommendation', value = motorSpeedError)]
encoders_message = DiagnosticStatus(name = 'PhidgetEncoders', level = 0,message = 'initialized', hardware_id='Phidget')
if (encodersError == 1):
encoders_message.level = 2
encoders_message.message = "Phidget Encoder board can't be initialized"
encoders_message.values = [ KeyValue(key = 'Recommendation', value = encoderBoardDisconnected)]
if (encodersError == 2):
encoders_message.level = 2
encoders_message.message = "Can't get encoder value"
encoders_message.values = [ KeyValue(key = 'Recommendation', value = encoderValueError)]
array.status = [ motors_message, encoders_message ]
diagnosticPub.publish(array)
def 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 status_cb(msg):
global BATTERY_STATES
global diag_publisher
diag_msg = DiagnosticArray()
diag_msg.header.stamp = rospy.Time.now()
diag_msg.status = []
temp_status = DiagnosticStatus()
temp_status.name = "Chassis Temperature"
temp_status.hardware_id = "automow_pcb"
temp_status.values = []
top_F = msg.temperature_1 * 9/5 + 32
bot_F = msg.temperature_2 * 9/5 + 32
temp_status.values.append(KeyValue(key="Top Celsius",
value="%3.2f C"%msg.temperature_1))
temp_status.values.append(KeyValue(key="Bottom Celsius",
value="%3.2f C"%msg.temperature_2))
temp_status.values.append(KeyValue(key="Top Fahrenheit",
value="%3.2f F"%(top_F)))
temp_status.values.append(KeyValue(key="Bottom Fahrenheit",
value="%3.2f F"%(bot_F)))
if top_F > 100 or bot_F > 100:
temp_status.message = "High Temperature"
temp_status.level = temp_status.WARN
elif top_F > 125 or bot_F > 125:
temp_status.message = "Critical Temperature"
temp_status.level = temp_status.ERROR
else:
temp_status.message = "OK"
temp_status.level = temp_status.OK
diag_msg.status.append(temp_status)
batt_status = DiagnosticStatus()
batt_status.name = "Battery Status"
batt_status.hardware_id = "automow_pcb"
batt_status.values = []
state = BATTERY_STATES[msg.battery_state]
batt_status.values.append(KeyValue(key="State",
value=state))
batt_status.values.append(KeyValue(key="Charge",
value="{:.0%}".format(msg.charge/100.0)))
batt_status.values.append(KeyValue(key="Voltage",
value="%3.2f V"%(msg.voltage/1000.0)))
batt_status.values.append(KeyValue(key="Battery Current",
value="%3.2f A"%(msg.current/1000.0)))
diag_msg.status.append(batt_status)
if msg.battery_state >= 4:
batt_status.level = batt_status.ERROR
else:
batt_status.level = batt_status.OK
batt_status.message = state
diag_publisher.publish(diag_msg)
def mark_diag_stale(diag_stat = None, error = False):
if not diag_stat:
diag_stat = DiagnosticStatus()
diag_stat.message = 'No Updates'
diag_stat.name = DIAG_NAME
else:
diag_stat.message = 'Updates Stale'
diag_stat.level = DiagnosticStatus.WARN
if error:
diag_stat.level = DiagnosticStatus.ERROR
return diag_stat
def odom_cb(self, msg):
with self.lock:
dist = msg.distance + (msg.angle * 0.25)
# check if base moved
if dist > self.dist + EPS:
self.start_time = rospy.Time.now()
self.start_angle = self.last_angle
self.dist = dist
# do imu test if possible
if rospy.Time.now() > self.start_time + rospy.Duration(10.0):
self.drift = fabs(self.start_angle - self.last_angle)*180/(pi*10)
self.start_time = rospy.Time.now()
self.start_angle = self.last_angle
self.last_measured = rospy.Time.now()
# publish diagnostics
d = DiagnosticArray()
d.header.stamp = rospy.Time.now()
ds = DiagnosticStatus()
ds.name = "imu_node: Imu Drift Monitor"
if self.drift < 0.5:
ds.level = DiagnosticStatus.OK
ds.message = 'OK'
elif self.drift < 1.0:
ds.level = DiagnosticStatus.WARN
ds.message = 'Drifting'
else:
ds.level = DiagnosticStatus.ERROR
ds.message = 'Drifting'
drift = self.drift
if self.drift < 0:
last_measured = 'No measurements yet, waiting for base to stop moving before measuring'
drift = 'N/A'
else:
age = (rospy.Time.now() - self.last_measured).to_sec()
if age < 60:
last_measured = '%f seconds ago'%age
elif age < 3600:
last_measured = '%f minutes ago'%(age/60)
else:
last_measured = '%f hours ago'%(age/3600)
ds.values = [
KeyValue('Last measured', last_measured),
KeyValue('Drift (deg/sec)', str(drift)) ]
d.status = [ds]
self.pub_diag.publish(d)
def callback(self, _msg):
msg = DiagnosticArray()
statuses = []
for _status in _msg.status:
status = DiagnosticStatus()
status.level = _status.level
status.name = _status.name
if _status.code == 0:
status.message = ""
else:
status.message = self.status_codes_by_module[_status.name].get(
_status.code, "Unknown error"
)
statuses.append(status)
msg.status = statuses
self.pub.publish(msg)
def 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 _camera_diag(level = 0):
array = DiagnosticArray()
stat = DiagnosticStatus()
stat.name = 'wge100: Driver Status'
stat.level = level
stat.message = 'OK'
motor_stat = DiagnosticStatus()
motor_stat.name = 'EtherCAT Master'
motor_stat.level = 0
motor_stat.values = [
KeyValue(key='Dropped Packets', value='0'),
KeyValue(key='RX Late Packet', value='0')]
mcb_stat = DiagnosticStatus()
mcb_stat.name = 'EtherCAT Device (my_motor)'
mcb_stat.level = 0
mcb_stat.values.append(KeyValue('Num encoder_errors', '0'))
array.header.stamp = rospy.get_rostime()
array.status.append(stat)
array.status.append(motor_stat)
array.status.append(mcb_stat)
return array
def 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 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'
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 connection_status(self):
stat = DiagnosticStatus(name="Connection", level=DiagnosticStatus.OK, message="OK")
stat.values = [
KeyValue("Baudrate", str(self.robot.get_connection_info()["baudrate"])),
KeyValue("Comport", str(self.robot.get_connection_info()["comport"]))]
if self.robot.is_connected() == False:
stat.message = "disconnected"
stat.level = DiagnosticStatus.ERROR
return stat
def speed_status(self):
stat = DiagnosticStatus(name="Speed", level=DiagnosticStatus.OK, message="OK")
stat.values = [
KeyValue("Linear speed (Vx)", str(self.robot.get_movesteer(None)[0])),
KeyValue("Angular speed (Vth)", str(self.robot.get_movesteer(None)[2]))]
if self.robot.get_movesteer(None)[0] > self.speedLimit:
stat.level = DiagnosticStatus.WARN
stat.message = "speed is too high"
return stat
def _publish_diag(self):
ok = False
with self._mutex:
ok = self._ok
msg = DiagnosticArray()
stat = DiagnosticStatus()
msg.status.append(stat)
# Simulate pr2_etherCAT diagnostics
stat.level = 0
if not ok:
stat.level = 2
stat.name = 'EtherCAT Master'
stat.message = 'OK'
stat.values.append(KeyValue('Dropped Packets', '0'))
stat.values.append(KeyValue('RX Late Packet', '0'))
# Check for encoder errors
mcb_stat = DiagnosticStatus()
mcb_stat.name = 'EtherCAT Device (my_motor)'
mcb_stat.level = 0
mcb_stat.values.append(KeyValue('Num encoder_errors', '0'))
# Test camera listener
stat_cam = DiagnosticStatus()
stat_cam.level = 0
stat_cam.name = 'wge100: Frequency Status'
stat_cam.message = 'OK'
# Test HK listener
stat_hk = DiagnosticStatus()
stat_hk.level = 0
stat_hk.name = 'tilt_hokuyo_node: Frequency Status'
stat_hk.message = 'OK'
msg.status.append(stat_cam)
msg.status.append(mcb_stat)
msg.status.append(stat_hk)
msg.header.stamp = rospy.get_rostime()
self.diag_pub.publish(msg)
self._last_diag_pub = rospy.get_time()
def cb_diagnostics_update(self):
tabpage_status = DiagnosticStatus()
tabpage_status.level = tabpage_status.OK
tabpage_status.name = self.handler_name + " Handler"
tabpage_status.hardware_id = self.ros_name
tabpage_status.message = "OK"
tabpage_status.values = []
tabpage_status.values.append(KeyValue(key="Number of Clients",
value=str(len(self.osc_clients))))
return tabpage_status
def publish_diagnostics(self, event):
msg = DiagnosticArray()
msg.header.stamp = rospy.get_rostime()
status = DiagnosticStatus()
status.name = rospy.get_name()
status.level = DiagnosticStatus.OK
status.message = "fake diagnostics"
status.hardware_id = rospy.get_name()
msg.status.append(status)
self.pub_diagnostics.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'
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 ntp_monitor(ntp_hostname,
offset=500,
self_offset=500,
diag_hostname=None,
error_offset=5000000,
do_self_test=True):
pub = rospy.Publisher("/diagnostics", DiagnosticArray)
rospy.init_node(NAME, anonymous=True)
hostname = socket.gethostname()
if diag_hostname is None:
diag_hostname = hostname
stat = DiagnosticStatus()
stat.level = DiagnosticStatus.OK
stat.name = "NTP offset from " + diag_hostname + " to " + ntp_hostname
stat.message = "OK"
stat.hardware_id = hostname
stat.values = []
self_stat = DiagnosticStatus()
self_stat.level = DiagnosticStatus.OK
self_stat.name = "NTP self-offset for " + diag_hostname
self_stat.message = "OK"
self_stat.hardware_id = hostname
self_stat.values = []
while not rospy.is_shutdown():
msg = DiagnosticArray()
msg.header.stamp = rospy.get_rostime()
st = ntp_diag(stat, ntp_hostname, offset, error_offset)
if st is not None:
msg.status.append(st)
if do_self_test:
st = ntp_diag(self_stat, hostname, self_offset, error_offset)
if st is not None:
msg.status.append(st)
pub.publish(msg)
time.sleep(1)
def _camera_diag(level = 0):
array = DiagnosticArray()
stat = DiagnosticStatus()
stat.name = 'wge100: Driver Status'
stat.level = level
stat.message = 'OK'
array.header.stamp = rospy.get_rostime()
array.status.append(stat)
return array
def _laptop_charge_to_diag(laptop_msg):
rv = DiagnosticStatus()
rv.level = DiagnosticStatus.OK
rv.message = 'OK'
rv.name = rospy.get_name()
if rv.name.startswith('/'):
rv.name = rv.name[1:]
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 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 pub_direction_diagnostic(function_name):
diag_msg = DiagnosticArray()
diag_msg.header.stamp = rospy.get_rostime()
comp_msg = DiagnosticStatus()
comp_msg.level = DiagnosticStatus.ERROR
comp_msg.name = function_name
comp_msg.message = "Movement status"
diag_msg.status.append(comp_msg)
diagnostics_pub.publish(diag_msg)
def pub_component_diagnostic(component_name):
diag_msg = DiagnosticArray()
diag_msg.header.stamp = rospy.get_rostime()
comp_msg = DiagnosticStatus()
comp_msg.level = DiagnosticStatus.ERROR
comp_msg.name = component_name
comp_msg.message = "Component status"
diag_msg.status.append(comp_msg)
diagnostics_pub.publish(diag_msg)
def create_node_status(self):
diag_status = DiagnosticStatus()
diag_status.name = "BHG: Node Status"
diag_status.message = "BHG: Normal"
diag_status.hardware_id = socket.gethostname()
for r_node in node_list:
# self.node_state[r_node] = rosnode.rosnode_ping(r_node, 1, False)
node_case_kv = KeyValue()
node_case_kv.key = r_node
node_case_kv.value = str(rosnode.rosnode_ping(r_node, 1, False))
diag_status.values.append(node_case_kv)
return diag_status
def publish_diagnostic(self, level, message):
""" Auxiliary method to publish Diagnostic messages """
diag_msg = DiagnosticArray()
diag_msg.header.frame_id = "imu"
diag_msg.header.stamp = rospy.Time.now()
imu_msg = DiagnosticStatus()
imu_msg.name = "IMU"
imu_msg.hardware_id = "OpenLog Artemis IMU"
imu_msg.level = level
imu_msg.message = message
diag_msg.status.append(imu_msg)
self.diag_pub.publish(diag_msg)
def test_DiagnosticStatus():
'''
PUBLISHER METHODE: DiagnosticStatus
'''
pub_DiagnosticStatus = rospy.Publisher('diagnostic', DiagnosticStatus, queue_size=10)
message = DiagnosticStatus()
message.name = "ROB"
message.message = "RUNNING"
print "DiagnosticStatus: " + message.name + " , " + message.message
pub_DiagnosticStatus.publish(message)
def get_status(self):
stat = 0
if not self._ok:
stat = 2
if rospy.get_time() - self._update_time > 3:
stat = 3
diag = DiagnosticStatus()
diag.level = stat
diag.name = 'Caster Slip Listener'
diag.message = 'OK'
if diag.level == 2:
diag.message = 'Caster Slipping'
if diag.level == 3:
diag.message = 'Caster Stale'
diag.level = 2
diag.values.append(KeyValue("Turret", str(TURRET_NAME)))
diag.values.append(KeyValue("R Wheel", str(R_WHEEL_NAME)))
diag.values.append(KeyValue("L Wheel", str(L_WHEEL_NAME)))
diag.values.append(KeyValue("Turret Position", str(self.last_position.turret)))
diag.values.append(KeyValue("R Wheel Position", str(self.last_position.r_wheel)))
diag.values.append(KeyValue("L Wheel Position", str(self.last_position.l_wheel)))
diag.values.append(KeyValue("Max Pos. Left Slip", str(self._max_l_err_pos)))
diag.values.append(KeyValue("Max Neg. Left Slip", str(self._max_l_err_neg)))
diag.values.append(KeyValue("Max Pos. Right Slip", str(self._max_r_err_pos)))
diag.values.append(KeyValue("Max Neg. Right Slip", str(self._max_r_err_neg)))
diag.values.append(KeyValue("Max Pos. Left Slip (Reset)", str(self._max_l_err_pos_reset)))
diag.values.append(KeyValue("Max Neg. Left Slip (Reset)", str(self._max_l_err_neg_reset)))
diag.values.append(KeyValue("Max Pos. Right Slip (Reset)", str(self._max_r_err_pos_reset)))
diag.values.append(KeyValue("Max Neg. Right Slip (Reset)", str(self._max_r_err_neg_reset)))
diag.values.append(KeyValue("Wheel Offset", str(WHEEL_OFFSET)))
diag.values.append(KeyValue("Wheel Diameter", str(WHEEL_RADIUS)))
diag.values.append(KeyValue("Allowed Slip", str(ALLOWED_SLIP)))
diag.values.append(KeyValue("Update Interval", str(UPDATE_INTERVAL)))
diag.values.append(KeyValue("Total Errors", str(self._num_errors)))
diag.values.append(KeyValue("Errors Since Reset", str(self._num_errors_since_reset)))
return diag
def sensor_status_to_diag(sensorList, hostname='localhost', ignore_fans=False):
stat = DiagnosticStatus()
stat.name = '%s Sensor Status'%hostname
stat.message = 'OK'
stat.level = DiagnosticStatus.OK
stat.hardware_id = hostname
for sensor in sensorList:
if sensor.getType() == "Temperature":
if sensor.getInput() > sensor.getCrit():
stat.level = max(stat.level, DiagnosticStatus.ERROR)
stat.message = "Critical Temperature"
elif sensor.getInput() > sensor.getHigh():
stat.level = max(stat.level, DiagnosticStatus.WARN)
stat.message = "High Temperature"
stat.values.append(KeyValue(key=" ".join([sensor.getName(), sensor.getType()]),
value=str(sensor.getInput())))
elif sensor.getType() == "Voltage":
if sensor.getInput() < sensor.getMin():
stat.level = max(stat.level, DiagnosticStatus.ERROR)
stat.message = "Low Voltage"
elif sensor.getInput() > sensor.getMax():
stat.level = max(stat.level, DiagnosticStatus.ERROR)
stat.message = "High Voltage"
stat.values.append(KeyValue(key=" ".join([sensor.getName(), sensor.getType()]),
value=str(sensor.getInput())))
elif sensor.getType() == "Speed":
if not ignore_fans:
if sensor.getInput() < sensor.getMin():
stat.level = max(stat.level, DiagnosticStatus.ERROR)
stat.message = "No Fan Speed"
stat.values.append(KeyValue(key=" ".join([sensor.getName(), sensor.getType()]),
value=str(sensor.getInput())))
return stat
def 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'
target_index = self.joint_names.index(
controller.joint_name)
self.current_joint_state[
target_index] = joint_state.current_pos
diag_msg.status.append(
status) # print(status.name, joint_state.current_pos)
except:
pass
self.diagnostics_pub.publish(diag_msg)
message = JointState()
message.name = self.raw_joint_names
message.header.stamp = rospy.Time.now()
message.position = self.map_to_simulated_frames(
self.current_joint_state)
self.joint_states_pub.publish(message)
rate.sleep()
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 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 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_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 calculate_attr(self, msgs):
status_msg = DiagnosticStatus()
attr = msgs[0].data + msgs[1].data
print("{0} + {1}".format(msgs[0].data, msgs[1].data))
status_msg = DiagnosticStatus()
status_msg.level = DiagnosticStatus.OK
status_msg.name = self._id
status_msg.values.append(KeyValue("enery", str(attr)))
status_msg.message = "QA status"
return status_msg
def load(self, argv):
'''
:param argv: a list with argv parameter needed to load the launch file.
The name and value are separated by `:=`
:type argv: list(str)
:return: True, if the launch file was loaded and argv, used while launch
:rtype: tuple(bool, [])
:raise LaunchConfigException: on load errors
'''
try:
self._capabilities = None
self._robot_description = None
roscfg = roslaunch.ROSLaunchConfig()
loader = roslaunch.XmlLoader()
self.argv = self.resolve_args(argv)
loader.ignore_unset_args = False
loader.load(self.filename, roscfg, verbose=False, argv=self.argv)
self.__roscfg = roscfg
if 'arg' in loader.root_context.resolve_dict:
self.resolve_dict = loader.root_context.resolve_dict['arg']
self.changed = True
# check for depricated parameter
diag_dep = DiagnosticArray()
if self._monitor_servicer is not None:
diag_dep.header.stamp = rospy.Time.now()
for n in roscfg.nodes:
node_fullname = roslib.names.ns_join(n.namespace, n.name)
associations_param = roslib.names.ns_join(
node_fullname, 'associations')
if associations_param in roscfg.params:
ds = DiagnosticStatus()
ds.level = DiagnosticStatus.WARN
ds.name = node_fullname
ds.message = 'Deprecated parameter detected'
ds.values.append(KeyValue('deprecated', 'associations'))
ds.values.append(KeyValue('new', 'nm/associations'))
rospy.logwarn(
"'associations' is deprecated, use 'nm/associations'! found for node: %s in %s"
% (node_fullname, self.filename))
diag_dep.status.append(ds)
if self._monitor_servicer is not None:
# set diagnostics
self._monitor_servicer._monitor._callback_diagnostics(diag_dep)
except roslaunch.XmlParseException as e:
test = list(
re.finditer(r"environment variable '\w+' is not set", utf8(e)))
message = utf8(e)
if test:
message = '%s\nenvironment substitution is not supported, use "arg" instead!' % message
raise LaunchConfigException(message)
return True, self.argv
def check_ok(self):
with self._mutex:
stat = 0 if self._ok else 2
msg = '' if self._ok else 'Dropped Packets'
if rospy.get_time() - self._update_time > 3:
stat = 3
msg = 'Packet Data Stale'
if self._update_time == -1:
msg = 'No Packet Data'
diag = DiagnosticStatus()
diag.name = 'EC Stats Packet Data'
diag.level = stat
diag.message = msg
if diag.level == 0:
diag.message = 'OK'
diag.values = [
KeyValue(key='Has Link?', value=str(self._has_link)),
KeyValue(key='Dropped Since Reset', value=str(self._total_dropped - self._drop_count_at_reset)),
KeyValue(key='Total Drops', value=str(self._total_dropped)),
KeyValue(key='Lost Link Count', value=str(self._lost_link_count)),
KeyValue(key='Lost Links Since Reset', value=str(self._lost_link_count_since_reset)),
KeyValue(key='Number of Resets', value=str(self._reset_count)),
KeyValue(key='Time Since Last Reset', value=str(rospy.get_time() - self._time_at_last_reset)),
KeyValue(key='Drops at Last Reset', value=str(self._drop_count_at_reset)),
KeyValue(key='Max Device Count', value=str(self._max_device_count)),
KeyValue(key='Interval Drops', value=str(self._interval_dropped)),
KeyValue(key='Total Late Packets', value=str(self._total_late)),
KeyValue(key='Interval Late Packets', value=str(self._interval_late)),
KeyValue(key='Total Sent Packets', value=str(self._total_sent)),
KeyValue(key='Interval Sent Packets', value=str(self._interval_sent)),
KeyValue(key='Total Bandwidth', value=str(self._total_bandwidth)),
KeyValue(key='Interval Bandwidth', value=str(self._interval_bandwidth))
]
return stat, msg, [ diag ]
def publish_stats(self, event):
msg = DiagnosticArray()
msg.header.stamp = rospy.get_rostime()
# Add all fake
hostname_list = self._options.diag_hostnames.split(", ")
for hostname in hostname_list:
status = DiagnosticStatus()
status.name = hostname
status.level = DiagnosticStatus.OK
status.message = "fake diagnostics"
status.hardware_id = hostname
msg.status.append(status)
self._fake_diag_pub.publish(msg)
def pub_heartbeat():
#print(err_level,message)
global heartbeat_pub
msg = DiagnosticArray()
msg.header.stamp = rospy.get_rostime()
usage_stat = DiagnosticStatus()
usage_stat.name = 'isapi_ptz_node'
usage_stat.level = err_level
usage_stat.hardware_id = ''
usage_stat.message = err_message
# usage_stat.values = [ KeyValue(key = 'Update Status', value = 'No Data' ),
# KeyValue(key = 'Time Since Last Update', value = 'N/A') ]
msg.status.append(usage_stat)
heartbeat_pub.publish(msg)
def pub_status(self):
gpu_stat = GPUStatus()
stat = DiagnosticStatus()
try:
card_out = computer_monitor.get_gpu_status(xml=True)
gpu_stat = computer_monitor.parse_smi_output(card_out)
stat = computer_monitor.gpu_status_to_diag(gpu_stat, self.hostname)
except Exception, e:
import traceback
rospy.logerr('Unable to process nVidia GPU data')
rospy.logerr(traceback.format_exc())
stat.name = '%s GPU Status' % self.hostname
stat.message = 'Could not get GPU information'
stat.level = DiagnosticStatus.ERROR
def test_DiagnosticStatus():
'''
PUBLISHER METHODE: DiagnosticStatus
'''
pub_DiagnosticStatus = rospy.Publisher('diagnostic',
DiagnosticStatus,
queue_size=10)
message = DiagnosticStatus()
message.name = "ROB"
message.message = "RUNNING"
print "DiagnosticStatus: " + message.name + " , " + message.message
pub_DiagnosticStatus.publish(message)
def controller_to_diag(c):
d = DiagnosticStatus()
d.name = 'Controller (' + c.name + ')'
d.level = 0
if (c.running):
d.message = 'Running'
else:
d.message = 'Stopped'
if (not use_sim_time and c.num_control_loop_overruns > 0):
d.message += ' !!! Broke Realtime, used ' + \
str(int(c.max_time.to_sec() * 1e6)) + \
' micro seconds in update loop'
if c.time_last_control_loop_overrun + rospy.Duration(
30.0) > rospy.Time.now():
d.level = 1
d.values.append(
KeyValue('Avg Time in Update Loop (usec)',
str(int(c.mean_time.to_sec() * 1e6))))
d.values.append(
KeyValue('Max Time in update Loop (usec)',
str(int(c.max_time.to_sec() * 1e6))))
d.values.append(
KeyValue('Variance on Time in Update Loop',
str(int(c.variance_time.to_sec() * 1e6))))
d.values.append(
KeyValue('Percent of Cycle Time Used',
str(int(c.mean_time.to_sec() / 0.00001))))
d.values.append(
KeyValue('Number of Control Loop Overruns',
str(int(c.num_control_loop_overruns))))
d.values.append(
KeyValue('Timestamp of Last Control Loop Overrun (sec)',
str(int(c.time_last_control_loop_overrun.to_sec()))))
return d
def diagnostics(self, state):
try:
da = DiagnosticArray()
ds = DiagnosticStatus()
ds.name = rospy.get_caller_id().lstrip('/') + ": Node State"
if state == 0:
ds.level = DiagnosticStatus.OK
ds.message = "%i sounds playing"%self.active_sounds
ds.values.append(KeyValue("Active sounds", str(self.active_sounds)))
ds.values.append(KeyValue("Allocated sound channels", str(self.num_channels)))
ds.values.append(KeyValue("Buffered builtin sounds", str(len(self.builtinsounds))))
ds.values.append(KeyValue("Buffered wave sounds", str(len(self.filesounds))))
ds.values.append(KeyValue("Buffered voice sounds", str(len(self.voicesounds))))
elif state == 1:
ds.level = DiagnosticStatus.WARN
ds.message = "Sound device not open yet."
else:
ds.level = DiagnosticStatus.ERROR
ds.message = "Can't open sound device. See http://wiki.ros.org/sound_play/Troubleshooting"
da.status.append(ds)
da.header.stamp = rospy.get_rostime()
self.diagnostic_pub.publish(da)
except Exception, e:
rospy.loginfo('Exception in diagnostics: %s'%str(e))
def _populate_status(self, cmd_out, name):
"""Populate status helper function
"""
status = DiagnosticStatus()
status.hardware_id = "wifi"
status.name = name
status.level = status.OK
status.message = pformat(cmd_out)
for k,v in cmd_out.items():
status.values.append(
KeyValue(k,str(v)),
)
return status
def diagnostics(level, msg_short, msg_long):
if level == 0:
rospy.loginfo(msg_long)
elif level == 1:
rospy.logwarn(msg_long)
elif level == 2:
rospy.logerr(msg_long)
d = DiagnosticArray()
d.header.stamp = rospy.Time.now()
ds = DiagnosticStatus()
ds.level = level
ds.message = msg_long
ds.name = msg_short
d.status = [ds]
pub_diag.publish(d)
def pub_diagnostics(self, msg):
# Pull out the percent charge from the message
percent_charge = msg.percentage
# Initialize the diagnostics array
diag_arr = DiagnosticArray()
# Time stamp the message with the incoming stamp
diag_arr.header.stamp = msg.header.stamp
# Initialize the status message
diag_msg = DiagnosticStatus()
# Make the name field descriptive of what we are measuring
diag_msg.name = "Laptop Charge"
# Add a key-value pair so we can drill down to the percent charge
diag_msg.values.append(KeyValue('percent_charge', str(percent_charge)))
# Set the diagnostics level based on the current charge and the threshold
# parameters
if percent_charge < self.error_percent:
diag_msg.level = DiagnosticStatus.ERROR
diag_msg.message = 'Battery needs recharging'
elif percent_charge < self.warn_percent:
diag_msg.level = DiagnosticStatus.WARN
diag_msg.message = 'Battery is below 50%'
else:
diag_msg.level = DiagnosticStatus.OK
diag_msg.message = 'Battery charge is OK'
# Append the status message to the diagnostic array
diag_arr.status.append(diag_msg)
# Publish the array
self.diag_pub.publish(diag_arr)
def diagnostics(self, state):
try:
da = DiagnosticArray()
ds = DiagnosticStatus()
ds.name = rospy.get_caller_id().lstrip('/') + ": Node State"
if state == 0:
ds.level = DiagnosticStatus.OK
ds.message = "%i sounds playing"%self.active_sounds
ds.values.append(KeyValue("Active sounds", str(self.active_sounds)))
ds.values.append(KeyValue("Allocated sound channels", str(self.num_channels)))
ds.values.append(KeyValue("Buffered builtin sounds", str(len(self.builtinsounds))))
ds.values.append(KeyValue("Buffered wave sounds", str(len(self.filesounds))))
ds.values.append(KeyValue("Buffered voice sounds", str(len(self.voicesounds))))
elif state == 1:
ds.level = DiagnosticStatus.WARN
ds.message = "Sound device not open yet."
else:
ds.level = DiagnosticStatus.ERROR
ds.message = "Can't open sound device. See http://pr.willowgarage.com/wiki/sound_play/Troubleshooting"
da.status.append(ds)
da.header.stamp = rospy.get_rostime()
self.diagnostic_pub.publish(da)
except Exception, e:
rospy.loginfo('Exception in diagnostics: %s'%str(e))
def publish_diag(self, level, choice):
msg = DiagnosticArray()
stat = DiagnosticStatus()
msg.status.append(stat)
stat.level = level
stat.name = 'EtherCAT Master'
stat.message = choice
stat.values.append(KeyValue(key='Dropped Packets', value='0'))
stat.values.append(KeyValue(key='RX Late Packet', value='0'))
mcb_stat = DiagnosticStatus()
mcb_stat.level = 0
mcb_stat.name = 'EtherCAT Device (cont_motor)'
mcb_stat.message = 'OK'
mcb_stat.values.append(KeyValue(key='Num encoder_errors', value='0'))
msg.status.append(mcb_stat)
self.diag_pub.publish(msg)
halted = Bool()
halted.data = level != 0
self.motors_pub.publish(halted)
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 publish(self, state):
STATE_INDEX_CHARGING = 0
STATE_INDEX_BATTERY = 1
STATE_INDEX_CONNECTION = 2
# timed gate: limit to 1 Hz
curr_time = rospy.get_rostime()
if (curr_time - self.last_diagnostics_time).to_sec() < 1.0:
return
self.last_diagnostics_time = curr_time
# compose diagnostics message
diag = DiagnosticArray()
diag.header.stamp = curr_time
# battery info
stat = DiagnosticStatus(name="Battery", level=DiagnosticStatus.OK, message="OK")
try:
battery_state_code = state[STATE_INDEX_BATTERY]
stat.message = self.STATE_TEXTS_BATTERY[battery_state_code]
if battery_state_code < 3:
stat.level = DiagnosticStatus.WARN
if battery_state_code < 1:
stat.level = DiagnosticStatus.ERROR
stat.message = "Please Recharge Battery (%s)." % self.STATE_TEXTS_BATTERY[battery_state_code]
except KeyError as ex:
stat.message = "Invalid Battery State %s" % ex
rospy.logwarn("Invalid Battery State %s" % ex)
stat.level = DiagnosticStatus.ERROR
diag.status.append(stat)
# connection info
stat = DiagnosticStatus(name='ps3joy'": Connection Type", level=DiagnosticStatus.OK, message="OK")
try:
stat.message = self.STATE_TEXTS_CONNECTION[state[STATE_INDEX_CONNECTION]]
except KeyError as ex:
stat.message = "Invalid Connection State %s" % ex
rospy.logwarn("Invalid Connection State %s" % ex)
stat.level = DiagnosticStatus.ERROR
diag.status.append(stat)
# charging info
stat = DiagnosticStatus(name='ps3joy'": Charging State", level=DiagnosticStatus.OK, message="OK")
try:
stat.message = self.STATE_TEXTS_CHARGING[state[STATE_INDEX_CHARGING]]
except KeyError as ex:
stat.message = "Invalid Charging State %s" % ex
rospy.logwarn("Invalid Charging State %s" % ex)
stat.level = DiagnosticStatus.ERROR
diag.status.append(stat)
# publish message
self.diag_pub.publish(diag)
def publish(self, msg):
"""Publish a single diagnostic status or a vector of diagnostic statuses."""
if not type(msg) is list:
msg = [msg]
for stat in msg:
stat.name = self.node.get_name() + ': ' + stat.name
now = self.clock.now()
da = DiagnosticArray()
db = DiagnosticStatus()
db.name = stat.name
db.message = stat.message
db.hardware_id = stat.hardware_id
db.values = stat.values
da.status.append(db)
da.header.stamp = now.to_msg() # Add timestamp for ROS 0.10
self.publisher.publish(da)
def wifi_to_diag(msg):
stat = DiagnosticStatus()
stat.name = DIAG_NAME
stat.level = DiagnosticStatus.OK
stat.message = 'OK'
stat.values.append(KeyValue(key='ESSID', value=msg.essid))
stat.values.append(KeyValue(key='Mac Address', value=msg.macaddr))
stat.values.append(KeyValue(key='Signal', value=str(msg.signal)))
stat.values.append(KeyValue(key='Noise', value=str(msg.noise)))
stat.values.append(KeyValue(key='Sig/Noise', value=str(msg.snr)))
stat.values.append(KeyValue(key='Channel', value=str(msg.channel)))
stat.values.append(KeyValue(key='Rate', value=msg.rate))
stat.values.append(KeyValue(key='TX Power', value=msg.tx_power))
stat.values.append(KeyValue(key='Quality', value=str(msg.quality)))
return stat
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)
