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 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 _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 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 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 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 state_cb(msg):
global last_publish_time
now = rospy.get_rostime()
if (now - last_publish_time).to_sec() > 1.0:
if len(msg.controller_statistics) == 0:
d = DiagnosticArray()
d.header.stamp = msg.header.stamp
ds = DiagnosticStatus()
ds.name = "Controller: No controllers loaded in controller manager"
d.status = [ds]
pub_diag.publish(d)
else:
for c in msg.controller_statistics:
d = DiagnosticArray()
d.header.stamp = msg.header.stamp
d.status = [controller_to_diag(c)]
pub_diag.publish(d)
last_publish_time = now
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 publish_diags():
with mutex:
global last_msg, last_update_time
if last_msg is None:
return
# Don't publish anything without updates
if rospy.get_time() - last_update_time > 3:
return
d = DiagnosticArray()
d.header.stamp = last_msg.header.stamp
if last_msg.joint_statistics == []:
ds = DiagnosticStatus()
ds.level = 0
ds.message = 'No Joint states from controller manager'
ds.name = "Joints: none"
d.status = [ds]
else:
d.status = [joint_to_diag(js) for js in last_msg.joint_statistics]
pub_diag.publish(d)
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 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(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_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, 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 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 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 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 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 update(self):
msg = DiagnosticArray()
msg.header.stamp = rospy.Time.now()
msg.status = []
it = 0
while it < 20:
status = DiagnosticStatus()
status.level = random.randint(0,2)
status.name = "Test %i"%it
status.hardware_id = "Dummy Diagnostics"
if status.level == 0:
message = "OK"
elif status.level == 1:
message = "WARN"
elif status.level == 2:
message = "ERROR"
status.message = message
status.values = []
ii = 0
while ii < 20:
status.values.append(KeyValue("Key %i"%ii,str(random.randint(0,50))))
ii += 1
it += 1
msg.status.append(status)
self.publisher.publish(msg)
msg = DiagnosticArray()
msg.header.stamp = rospy.Time.now()
msg.status = []
status = DiagnosticStatus()
status.level = status.WARN
status.name = "Test Warn"
status.hardware_id = "Dummy Diagnostics"
status.message = "Warning - This is a test"
status.values = []
msg.status.append(status)
self.publisher.publish(msg)
def _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 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 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 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():
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 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 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 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 wrapper(jetson):
# Initialization ros pubblisher
pub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=1)
# Set default rate jetson stats 2hz
rate = rospy.Rate(2)
# Extract board information
board = jetson.board
# Define hardware name
hardware = board["board"]["Name"]
# 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'])]
if 'SWAP' in stats:
arr.status += [swap_status(hardware, stats['SWAP'])]
if 'FAN' in stats:
arr.status += [fan_status(hardware, stats['FAN'])]
if 'VOLT' in stats:
arr.status += [voltage_status(hardware, stats['VOLT'])]
if 'TEMP' in stats:
arr.status += [temp_status(hardware, stats['TEMP'])]
if 'EMC' in stats:
arr.status += [emc_status(hardware, stats['EMC'])]
# Status board and board info
arr.status += [board_status(hardware, board)]
# Add disk status
arr.status += [disk_status(hardware, jetson.disk)]
# Update status jtop
rospy.logdebug("jtop message %s" % rospy.get_time())
pub.publish(arr)
rate.sleep()
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 _build_diag_msg(self) -> DiagnosticArray:
msg = DiagnosticArray()
msg.header.stamp = rospy.Time.now()
msg.status = [
DiagnosticStatus(
name="contract",
hardware_id=str(uid),
message="Report contract",
values=[
KeyValue(key="format", value="yaml"),
KeyValue(
key="material",
# Default color is green
value=self._AGENT_COLOR_MAP.get(
str(uid), "Gazebo/GreenTransparentOverlay")),
KeyValue(key="data", value=repr(contr))
]) for uid, contr in self._contr_dict.items()
]
return msg
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 _diag_cb(self, event):
# rospy callbacks are not thread safe. This prevents serial interference
self._diagnostics_update()
darr = DiagnosticArray()
darr.header.stamp = rospy.get_rostime()
darr.status = [
DiagnosticStatus(
name="Roboclaw Driver",
message="OK",
hardware_id="none",
values=[
KeyValue(key='Firmware Version',
value=str(self._firmware_version)),
KeyValue(
key='Left Motor Max Current',
value='{MAX_CURRENT} A'.format(
MAX_CURRENT=str(self._left_motor_max_current))),
KeyValue(key='Left Motor Current',
value='{CURRENT} A'.format(
CURRENT=str(self._left_motor_current))),
# KeyValue(key='Left Motor Running Speed', value='{SPEED} '.format(SPEED=str(self._left_motor_speed))),
KeyValue(
key='Right Motor Max Current',
value='{MAX_CURRENT} A'.format(
MAX_CURRENT=str(self._right_motor_max_current))),
KeyValue(key='Right Motor Current',
value='{CURRENT} A'.format(
CURRENT=str(self._right_motor_current))),
# KeyValue(key='Right Motor Running Speed', value='{SPEED} '.format(SPEED=str(self._right_motor_speed))),
KeyValue(key='Battery Voltage',
value='{VOLTAGE}V'.format(
VOLTAGE=str(self._battery_voltage))),
KeyValue(key='Drive Mode',
value='Closed Loop Control'
if self._esc_feedback_controls_enabled else
'Open Loop Control')
])
]
self._pub_diag.publish(darr)
def publish_status(self):
self._mutex.acquire()
self.check_diags()
status = TestStatus()
if rospy.get_time() - self._ethercat_update_time > 3:
status.test_ok = TestStatus.TEST_STALE
status.message = 'EtherCAT Master Stale'
elif rospy.get_time() - self._mech_update_time > 1:
status.message = 'Mechanism state stale'
status.test_ok = TestStatus.TEST_STALE
elif self._ethercat_ok and self._transmissions_ok:
status.test_ok = TestStatus.TEST_RUNNING
status.message = 'OK'
else:
status.test_ok = TestStatus.TEST_ERROR
if not self._transmissions_ok:
status.message = 'Transmission Broken'
else:
status.message = 'EtherCAT Halted'
self.test_status_pub.publish(status)
array = DiagnosticArray()
array.status = self._trans_status
self.diag_pub.publish(array)
# DISABLE FOR TESTING
# Halt the test if we have a bad transmission
#if self._ethercat_ok and not self._transmissions_ok:
# self.halt_motors()
if not rospy.is_shutdown():
timer = threading.Timer(0.5, self.publish_status)
timer.start()
self._mutex.release()
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 publish():
leakPub = rospy.Publisher('Leak', Bool, queue_size=1)
diagPub = rospy.Publisher('/diagnostic', DiagnosticArray, queue_size=1)
rospy.init_node('Leak')
#GPIO.setmode(GPIO.BCM)
#GPIO.setup(CHANNEL, GPIO.IN)
GPIO.setup(CHANNEL, gpio.IN)
freq = rospy.Rate(1)
leak = Bool()
pub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=1)
diag = DiagnosticArray()
while not rospy.is_shutdown():
levelData = 0
if (leak.data):
levelData = 2
leak.data = GPIO.input(CHANNEL)
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 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)
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)
sleep(1)
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()
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
array.status = statuses[ind]
pub.publish(array)
sleep(1)
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()
print rospy.get_rostime()
pub.publish(array)
sleep(1)
count = count + 1
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)
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()
if rospy.get_time() - start_time < 5:
# Shouldn't disappear
array.status.append(DiagnosticStatus(0, 'expected4', 'I will be stale', '', []))
array.status.append(DiagnosticStatus(0, 'expected5', 'I will be stale', '', []))
# Should disappear
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', '', []),
DiagnosticStatus(1, 'Joint (fl_caster_rotation_joint)', 'Uncalibrated', '', []),
DiagnosticStatus(1, 'Joint (fr_caster_l_wheel_joint)', 'Uncalibrated', '', []),
DiagnosticStatus(0, 'Joint (fr_caster_r_wheel_joint)', 'OK', '', []),
DiagnosticStatus(0, 'Joint (fr_caster_rotation_joint)', 'OK', '', [])]
array.header.stamp = rospy.get_rostime()
# Test that runtime monitor can handle an item changing levels
if rospy.get_time() - start_time > 5:
array.status.append(DiagnosticStatus(2, 'EtherCAT Device (fr_caster_l_wheel_motor)', 'Motor model', '', []))
else:
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)
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')
#else:
while not rospy.is_shutdown():
status.values = []
status.values.append(KeyValue(key='chrony version', value=chronyVersion) )
def __publish_diagnostic_information(self):
diag_msg = DiagnosticArray()
rate = rospy.Rate(self.diagnostics_rate)
while not rospy.is_shutdown() and self.running:
diag_msg.status = []
diag_msg.header.stamp = rospy.Time.now()
status = DiagnosticStatus()
status.name = 'Dynamixel Serial Bus (%s)' % self.port_namespace
status.hardware_id = 'Dynamixel Serial Bus on port %s' % self.port_name
status.values.append(KeyValue('Baud Rate', str(self.baud_rate)))
status.values.append(KeyValue('Min Motor ID', str(self.min_motor_id)))
status.values.append(KeyValue('Max Motor ID', str(self.max_motor_id)))
status.values.append(KeyValue('Desired Update Rate', str(self.update_rate)))
status.values.append(KeyValue('Actual Update Rate', str(self.actual_rate)))
status.values.append(KeyValue('# Non Fatal Errors', str(self.error_counts['non_fatal'])))
status.values.append(KeyValue('# Checksum Errors', str(self.error_counts['checksum'])))
status.values.append(KeyValue('# Dropped Packet Errors', str(self.error_counts['dropped'])))
status.level = DiagnosticStatus.OK
status.message = 'OK'
if self.actual_rate - self.update_rate < -5:
status.level = DiagnosticStatus.WARN
status.message = 'Actual update rate is lower than desired'
diag_msg.status.append(status)
for motor_state in self.current_state.motor_states:
mid = motor_state.id
status = DiagnosticStatus()
status.name = 'Robotis Dynamixel Motor %d on port %s' % (mid, self.port_namespace)
status.hardware_id = 'DXL-%d@%s' % (motor_state.id, self.port_namespace)
status.values.append(KeyValue('Model Name', str(self.motor_static_info[mid]['model'])))
status.values.append(KeyValue('Firmware Version', str(self.motor_static_info[mid]['firmware'])))
status.values.append(KeyValue('Return Delay Time', str(self.motor_static_info[mid]['delay'])))
status.values.append(KeyValue('Minimum Voltage', str(self.motor_static_info[mid]['min_voltage'])))
status.values.append(KeyValue('Maximum Voltage', str(self.motor_static_info[mid]['max_voltage'])))
status.values.append(KeyValue('Minimum Position (CW)', str(self.motor_static_info[mid]['min_angle'])))
status.values.append(KeyValue('Maximum Position (CCW)', str(self.motor_static_info[mid]['max_angle'])))
status.values.append(KeyValue('Goal', str(motor_state.goal)))
status.values.append(KeyValue('Position', str(motor_state.position)))
status.values.append(KeyValue('Error', str(motor_state.error)))
status.values.append(KeyValue('Velocity', str(motor_state.speed)))
status.values.append(KeyValue('Load', str(motor_state.load)))
status.values.append(KeyValue('Voltage', str(motor_state.voltage)))
status.values.append(KeyValue('Temperature', str(motor_state.temperature)))
status.values.append(KeyValue('Moving', str(motor_state.moving)))
if motor_state.temperature >= self.error_level_temp:
status.level = DiagnosticStatus.ERROR
status.message = 'OVERHEATING'
elif motor_state.temperature >= self.warn_level_temp:
status.level = DiagnosticStatus.WARN
status.message = 'VERY HOT'
else:
status.level = DiagnosticStatus.OK
status.message = 'OK'
diag_msg.status.append(status)
self.diagnostics_pub.publish(diag_msg)
rate.sleep()
#!/usr/bin/env python
import 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()