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