def send_qa_value_msgs(self, key_names, init_value, end_value, step=0.1):
##############################################################################
key_value = init_value
rospy.loginfo("- D - Test sending %s - %s - Step value %s" % (str(key_names), str(key_value), str(step)))
max_steps = (end_value - init_value) / step
rospy.logwarn("--- D --- Max steps %s " % (str(max_steps)))
step_count = 0
while not rospy.is_shutdown() and not self.success and max_steps > step_count:
diag_msg = DiagnosticArray()
diag_msg.header.stamp = rospy.get_rostime()
for k_name in key_names:
status_msg = DiagnosticStatus()
status_msg.level = DiagnosticStatus.OK
status_msg.name = ""
status_msg.values.append(
KeyValue(str(k_name), str(key_value)))
status_msg.message = "QA status"
diag_msg.status.append(status_msg)
self.message_pub.publish(diag_msg)
key_value = key_value + step
step_count = step_count + 1
rospy.sleep(2.0)
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 loop(pub):
stat = []
for c in range(0,4):
floatval = []
strval = []
floatval.append(KeyValue(c, "Time Remaining Controller %d"%c))
floatval.append(KeyValue(c+.1, "Average charge percent Controller %d"%c))
floatval.append(KeyValue(c+.2, "Current Controller %d"%c))
floatval.append(KeyValue(c+.2, "Voltage Controller %d"%c))
strval.append(DiagnosticString("String Value","Controller String Label"))
stat.append(DiagnosticStatus(c, "IBPS %d"%c, "All good", floatval,strval))
## @todo make the status string represent errors etc
for b in range(0,4):
bval = []
bstrval = []
bval.append(KeyValue(c+b*.01+.1, "present (0,1)"))
bval.append(KeyValue(c+b*.01+.2, "charging (0,1)"))
bval.append(KeyValue(c+b*.01+.3, "supplying power (0,1)"))
bstrval.append(DiagnosticString("String Value","Controller, String Label"))
for a in range(0,100):
bstrval.append(DiagnosticString("Lots of reeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaallllllllllllllllllllllllllllllllllllllyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyy long string values!\nand\nnewlines", "Test"))
stat.append(DiagnosticStatus(b, "Smart Battery %d.%d"%(c,b), "All good", bval, bstrval))
## @todo make the status string represent errors etc
out = DiagnosticArray(None, stat)
pub.publish(out)
print "Published"
def spin(self):
r = rospy.Rate(self._diagnostic_freq)
while not rospy.is_shutdown():
diag = DiagnosticArray()
diag.header.stamp = rospy.get_rostime()
for mon in self._monitors:
d = DiagnosticStatus()
d.name=mon.get_name()
d.hardware_id="PC0"
d.message=mon.get_field_value("status")
d.level=mon.get_field_value("status_level")
for key in mon.get_fields():
p = KeyValue()
p.key = key
p.value = str(mon.get_field_value(key))
d.values.append(p)
diag.status.append(d)
self._diagnostic_pub.publish(diag)
r.sleep()
self._cpu_temp_mon.stop()
self._cpu_usage_mon.stop()
self._wlan_mon.stop()
self._bandwidth_mon.stop()
def 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, hostname, diag_hostname, home_dir = ''):
self._hostname = hostname
self._home_dir = home_dir
self.unit = 'G'
self.low_hd_level = rospy.get_param('~low_hd_level', 5) #self.unit
self.critical_hd_level = rospy.get_param('~critical_hd_level', 1) #self.unit
self._usage_stat = DiagnosticStatus()
self._usage_stat.level = DiagnosticStatus.WARN
self._usage_stat.hardware_id = hostname
self._usage_stat.name = '%s HD Usage' % diag_hostname
self._usage_stat.message = 'No Data'
self._usage_stat.values = []
self._io_stat = DiagnosticStatus()
self._io_stat.name = '%s HD IO' % diag_hostname
self._io_stat.level = DiagnosticStatus.WARN
self._io_stat.hardware_id = hostname
self._io_stat.message = 'No Data'
self._io_stat.values = []
self._diag_pub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=1)
self._publish_timer = rospy.Timer(rospy.Duration(1.0), self.publish_stats)
self._usage_timer = rospy.Timer(rospy.Duration(5.0), self.check_disk_usage)
self._io_timer = rospy.Timer(rospy.Duration(5.0), self.check_io_stat)
def diagnostic_publisher(self):
"""
Fonction de publication des diagnostiques :
- current_wpt
- Etat
- ...
Lancement dans un thread pour avoir une publication régulière
"""
diagnostics = DiagnosticArray()
while not (rospy.is_shutdown()):
diagnostics.status.append(
DiagnosticStatus(level=0,
name="controller/waypoints/Number",
message=str(self.wp_number)))
diagnostics.status.append(
DiagnosticStatus(level=0,
name="controller/waypoints/Current",
message=str(self.current_wp)))
diagnostics.status.append(
DiagnosticStatus(level=0,
name="controller/state/Mode",
message=self.mode))
diagnostics.status.append(
DiagnosticStatus(level=0,
name="controller/state/Arming",
message=self.arming))
diagnostics.header.stamp = rospy.Time.now()
self.diag_pub.publish(diagnostics)
rospy.sleep(0.5)
def info_status(self):
stat = DiagnosticStatus(name="Info_Platform", level=DiagnosticStatus.OK, message="OK")
stat.values = [
KeyValue("", str(self.robot.get_status()[0])),
KeyValue("", str(self.robot.get_status()[1])),
KeyValue("", str(self.robot.get_status()[2]))]
return stat
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 fill_diags(name, summary, hid, diags):
ds = DiagnosticStatus()
ds.values = [KeyValue(k, str(v)) for (k, v) in diags]
ds.hardware_id = hid
ds.name = rospy.get_caller_id().lstrip('/') + ": " + name
ds.message = summary
return ds
def _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 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 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 send_unsupported_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_unsupported_callback)
timeout_t = time.time() + timeout # Default timeout 5 sec.
while (not rospy.is_shutdown() and not self.success_us
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_NOT_SUPPORTED), 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 __init__(self, hostname, diag_hostname):
self._diag_pub = rospy.Publisher('/diagnostics',
DiagnosticArray,
queue_size=100)
self._mutex = threading.Lock()
self._check_core_temps = rospy.get_param('~check_core_temps', True)
self._cpu_load_warn = rospy.get_param('~cpu_load_warn', cpu_load_warn)
self._cpu_load_error = rospy.get_param('~cpu_load_error',
cpu_load_error)
self._cpu_load1_warn = rospy.get_param('~cpu_load1_warn',
cpu_load1_warn)
self._cpu_load5_warn = rospy.get_param('~cpu_load5_warn',
cpu_load5_warn)
self._cpu_temp_warn = rospy.get_param('~cpu_temp_warn', cpu_temp_warn)
self._cpu_temp_error = rospy.get_param('~cpu_temp_error',
cpu_temp_error)
self._num_cores = multiprocessing.cpu_count()
self._temps_timer = None
self._usage_timer = None
# Get temp_input files
self._temp_vals = self.get_core_temp_names()
# CPU stats
self._temp_stat = DiagnosticStatus()
self._temp_stat.name = 'CPU Temperature (%s)' % diag_hostname
self._temp_stat.level = 1
self._temp_stat.hardware_id = hostname
self._temp_stat.message = 'No Data'
self._temp_stat.values = [
KeyValue(key='Update Status', value='No Data'),
KeyValue(key='Time Since Last Update', value='N/A')
]
self._usage_stat = DiagnosticStatus()
self._usage_stat.name = 'CPU Usage (%s)' % diag_hostname
self._usage_stat.level = 1
self._usage_stat.hardware_id = hostname
self._usage_stat.message = 'No Data'
self._usage_stat.values = [
KeyValue(key='Update Status', value='No Data'),
KeyValue(key='Time Since Last Update', value='N/A')
]
self._last_temp_time = 0
self._last_usage_time = 0
self._last_publish_time = 0
self._usage_old = 0
self._has_warned_mpstat = False
self._has_error_core_count = False
# Start checking everything
self.check_temps()
self.check_usage()
def drawQuadraticBezier(self, ele, da):
# 开始点
ps = np.array([ele.start.real, ele.start.imag])
# 控制点
p = np.array([ele.control.real, ele.control.imag])
# 结束点
pe = np.array([ele.end.real, ele.end.imag])
point = self.QuadraticBezier(ps, p, pe, 0)
start = int(point[0]), int(point[1])
z = 30.25
# 创建点
point = {
'x': str(start[0]),
'y': str(start[1]),
'z': str(z),
'type': 'MOVE',
'type2': 'drawQuadraticBezierStart'
}
points = DiagnosticStatus()
points.values.append(KeyValue('x', str(start[0])))
points.values.append(KeyValue('y', str(start[1])))
points.values.append(KeyValue('z', str(z)))
points.values.append(KeyValue('type', 'MOVE'))
points.values.append(KeyValue('type2', 'drawQuadraticBezierStart'))
da.status.append(points)
# 添加到容器中
self.points.append(point)
# 40个点
for i in range(1, 41):
result = self.QuadraticBezier(ps, p, pe, i / 40)
end = int(result[0]), int(result[1])
# 创建点
point = {
'x': str(end[0]),
'y': str(end[1]),
'z': str(z),
'type': 'MOVE',
'type2': 'drawQuadraticBezierEnd'
}
points = DiagnosticStatus()
points.values.append(KeyValue('x', str(end[0])))
points.values.append(KeyValue('y', str(end[1])))
points.values.append(KeyValue('z', str(z)))
points.values.append(KeyValue('type', 'MOVE'))
points.values.append(KeyValue('type2', 'drawQuadraticBezierEnd'))
da.status.append(points)
# 添加到容器中
self.points.append(point)
def __init__(self, hostname):
rospy.init_node('cpu_monitor_%s' % hostname)
self._diag_pub = rospy.Publisher('/diagnostics', DiagnosticArray)
self._mutex = threading.Lock()
self._check_ipmi = rospy.get_param('check_ipmi_tool', True)
self._enforce_speed = rospy.get_param('enforce_clock_speed', True)
# Get temp_input files
self._temp_vals = get_core_temp_names()
# CPU stats
self._temp_stat = DiagnosticStatus()
self._temp_stat.name = '%s CPU Temperature' % hostname
self._temp_stat.level = 2
self._temp_stat.hardware_id = hostname
self._temp_stat.message = 'No Data'
self._temp_stat.values = [
KeyValue(key='Update Status', value='No Data'),
KeyValue(key='Time Since Last Update', value='N/A')
]
self._usage_stat = DiagnosticStatus()
self._usage_stat.name = '%s CPU Usage' % hostname
self._usage_stat.level = 2
self._usage_stat.hardware_id = hostname
self._usage_stat.message = 'No Data'
self._usage_stat.values = [
KeyValue(key='Update Status', value='No Data'),
KeyValue(key='Time Since Last Update', value='N/A')
]
self._nfs_stat = DiagnosticStatus()
self._nfs_stat.name = '%s NFS IO' % hostname
self._nfs_stat.level = 2
self._nfs_stat.hardware_id = hostname
self._nfs_stat.message = 'No Data'
self._nfs_stat.values = [
KeyValue(key='Update Status', value='No Data'),
KeyValue(key='Time Since Last Update', value='N/A')
]
self._last_temp_time = 0
self._last_usage_time = 0
self._last_nfs_time = 0
self._last_publish_time = 0
self._temps_timer = None
self._usage_timer = None
self._nfs_timer = None
self._publish_timer = None
##@todo Need wireless stuff, at some point, put NFS in usage status
# Start checking everything
self.check_temps()
self.check_nfs_stat()
self.check_usage()
def __init__(self):
device = get_param('~device', 'auto')
baudrate = get_param('~baudrate', 0)
timeout = get_param('~timeout', 0.002)
if device == 'auto':
devs = mtdevice.find_devices()
if devs:
device, baudrate = devs[0]
rospy.loginfo("Detected MT device on port %s @ %d bps" %
(device, baudrate))
else:
rospy.logerr("Fatal: could not find proper MT device.")
rospy.signal_shutdown("Could not find proper MT device.")
return
if not baudrate:
baudrate = mtdevice.find_baudrate(device)
if not baudrate:
rospy.logerr("Fatal: could not find proper baudrate.")
rospy.signal_shutdown("Could not find proper baudrate.")
return
rospy.loginfo("MT node interface: %s at %d bd." % (device, baudrate))
self.mt = mtdevice.MTDevice(device, baudrate, timeout)
self.frame_id = get_param('~frame_id', '/base_imu')
self.frame_local = get_param('~frame_local', 'ENU')
self.diag_msg = DiagnosticArray()
self.stest_stat = DiagnosticStatus(name='mtnode: Self Test',
level=1,
message='No status information')
self.xkf_stat = DiagnosticStatus(name='mtnode: XKF Valid',
level=1,
message='No status information')
self.gps_stat = DiagnosticStatus(name='mtnode: GPS Fix',
level=1,
message='No status information')
self.diag_msg.status = [self.stest_stat, self.xkf_stat, self.gps_stat]
# publishers created at first use to reduce topic clutter
self.diag_pub = None
self.imu_pub = None
self.gps_pub = None
self.vel_pub = None
self.mag_pub = None
self.temp_pub = None
self.press_pub = None
self.analog_in1_pub = None # decide type+header
self.analog_in2_pub = None # decide type+header
self.ecef_pub = None
self.time_ref_pub = None
# TODO pressure, ITOW from raw GPS?
self.old_bGPS = 256 # publish GPS only if new
# publish a string version of all data; to be parsed by clients
self.str_pub = rospy.Publisher('imu_data_str', String, queue_size=10)
def update_diagnostics(self, event=None):
stat = DiagnosticStatus()
stat.level = DiagnosticStatus.WARN
stat.name = '%s NTP Offset' % self.diag_hostname
stat.message = 'No Data'
stat.hardware_id = self.diag_hostname
stat.values = []
try:
for st, host, off in [(stat, self.ntp_server, self.offset)]:
p = Popen(["ntpdate", "-q", host],
stdout=PIPE,
stdin=PIPE,
stderr=PIPE)
retcode = p.wait()
stdout, stderr = p.communicate()
try:
stdout = stdout.decode() #python3
except (UnicodeDecodeError, AttributeError):
pass
if retcode != 0:
st.level = DiagnosticStatus.ERROR
st.message = 'ntpdate Error'
st.values = [
KeyValue(key='ntpdate Error', value=stderr),
KeyValue(key='Output', value=stdout)
]
continue
measured_offset = float(
re.search("offset (.*),", stdout).group(1)) * 1000000
st.level = DiagnosticStatus.OK
st.message = "OK"
st.values = [
KeyValue("NTP Server", self.ntp_server),
KeyValue("Offset (us)", str(measured_offset)),
KeyValue("Offset tolerance (us)", str(off)),
KeyValue("Offset tolerance (us) for Error",
str(self.error_offset))
]
if (abs(measured_offset) > off):
st.level = DiagnosticStatus.WARN
st.message = "NTP Offset Too High"
if (abs(measured_offset) > self.error_offset):
st.level = DiagnosticStatus.ERROR
st.message = "NTP Offset Too High"
except Exception as e:
stat.level = DiagnosticStatus.ERROR
stat.message = 'ntpdate Exception'
stat.values = [KeyValue(key='Exception', value=str(e))]
self.msg = DiagnosticArray()
self.msg.header.stamp = rospy.get_rostime()
self.msg.status = [stat]
def speed_status(self):
stat = DiagnosticStatus(name="Speed", level=DiagnosticStatus.OK, message="OK")
stat.values = [
KeyValue("Linear speed (Vx)", str(self.robot.get_movesteer(None)[0])),
KeyValue("Angular speed (Vth)", str(self.robot.get_movesteer(None)[2]))]
if self.robot.get_movesteer(None)[0] > self.speedLimit:
stat.level = DiagnosticStatus.WARN
stat.message = "speed is too high"
return stat
def connection_status(self):
stat = DiagnosticStatus(name="Connection", level=DiagnosticStatus.OK, message="OK")
stat.values = [
KeyValue("Baudrate", str(self.robot.get_connection_info()["baudrate"])),
KeyValue("Comport", str(self.robot.get_connection_info()["comport"]))]
if self.robot.is_connected() == False:
stat.message = "disconnected"
stat.level = DiagnosticStatus.ERROR
return stat
def publish_diagnostics(self, name, msg):
rospy.loginfo("Node: " + name + " has died")
darray = DiagnosticArray()
dstatus = DiagnosticStatus()
dstatus.level = DiagnosticStatus.WARN
dstatus.name = "verbose_logger"
dstatus.message = name + ": " + msg
darray.status.append(dstatus)
self.diag_pub.publish(darray)
def __init__(self):
device = get_param('~device', 'auto')
baudrate = get_param('~baudrate', 0)
if device == 'auto':
devs = mtdevice.find_devices()
if devs:
device, baudrate = devs[0]
rospy.loginfo("Detected MT device on port %s @ %d bps" %
(device, baudrate))
else:
rospy.logerr("Fatal: could not find proper MT device.")
rospy.signal_shutdown("Could not find proper MT device.")
return
if not baudrate:
baudrate = mtdevice.find_baudrate(device)
if not baudrate:
rospy.logerr("Fatal: could not find proper baudrate.")
rospy.signal_shutdown("Could not find proper baudrate.")
return
rospy.loginfo("MT node interface: %s at %d bd." % (device, baudrate))
self.mt = mtdevice.MTDevice(device, baudrate)
self.frame_id = get_param('~frame_id', '/base_imu')
self.diag_pub = rospy.Publisher('/diagnostics', DiagnosticArray)
self.diag_msg = DiagnosticArray()
self.stest_stat = DiagnosticStatus(name='mtnode: Self Test',
level=1,
message='No status information')
self.xkf_stat = DiagnosticStatus(name='mtnode: XKF Valid',
level=1,
message='No status information')
self.gps_stat = DiagnosticStatus(name='mtnode: GPS Fix',
level=1,
message='No status information')
self.diag_msg.status = [self.stest_stat, self.xkf_stat, self.gps_stat]
self.imu_pub = rospy.Publisher('imu/data', Imu)
self.gps_pub = rospy.Publisher('fix', NavSatFix)
self.xgps_pub = rospy.Publisher('fix_extended', GPSFix)
self.vel_pub = rospy.Publisher('velocity', TwistStamped)
self.mag_pub = rospy.Publisher('magnetic', Vector3Stamped)
self.temp_pub = rospy.Publisher('temperature',
Float32) # decide type+header
self.press_pub = rospy.Publisher('pressure',
Float32) # decide type+header
self.analog_in1_pub = rospy.Publisher('analog_in1',
UInt16) # decide type+header
self.analog_in2_pub = rospy.Publisher('analog_in2',
UInt16) # decide type+header
# TODO pressure, ITOW from raw GPS?
self.old_bGPS = 256 # publish GPS only if new
# publish a string version of all data; to be parsed by clients
self.str_pub = rospy.Publisher('imu_data_str', String)
def publish_diag(self, level, choice):
msg = DiagnosticArray()
stat = DiagnosticStatus()
msg.status.append(stat)
stat.level = level
stat.name = 'EtherCAT Master' # So ghetto
stat.message = choice
self.diag_pub.publish(msg)
def send_diags():
# See diagnostics with: rosrun rqt_runtime_monitor rqt_runtime_monitor
msg = DiagnosticArray()
msg.status = []
msg.header.stamp = rospy.Time.now()
for gripper in grippers:
for servo in gripper.servos:
status = DiagnosticStatus()
status.name = "Gripper '%s' servo %d"%(gripper.name, servo.servo_id)
status.hardware_id = '%s'%servo.servo_id
temperature = servo.read_temperature()
status.values.append(KeyValue('Temperature', str(temperature)))
status.values.append(KeyValue('Voltage', str(servo.read_voltage())))
if temperature >= 70:
status.level = DiagnosticStatus.ERROR
status.message = 'OVERHEATING'
elif temperature >= 65:
status.level = DiagnosticStatus.WARN
status.message = 'HOT'
else:
status.level = DiagnosticStatus.OK
status.message = 'OK'
msg.status.append(status)
diagnostics_pub.publish(msg)
def 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 publish_motor_status(self, event=None):
status = self.motor.get_status()
data_list = []
for key in status:
data_list.append(KeyValue(key, str(status[key])))
msg = DiagnosticStatus()
msg.values = data_list
self.motor_status_pub.publish(msg)
def _camera_diag(level = 0):
array = DiagnosticArray()
stat = DiagnosticStatus()
stat.name = 'wge100: Driver Status'
stat.level = level
stat.message = 'OK'
array.header.stamp = rospy.get_rostime()
array.status.append(stat)
return array
def pub_component_diagnostic(component_name):
diag_msg = DiagnosticArray()
diag_msg.header.stamp = rospy.get_rostime()
comp_msg = DiagnosticStatus()
comp_msg.level = DiagnosticStatus.ERROR
comp_msg.name = component_name
comp_msg.message = "Component status"
diag_msg.status.append(comp_msg)
diagnostics_pub.publish(diag_msg)
def test_merge_summary_dmsg(self):
d = DiagnosticStatusWrapper(level=b'0', message='ok')
m = DiagnosticStatus(level=b'1', message='warn')
d.mergeSummary(m)
self.assertEqual(d.level, b'1')
self.assertEqual(d.message, 'warn')
m = DiagnosticStatus(level=b'2', message='err')
d.mergeSummary(m)
self.assertEqual(d.level, b'2')
self.assertEqual(d.message, 'warn; err')
def get_new_diagnostic_message(self):
from diagnostic_msgs.msg import DiagnosticArray
from diagnostic_msgs.msg import DiagnosticStatus
from diagnostic_msgs.msg import KeyValue
msg = DiagnosticArray(None, [])
msg.status.append(DiagnosticStatus(0, "abcdef", "ghijkl", "NONE", [KeyValue('abc', str(repack(12.34))), KeyValue('jkl', 'ghbvf')]))
msg.status.append(DiagnosticStatus(0, "mnop", "qrst", "NONE", [KeyValue('def', str(repack(56.78))), KeyValue('mno', 'klmnh')]))
msg.status.append(DiagnosticStatus(0, "uvw", "xyz", "NONE", [KeyValue('ghi', str(repack(90.12))), KeyValue('pqr', 'erfcd')]))
return msg
def pub_direction_diagnostic(function_name):
diag_msg = DiagnosticArray()
diag_msg.header.stamp = rospy.get_rostime()
comp_msg = DiagnosticStatus()
comp_msg.level = DiagnosticStatus.ERROR
comp_msg.name = function_name
comp_msg.message = "Movement status"
diag_msg.status.append(comp_msg)
diagnostics_pub.publish(diag_msg)
def create_node_status(self):
diag_status = DiagnosticStatus()
diag_status.name = "BHG: Node Status"
diag_status.message = "BHG: Normal"
diag_status.hardware_id = socket.gethostname()
for r_node in node_list:
# self.node_state[r_node] = rosnode.rosnode_ping(r_node, 1, False)
node_case_kv = KeyValue()
node_case_kv.key = r_node
node_case_kv.value = str(rosnode.rosnode_ping(r_node, 1, False))
diag_status.values.append(node_case_kv)
return diag_status
def publish_diagnostics(self, time):
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
if self.power_state == 4:
stat.level=DiagnosticStatus.ERROR
diag.status.append(stat)
self.diag_pub.publish(diag)
def publish_diagnostics(self, time):
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
if self.power_state == 4:
stat.level=DiagnosticStatus.ERROR
diag.status.append(stat)
self.diag_pub.publish(diag)
def test_DiagnosticStatus():
'''
PUBLISHER METHODE: DiagnosticStatus
'''
pub_DiagnosticStatus = rospy.Publisher('diagnostic', DiagnosticStatus, queue_size=10)
message = DiagnosticStatus()
message.name = "ROB"
message.message = "RUNNING"
print "DiagnosticStatus: " + message.name + " , " + message.message
pub_DiagnosticStatus.publish(message)
def get_diagnostic(self):
diag_msg = DiagnosticStatus()
sys_status, gyro_status, accel_status, mag_status = self.bno.get_calibration_status(
)
sys = KeyValue("System status", str(sys_status))
gyro = KeyValue("Gyro calibration status", str(gyro_status))
accel = KeyValue("Accelerometer calibration status", str(accel_status))
mag = KeyValue("Magnetometer calibration status", str(mag_status))
diag_msg.values = [sys, gyro, accel, mag]
return diag_msg
def _get_status(self):
"""
Omits some errors that aren't really errors. The overload error is treated as a warning
"""
status = self.diagnostics_msg.status[0].level
if status == DiagnosticStatus().ERROR and self._receiving():
# Check that the FT sensor reports new values
w1 = from_wrench(self.raw_queue[0].wrench)
w2 = from_wrench(self.raw_queue[self.queue_len - 1].wrench)
status = DiagnosticStatus().ERROR if np.allclose(
w1, w2) else DiagnosticStatus().OK
return status
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 __init__(self, hostname, diag_hostname):
self._check_ipmi = rospy.get_param('~check_ipmi_tool', False)
self._check_core_temps = rospy.get_param('~check_core_temps', False)
self._core_load_warn = rospy.get_param('~core_load_warn', 90)
self._core_load_error = rospy.get_param('~core_load_error', 110)
self._load1_threshold = rospy.get_param('~load1_threshold', 5.0)
self._load5_threshold = rospy.get_param('~load5_threshold', 3.0)
self._core_temp_warn = rospy.get_param('~core_temp_warn', 90)
self._core_temp_error = rospy.get_param('~core_temp_error', 95)
self._mem_warn = rospy.get_param('~mem_warn', 25)
self._mem_error = rospy.get_param('~mem_error', 1)
if psutil.__version__ < '2.0.0':
self._num_cores = rospy.get_param('~num_cores', psutil.NUM_CPUS)
else:
self._num_cores = rospy.get_param('~num_cores', psutil.cpu_count())
# Get temp_input files
self._temp_vals = self.get_core_temp_names()
# CPU stats
self._info_stat = DiagnosticStatus()
self._info_stat.name = '%s CPU Info' % diag_hostname
self._info_stat.level = DiagnosticStatus.WARN
self._info_stat.hardware_id = hostname
self._info_stat.message = 'No Data'
self._info_stat.values = []
self._usage_stat = DiagnosticStatus()
self._usage_stat.name = '%s CPU Usage' % diag_hostname
self._usage_stat.level = DiagnosticStatus.WARN
self._usage_stat.hardware_id = hostname
self._usage_stat.message = 'No Data'
self._usage_stat.values = []
self._memory_stat = DiagnosticStatus()
self._memory_stat.name = '%s Memory Usage' % diag_hostname
self._memory_stat.level = DiagnosticStatus.WARN
self._memory_stat.hardware_id = hostname
self._memory_stat.message = 'No Data'
self._memory_stat.values = []
self._diag_pub = rospy.Publisher('/diagnostics',
DiagnosticArray,
queue_size=1)
self._publish_timer = rospy.Timer(rospy.Duration(1.0),
self.publish_stats)
self._info_timer = rospy.Timer(rospy.Duration(5.0), self.check_info)
self._usage_timer = rospy.Timer(rospy.Duration(5.0), self.check_usage)
self._memory_timer = rospy.Timer(rospy.Duration(5.0),
self.check_memory)
def load(self, argv):
'''
:param argv: a list with argv parameter needed to load the launch file.
The name and value are separated by `:=`
:type argv: list(str)
:return: True, if the launch file was loaded and argv, used while launch
:rtype: tuple(bool, [])
:raise LaunchConfigException: on load errors
'''
try:
self._capabilities = None
self._robot_description = None
roscfg = roslaunch.ROSLaunchConfig()
loader = roslaunch.XmlLoader()
self.argv = self.resolve_args(argv)
loader.ignore_unset_args = False
loader.load(self.filename, roscfg, verbose=False, argv=self.argv)
self.__roscfg = roscfg
if 'arg' in loader.root_context.resolve_dict:
self.resolve_dict = loader.root_context.resolve_dict['arg']
self.changed = True
# check for depricated parameter
diag_dep = DiagnosticArray()
if self._monitor_servicer is not None:
diag_dep.header.stamp = rospy.Time.now()
for n in roscfg.nodes:
node_fullname = roslib.names.ns_join(n.namespace, n.name)
associations_param = roslib.names.ns_join(
node_fullname, 'associations')
if associations_param in roscfg.params:
ds = DiagnosticStatus()
ds.level = DiagnosticStatus.WARN
ds.name = node_fullname
ds.message = 'Deprecated parameter detected'
ds.values.append(KeyValue('deprecated', 'associations'))
ds.values.append(KeyValue('new', 'nm/associations'))
rospy.logwarn(
"'associations' is deprecated, use 'nm/associations'! found for node: %s in %s"
% (node_fullname, self.filename))
diag_dep.status.append(ds)
if self._monitor_servicer is not None:
# set diagnostics
self._monitor_servicer._monitor._callback_diagnostics(diag_dep)
except roslaunch.XmlParseException as e:
test = list(
re.finditer(r"environment variable '\w+' is not set", utf8(e)))
message = utf8(e)
if test:
message = '%s\nenvironment substitution is not supported, use "arg" instead!' % message
raise LaunchConfigException(message)
return True, self.argv
def battery_status(self):
stat = DiagnosticStatus(name="Battery", level=DiagnosticStatus.OK, message="OK")
stat.values = [
KeyValue("Voltage avg(Vin)", str(self.robot.get_avg_voltageIn())),
KeyValue("Voltage max(Vin)", str(self.robot.get_max_voltageIn())),
KeyValue("Voltage min(Vin)", str(self.robot.get_min_voltageIn())),
KeyValue("Current avg(A)", str(self.robot.get_avg_current())),
KeyValue("Current max(A)", str(self.robot.get_avg_current())),
KeyValue("Current min(A)", str(self.robot.get_avg_current())),
KeyValue("Voltage avg(V5V)", str(self.robot.get_avg_5voltage())),
KeyValue("Voltage max(V5V)", str(self.robot.get_max_5voltage())),
KeyValue("Voltage min(V5V)", str(self.robot.get_min_5voltage())),
KeyValue("Voltage avg(V3.3)", str(self.robot.get_avg_3voltage())),
KeyValue("Voltage max(V3.3)", str(self.robot.get_max_3voltage())),
KeyValue("Voltage min(V3.3)", str(self.robot.get_min_3voltage()))]
if self.robot.get_status().VoltageLow == True:
stat.level = DiagnosticStatus.WARN
stat.message = "Voltage too low"
if self.robot.get_status().CurrentError == True:
stat.level = DiagnosticStatus.WARN
stat.message = "Current error"
if self.robot.get_status().Voltage3v3Low == True:
stat.level = DiagnosticStatus.WARN
stat.message = "Voltage3.3 too low"
return stat
def 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)
levels = [status.level for status in filtered_statuses]
new_message = DiagnosticStatus()
new_message.name = "Robot Status"
new_message.level = max(levels)
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 __init__(self, *args, **kwds):
"""
Constructor. Any message fields that are implicitly/explicitly
set to None will be assigned a default value. The recommend
use is keyword arguments as this is more robust to future message
changes. You cannot mix in-order arguments and keyword arguments.
The available fields are:
level,name,message,hardware_id,values
@param args: complete set of field values, in .msg order
@param kwds: use keyword arguments corresponding to message field names
to set specific fields.
"""
DiagnosticStatus.__init__(self, *args, **kwds)
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 publish_diagnostics(self, time):
diag = DiagnosticArray()
diag.header.stamp = time
for sphero_name in self.sphero_dict:
sphero = self.sphero_dict[sphero_name]
stat = DiagnosticStatus(name=sphero_name, level=DiagnosticStatus.OK, message=sphero.power_state_msg)
#stat.message="Battery Status"
if sphero.power_state == 3:
stat.level=DiagnosticStatus.WARN
if sphero.power_state == 4:
stat.level=DiagnosticStatus.ERROR
diag.status.append(stat)
self.diag_pub.publish(diag)
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 _laptop_charge_to_diag(laptop_msg):
rv = DiagnosticStatus()
rv.level = DiagnosticStatus.OK
rv.message = 'OK'
rv.name = 'Laptop Battery'
rv.hardware_id = socket.gethostname()
if not laptop_msg.present:
rv.level = DiagnosticStatus.ERROR
rv.message = 'Laptop battery missing'
charging_state = {
SmartBatteryStatus.DISCHARGING: 'Not Charging',
SmartBatteryStatus.CHARGING: 'Charging',
SmartBatteryStatus.CHARGED: 'Trickle Charging'
}.get(laptop_msg.charge_state, 'Not Charging')
rv.values.append(KeyValue('Voltage (V)', str(laptop_msg.voltage)))
rv.values.append(KeyValue('Current (A)', str(laptop_msg.rate)))
rv.values.append(KeyValue('Charge (Ah)', str(laptop_msg.charge)))
rv.values.append(KeyValue('Capacity (Ah)', str(laptop_msg.capacity)))
rv.values.append(KeyValue('Design Capacity (Ah)', str(laptop_msg.design_capacity)))
rv.values.append(KeyValue('Charging State', str(charging_state)))
return rv
def 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_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 wifi_to_diag(msg):
stat = DiagnosticStatus()
stat.name = DIAG_NAME
stat.level = DiagnosticStatus.OK
stat.message = 'OK'
stat.values.append(KeyValue(key='ESSID', value=msg.essid))
stat.values.append(KeyValue(key='Mac Address', value=msg.macaddr))
stat.values.append(KeyValue(key='Signal', value=str(msg.signal)))
stat.values.append(KeyValue(key='Noise', value=str(msg.noise)))
stat.values.append(KeyValue(key='Sig/Noise', value=str(msg.snr)))
stat.values.append(KeyValue(key='Channel', value=str(msg.channel)))
stat.values.append(KeyValue(key='Rate', value=msg.rate))
stat.values.append(KeyValue(key='TX Power', value=msg.tx_power))
stat.values.append(KeyValue(key='Quality', value=str(msg.quality)))
return stat
def 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 cb_diagnostics_update(self):
tabpage_status = DiagnosticStatus()
tabpage_status.level = tabpage_status.OK
tabpage_status.name = self.handler_name + " Handler"
tabpage_status.hardware_id = self.ros_name
tabpage_status.message = "OK"
tabpage_status.values = []
tabpage_status.values.append(KeyValue(key="Number of Clients",
value=str(len(self.osc_clients))))
return tabpage_status
def mark_diag_stale(diag_stat = None, error = False):
if not diag_stat:
diag_stat = DiagnosticStatus()
diag_stat.message = 'No Updates'
diag_stat.name = DIAG_NAME
else:
diag_stat.message = 'Updates Stale'
diag_stat.level = DiagnosticStatus.WARN
if error:
diag_stat.level = DiagnosticStatus.ERROR
return diag_stat
def 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 getEtherbotixDiagnostics(self, etherbotix):
msg = DiagnosticStatus()
msg.name = "etherbotix"
msg.level = DiagnosticStatus.OK
msg.message = "OK"
msg.hardware_id = etherbotix.getUniqueId()
# System Voltage
if etherbotix.system_voltage < 10.0:
msg.level = DiagnosticStatus.ERROR
msg.message = "Battery depleted!"
msg.values.append(KeyValue("Voltage", str(etherbotix.system_voltage)+"V"))
# Currents
msg.values.append(KeyValue("Servo Current", str(etherbotix.servo_current)+"A"))
msg.values.append(KeyValue("Aux. Current", str(etherbotix.aux_current)+"A"))
msg.values.append(KeyValue("Packets", str(etherbotix.packets_recv)))
msg.values.append(KeyValue("Packets Bad", str(etherbotix.packets_bad)))
return msg
def _laptop_charge_to_diag(laptop_msg):
rv = DiagnosticStatus()
rv.level = DiagnosticStatus.OK
rv.message = 'OK'
rv.name = 'Laptop Battery'
rv.hardware_id = socket.gethostname()
if not laptop_msg.present:
rv.level = DiagnosticStatus.ERROR
rv.message = 'Laptop battery missing'
rv.values.append(KeyValue('Voltage (V)', str(laptop_msg.voltage)))
rv.values.append(KeyValue('Current (A)', str(laptop_msg.rate)))
rv.values.append(KeyValue('Charge (Ah)', str(laptop_msg.charge)))
rv.values.append(KeyValue('Capacity (Ah)', str(laptop_msg.capacity)))
rv.values.append(KeyValue('Design Capacity (Ah)', str(laptop_msg.design_capacity)))
return rv
def _ecat_diag(enc_errors = 0):
array = DiagnosticArray()
stat = DiagnosticStatus()
stat.name = 'EtherCAT Master'
stat.level = 0
stat.message = 'OK'
stat.values.append(KeyValue('Dropped Packets', '0'))
stat.values.append(KeyValue('RX Late Packet', '0'))
mcb_stat = DiagnosticStatus()
mcb_stat.name = 'EtherCAT Device (my_motor)'
mcb_stat.level = 0
mcb_stat.values.append(KeyValue('Num encoder_errors', str(enc_errors)))
array.header.stamp = rospy.get_rostime()
array.status.append(stat)
array.status.append(mcb_stat)
return array
def _laptop_charge_to_diag(laptop_msg):
rv = DiagnosticStatus()
rv.level = DiagnosticStatus.OK
rv.message = 'OK'
rv.name = rospy.get_name()
if rv.name.startswith('/'):
rv.name = rv.name[1:]
if not laptop_msg.present:
rv.level = DiagnosticStatus.ERROR
rv.message = 'Laptop battery missing'
rv.values.append(KeyValue('Voltage (V)', str(laptop_msg.voltage)))
rv.values.append(KeyValue('Current (A)', str(laptop_msg.rate)))
rv.values.append(KeyValue('Charge (Ah)', str(laptop_msg.charge)))
rv.values.append(KeyValue('Capacity (Ah)', str(laptop_msg.capacity)))
rv.values.append(KeyValue('Design Capacity (Ah)', str(laptop_msg.design_capacity)))
return rv
def cb_diagnostics_update(self):
"""
Callback periodically called to update the diagnostics status of the
tabpage handler.
@return: A status message for the tabpage handler
@rtype: C{diagnostic_msgs/DiagnosticStatus}
"""
tabpage_status = DiagnosticStatus()
tabpage_status.level = tabpage_status.OK
tabpage_status.name = " ".join([self.handler_name,
"Handler"])
tabpage_status.hardware_id = self.parent.ros_name
tabpage_status.message = "OK"
tabpage_status.values = []
return tabpage_status
