def run(self):
nodes = rosnode.get_node_names()
status_list = []
for driver in self.drivers:
status = DiagnosticStatus()
status.name = "%s Module Driver" % driver.split('/')[1]
status.hardware_id = driver.split('/')[1]
if driver in nodes:
status.level = DiagnosticStatus.OK
status.message = 'OK'
status.values.insert(0,
KeyValue(key='Driver Status', value='Up'))
self.last_updates[
status.hardware_id] = rospy.Time.now().to_sec()
else:
status.level = DiagnosticStatus.ERROR
status.message = 'Module node not running'
status.values.insert(
0, KeyValue(key='Driver Status', value='Down'))
self.last_updates[
status.hardware_id] = rospy.Time.now().to_sec()
# Check if message is stale (older than 35 seconds)
try:
elapsed = rospy.Time.now().to_sec() - self.last_updates[
status.hardware_id]
except KeyError:
elapsed = 0
if elapsed > 35:
status.level = DiagnosticStatus.STALE
status.message = 'Stale'
status.values.insert(
0, KeyValue(key='Update Status', value='Stale'))
status.values.insert(
1, KeyValue(key='Time Since Update', value=str(elapsed)))
status_list.append(status)
diag_msg = DiagnosticArray()
diag_msg.header.stamp = rospy.Time.now()
for st in status_list:
diag_msg.status.append(st)
self.diag_pub.publish(diag_msg)
self.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 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 run(self):
# Run while ROS is active
while not rospy.is_shutdown():
# Diagnostics message
temp_diag = DiagnosticArray()
temp_diag.header.stamp = rospy.Time().now()
temp_diag.status.append(DiagnosticStatus())
temp_diag.status[0].name = 'Internal Temperature'
temp_diag.status[0].hardware_id = self.vehicle_name
# Compare internal temperature with threshold levels
temp_diag.status[0].level = DiagnosticStatus.OK
temp_diag.status[0].message = 'OK'
temp_diag.status[0].values.insert(
0, KeyValue(key='Update Status', value='OK'))
if self.temperature.temperature >= self.temp_level_warn:
temp_diag.status[0].level = DiagnosticStatus.WARN
temp_diag.status[0].message = 'Warning'
temp_diag.status[0].values.insert(
0, KeyValue(key='Update Status', value='Warning'))
elif self.temperature.temperature >= self.temp_level_error:
temp_diag.status[0].level = DiagnosticStatus.ERROR
temp_diag.status[0].message = 'Error'
temp_diag.status[0].values.insert(
0, KeyValue(key='Update Status', value='Error'))
temp_diag.status[0].values.insert(
1,
KeyValue(key='Temperature',
value=str(self.temperature.temperature)))
# Check if message is stale (older than 35 seconds)
elapsed = rospy.Time().now().to_sec() - self.last_update
if elapsed > 35:
temp_diag.status[0].level = DiagnosticStatus.STALE
temp_diag.status[0].message = 'Stale'
temp_diag.status[0].values.insert(
0, KeyValue(key='Update Status', value='Stale'))
temp_diag.status[0].values.insert(
1, KeyValue(key='Time Since Update', value=str(elapsed)))
# Publish diagnostics message
self.status_pub.publish(temp_diag)
# Sleep for some time
self.rate.sleep()
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'], '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 'VOLT' in stats:
arr.status += [power_status(hardware, stats['VOLT'])]
if 'TEMP' in stats:
arr.status += [temp_status(hardware, stats['TEMP'])]
# 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_stats(self):
self._mutex.acquire()
# Update everything with last update times
update_status_stale(self._temp_stat, self._last_temp_time)
update_status_stale(self._usage_stat, self._last_usage_time)
update_status_stale(self._nfs_stat, self._last_nfs_time)
msg = DiagnosticArray()
msg.status.append(self._temp_stat)
msg.status.append(self._usage_stat)
msg.status.append(self._nfs_stat)
if rospy.get_time() - self._last_publish_time > 0.5:
self._diag_pub.publish(msg)
self._last_publish_time = rospy.get_time()
self._mutex.release()
def publish(self, msg):
"""Publish a single diagnostic status or a vector of diagnostic statuses."""
if not type(msg) is list:
msg = [msg]
for stat in msg:
stat.name = self.node.get_name() + ': ' + stat.name
now = self.clock.now()
da = DiagnosticArray()
db = DiagnosticStatus()
db.name = stat.name
db.message = stat.message
db.hardware_id = stat.hardware_id
db.values = stat.values
da.status.append(db)
da.header.stamp = now.to_msg() # Add timestamp for ROS 0.10
self.publisher.publish(da)
def publish_stats(self):
with self._mutex:
update_status_stale(self._temp_stat, self._last_temp_time)
msg = DiagnosticArray()
msg.header.stamp = rospy.get_rostime()
msg.status.append(self._temp_stat)
if self._home_dir != '':
update_status_stale(self._usage_stat, self._last_usage_time)
msg.status.append(self._usage_stat)
#for s in msg.status:
#for v in s.values:
#print v, type(v.value)
if rospy.get_time() - self._last_publish_time > 0.5:
self._diag_pub.publish(msg)
self._last_publish_time = rospy.get_time()
def update(self, joints, controllers):
""" Publish diagnostics. """
now = rospy.Time.now()
if now > self.t_next:
# create message
msg = DiagnosticArray()
msg.header.stamp = now
for controller in controllers:
d = controller.getDiagnostics()
if d:
msg.status.append(d)
for joint in joints:
d = joint.getDiagnostics()
if d:
msg.status.append(d)
# publish and update stats
self.pub.publish(msg)
self.t_next = now + self.t_delta
def get_diagnostics(self, time):
"""Publish information about battery in DiagnosticArray msg type."""
self.robot.get_power_state(False)
diag = DiagnosticArray()
diag.header.stamp = time
stat = DiagnosticStatus(name="Battery Status",
level=DiagnosticStatus.OK,
message=self.power_state_msg)
if self.power_state == 3:
stat.level = DiagnosticStatus.WARN
rospy.logwarn("Battery low")
if self.power_state == 4:
stat.level = DiagnosticStatus.ERROR
rospy.logerr("Battery Critical")
diag.status.append(stat)
self.diag_pub.publish(diag)
def publish(self, state):
curr_time = rospy.get_rostime()
# limit to 1hz
if (curr_time - self.last_diagnostics_time).to_sec() < 1.0:
return
self.last_diagnostics_time = curr_time
diag = DiagnosticArray()
diag.header.stamp = curr_time
#battery info
stat = DiagnosticStatus(name="Battery",
level=DiagnosticStatus.OK,
message="OK")
try:
stat.message = self.battery_state[state[1]]
if state[1] < 3:
stat.level = DiagnosticStatus.WARN
stat.message = "Please Recharge Battery (%s)." % self.battery_state[
state[1]]
except KeyError as ex:
stat.message = "Invalid Battery State %s" % ex
rospy.logwarn("Invalid Battery State %s" % ex)
diag.status.append(stat)
#battery info
stat = DiagnosticStatus(name="Connection",
level=DiagnosticStatus.OK,
message="OK")
try:
stat.message = self.connection[state[2]]
except KeyError as ex:
stat.message = "Invalid Connection State %s" % ex
rospy.logwarn("Invalid Connection State %s" % ex)
diag.status.append(stat)
#battery info
stat = DiagnosticStatus(name="Charging State",
level=DiagnosticStatus.OK,
message="OK")
try:
stat.message = self.charging_state[state[0]]
except KeyError as ex:
stat.message = "Invalid Charging State %s" % ex
rospy.logwarn("Invalid Charging State %s" % ex)
diag.status.append(stat)
#publish
self.diag_pub.publish(diag)
def publish_diagnostics(self, event):
"""Generate a diagnostic message of device statuses, voltage levels,
and limit switch triggers. Not critical code.
"""
with self.lock:
diagnostic_arr = DiagnosticArray(
) # create object to hold data to publish
# Generate a ROS message header for time stamping
header = Header()
header.stamp = rospy.Time.now()
diagnostic_arr.header = header
# For all the devices, set the status based on Pololu serial variables
# and append these statues to the diagnostic_arr status field
for i in range(len(self.devices)):
dev = self.devices[i]
name = self.dev_names[i]
status = DiagnosticStatus()
status.name = name
status.hardware_id = str(self.devices[i].dev_num)
errstat = dev.get_error_status()
sererr = dev.get_serial_errors()
if errstat == "OK" and sererr == "OK":
status.level = 0
status.message = errstat + "\n" + sererr
else:
status.level = 1
status.message = errstat + "\n" + sererr
# Get voltage level and set error status if it is too high or low
voltage_level = dev.get_input_voltage()
if voltage_level > 30 or voltage_level < 20:
status.level = 2
status.message = "Voltage out of range: %.2f" % (
voltage_level)
status.values.append(
KeyValue("Voltage", "%.2f" % voltage_level))
status.values.append(KeyValue("Speed", str(dev.get_speed())))
diagnostic_arr.status.append(status)
self.diagnostic_pub.publish(diagnostic_arr)
def publish_diagnostics(self, err=None):
'''
Publishes current kill board state in ROS diagnostics package, making it easy to use in GUIs and other ROS tools
'''
msg = DiagnosticArray()
msg.header.stamp = rospy.Time.now()
status = DiagnosticStatus()
status.name = 'kill_board'
status.hardware_id = self.port
if not self.connected:
status.level = DiagnosticStatus.ERROR
status.message = 'Cannot connect to kill board. Retrying every second.'
status.values.append(KeyValue("Exception", str(err)))
else:
status.level = DiagnosticStatus.OK
for key, value in self.board_status.items():
status.values.append(KeyValue(key, str(value)))
msg.status.append(status)
self.diagnostics_pub.publish(msg)
def publish_stats(self):
with self._mutex:
# Update everything with last update times
update_status_stale(self._temp_stat, self._last_temp_time)
update_status_stale(self._usage_stat, self._last_usage_time)
msg = DiagnosticArray()
msg.header.stamp = rospy.get_rostime()
msg.status.append(self._temp_stat)
msg.status.append(self._usage_stat)
if rospy.get_time() - self._last_publish_time > 0.5:
self._diag_pub.publish(msg)
self._last_publish_time = rospy.get_time()
# Restart temperature checking if it goes stale, #4171
# Need to run this without mutex
if rospy.get_time() - self._last_temp_time > 90:
self._restart_temp_check()
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 ntp_monitor(ntp_hostname,
offset=500,
self_offset=500,
diag_hostname=None,
error_offset=5000000,
do_self_test=True):
pub = rospy.Publisher("/diagnostics", DiagnosticArray)
rospy.init_node(NAME, anonymous=True)
hostname = socket.gethostname()
if diag_hostname is None:
diag_hostname = hostname
stat = DiagnosticStatus()
stat.level = DiagnosticStatus.OK
stat.name = "NTP offset from " + diag_hostname + " to " + ntp_hostname
stat.message = "OK"
stat.hardware_id = hostname
stat.values = []
self_stat = DiagnosticStatus()
self_stat.level = DiagnosticStatus.OK
self_stat.name = "NTP self-offset for " + diag_hostname
self_stat.message = "OK"
self_stat.hardware_id = hostname
self_stat.values = []
while not rospy.is_shutdown():
msg = DiagnosticArray()
msg.header.stamp = rospy.get_rostime()
st = ntp_diag(stat, ntp_hostname, offset, error_offset)
if st is not None:
msg.status.append(st)
if do_self_test:
st = ntp_diag(self_stat, hostname, self_offset, error_offset)
if st is not None:
msg.status.append(st)
pub.publish(msg)
time.sleep(1)
def 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 ros_msg(grpcmsg):
try:
result = DiagnosticArray()
result.header.stamp = rospy.Time(grpcmsg.timestamp)
for stat in grpcmsg.status:
ds = DiagnosticStatus()
ds.level = stat.level
ds.name = stat.name
ds.message = stat.message
ds.hardware_id = stat.hardware_id
for val in stat.values:
dv = KeyValue()
dv.key = val.key
dv.value = val.value
ds.values.append(dv)
result.status.append(ds)
return result
except Exception as _err:
import traceback
raise Exception(traceback.format_exc())
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 __init__(self, level_options):
self.level_options = level_options
interval = rospy.get_param("~interval", 1.0)
# Initialization jtop
self.jetson = jtop.jtop(interval=interval)
# Define Diagnostic array message
# http://docs.ros.org/api/diagnostic_msgs/html/msg/DiagnosticStatus.html
self.arr = DiagnosticArray()
# Initialization ros publisher
self.pub = rospy.Publisher('/diagnostics',
DiagnosticArray,
queue_size=1)
# Additional publisher for some plottable output
self.pub_jetson_status = rospy.Publisher('/gpu_monitor_jetson',
JetsonStatus,
queue_size=1)
# Initialize services server
rospy.Service('/jtop/nvpmodel', nvpmodel, self.nvpmodel_service)
rospy.Service('/jtop/jetson_clocks', jetson_clocks,
self.jetson_clocks_service)
rospy.Service('/jtop/fan', fan, self.fan_service)
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 test_restamp_filter():
filter = RestampFilter()
parser = argparse.ArgumentParser('restamp')
filter.add_arguments(parser)
args = parser.parse_args([])
filter.set_args(None, args)
topic_metadata = TopicMetadata(
'/data', 'diagnostic_msgs/msg/DiagnosticArray', 'cdr')
assert(filter.filter_topic(topic_metadata) == topic_metadata)
ns_stamp = 500 * 1000 * 1000 - 100
msg = DiagnosticArray()
msg.header.stamp.sec = 0
msg.header.stamp.nanosec = ns_stamp
# timestamp within the bag and cut duration
bag_msg = ('/data', serialize_message(msg), 500 * 1000 * 1000)
(_, _, t) = filter.filter_msg(bag_msg)
assert(t == ns_stamp)
def state_callback(self, data):
"""
Callback de lecture de l'état du drone :
- Armed ou Disarmed et Mode (Hold, Manual, Loiter...)
- Publication de diagnostics
Entrée :
data : mavros_msgs State Message
"""
self.arming = data.armed * "Armed" + (1 - data.armed) * "Disarmed"
self.mode = data.mode
diagnostics = DiagnosticArray()
diagnostics.status.append(
DiagnosticStatus(level=0,
name="controller/state/Arming",
message=self.arming))
diagnostics.status.append(
DiagnosticStatus(level=0,
name="controller/state/Mode",
message=self.mode))
diagnostics.header.stamp = rospy.Time.now()
self.diag_pub.publish(diagnostics)
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 __init__(self):
rospy.init_node("wlan_monitor")
self.get_params()
self._wlan_stat = DiagnosticStatus()
self._wlan_stat.name = '%s WLAN Info' % self.diag_hostname
self._wlan_stat.hardware_id = self.diag_hostname
self._wlan_stat.level = DiagnosticStatus.OK
self._wlan_stat.message = 'No Data'
self._wlan_stat.values = []
self.msg = DiagnosticArray()
self.msg.header.stamp = rospy.get_rostime()
self.msg.status = [self._wlan_stat]
self.diag_pub = rospy.Publisher("/diagnostics",
DiagnosticArray,
queue_size=1)
self.diag_timer = rospy.Timer(rospy.Duration(1.0),
self.publish_diagnostics)
if self.monitor_local:
self.iwconfig = IwConfigLocal()
else:
try:
self.iwconfig = IwConfigSSH(self.diag_hostname, self.user,
self.password)
except Exception as e:
msg = "Cannot connect to router via ssh. Please check if ssh key of user '{}' is contained in the router configuration or password is provided. Error message: {}".format(
getpass.getuser(), e)
rospy.logerr(msg)
self._wlan_stat.level = DiagnosticStatus.ERROR
self._wlan_stat.message = msg
self._wlan_stat.values = [
KeyValue(key='Exception', value=str(e))
]
self.msg.status = [self._wlan_stat]
return
self.monitor_timer = rospy.Timer(rospy.Duration(1.0),
self.update_diagnostics)
def publish_diagnostics(self, time):
diag = DiagnosticArray()
diag.header.stamp = time
values = []
values.append(KeyValue(key="voltage", value=str(self.batt_voltage)))
values.append(KeyValue(key="numCharges", value=str(self.num_charges)))
values.append(
KeyValue(key="timeSinceCharge", value=str(self.time_since_chg)))
stat = DiagnosticStatus(name="Battery Status",
level=DiagnosticStatus.OK,
message=self.power_state_msg,
values=values)
if self.power_state == 3:
stat.level = DiagnosticStatus.WARN
if self.power_state == 4:
stat.level = DiagnosticStatus.ERROR
diag.status.append(stat)
self.diag_pub.publish(diag)
def _publish_diagnostic(self):
"""Publish diagnostics."""
diagnostic = DiagnosticArray()
diagnostic.header.stamp = rospy.Time.now()
status = DiagnosticStatus()
status.name = "LocalXY Origin"
status.hardware_id = "origin_publisher"
if self.origin is None:
status.level = DiagnosticStatus.ERROR
status.message = "No Origin"
else:
if self.origin_source == "gpsfix":
status.level = DiagnosticStatus.OK
status.message = "Has Origin (GPSFix)"
elif self.origin_source == "navsat":
status.level = DiagnosticStatus.OK
status.message = "Has Origin (NavSatFix)"
elif self.origin_source == "custom":
status.level = DiagnosticStatus.OK
status.message = "Has Origin (Custom Topic)"
else:
status.level = DiagnosticStatus.WARN
status.message = "Origin Was Set Manually"
frame_id = self.origin.header.frame_id
status.values.append(
KeyValue(key="Origin Frame ID", value=frame_id))
latitude = "%f" % self.origin.pose.position.y
status.values.append(KeyValue(key="Latitude", value=latitude))
longitude = "%f" % self.origin.pose.position.x
status.values.append(KeyValue(key="Longitude", value=longitude))
altitude = "%f" % self.origin.pose.position.z
status.values.append(KeyValue(key="Altitude", value=altitude))
diagnostic.status.append(status)
self.diagnostic_pub.publish(diagnostic)
def test_reframe_filter():
filter = ReframeFilter()
parser = argparse.ArgumentParser('reframe')
filter.add_arguments(parser)
args = parser.parse_args(['-t', '/data', '--frame', 'frame1'])
filter.set_args(None, args)
topic_metadata = TopicMetadata(
'/data', 'diagnostic_msgs/msg/DiagnosticArray', 'cdr')
assert(filter.filter_topic(topic_metadata) == topic_metadata)
msg = DiagnosticArray()
msg.header.frame_id = 'frame0'
msg.header.stamp.sec = 0
msg.header.stamp.nanosec = 1
# timestamp within the bag and cut duration
bag_msg = ('/data', serialize_message(msg), 1)
(_, data, _) = filter.filter_msg(bag_msg)
new_msg = deserialize_message(data, DiagnosticArray)
assert(new_msg.header.frame_id == 'frame1')
def __init__(self):
print("Setting up Diagnostics Node")
# Variable Initialization
print("Diagnostics Node: Initializing Variables")
self.names = [] # List of different diagnostics message names
self.received_diag_msg = []
# Configuration Parameters
self.Hertz = 100 # frequency of while loop
# Subscribers
print("Diagnostics Node: Defining Subscribers")
rospy.Subscriber("/diagnostics",
DiagnosticArray,
self.callbackDiagnostics,
queue_size=1000) # Diagnostics
# Publishers
print("Diagnostics Node: Defining Publishers")
self.diag_pub = rospy.Publisher('/diagnostics_agg',
DiagnosticArray,
queue_size=1)
# Messages
print("Diagnostics Node: Defining Messages")
self.diag_agg_msg = DiagnosticArray()
self.diag_agg_status = []
# Loop Timing
rate = rospy.Rate(self.Hertz)
time.sleep(2) # allows the callback functions to populate variables
print("Commencing Diagnostics Node Execution")
while not rospy.is_shutdown():
self.diag_agg_msg.status = self.diag_agg_status
self.diag_pub.publish(self.diag_agg_msg)
rate.sleep()
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()
