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 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 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 ntp_monitor(ntp_hostname, offset=500, self_offset=500):
try:
offset = int(offset)
self_offset = int(self_offset)
except:
parser.error("offset must be a number")
pub = rospy.Publisher("/diagnostics", DiagnosticArray)
rospy.init_node(NAME, anonymous=True)
hostname = socket.gethostname()
stat = DiagnosticStatus()
stat.level = 0
stat.name = "NTP offset from: "+ hostname + " to: " +ntp_hostname
stat.message = "Acceptable synchronization"
stat.hardware_id = hostname
stat.values = []
self_stat = DiagnosticStatus()
self_stat.level = 0
self_stat.name = "NTP offset from: "+ hostname + " to: self."
self_stat.message = "Acceptable synchronization"
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 = 0
st.message = "Acceptable synchronization"
st.values = [ KeyValue("offset (us)", str(measured_offset)) ]
if (abs(measured_offset) > off):
st.level = 2
st.message = "Offset too great"
else:
st.level = 2
st.message = "Error running ntpupdate"
st.values = [ KeyValue("offset (us)", "N/A") ]
pub.publish(DiagnosticArray(None, [stat, self_stat]))
time.sleep(1)
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 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 _camera_diag(level = 0):
array = DiagnosticArray()
stat = DiagnosticStatus()
stat.name = 'wge100: Driver Status'
stat.level = level
stat.message = 'OK'
motor_stat = DiagnosticStatus()
motor_stat.name = 'EtherCAT Master'
motor_stat.level = 0
motor_stat.values = [
KeyValue(key='Dropped Packets', value='0'),
KeyValue(key='RX Late Packet', value='0')]
mcb_stat = DiagnosticStatus()
mcb_stat.name = 'EtherCAT Device (my_motor)'
mcb_stat.level = 0
mcb_stat.values.append(KeyValue('Num encoder_errors', '0'))
array.header.stamp = rospy.get_rostime()
array.status.append(stat)
array.status.append(motor_stat)
array.status.append(mcb_stat)
return array
def 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 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 _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 _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 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 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 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 get_diagnostics_message(self):
message = DiagnosticStatus()
message.name = "ODrive report"
message.message = "ODrive diagnostics"
message.hardware_id = self.serial_no
if self.device is None:
message.level = 1
message.values = [KeyValue("connected", str(False))]
else:
message.values = [
KeyValue("connected", str(True)),
KeyValue("bus-voltage", str(self.device.vbus_voltage)),
KeyValue("axis0-error",
oenums.errors.axis[self.device.axis0.error]),
KeyValue(
"axis0-encoder-error",
oenums.errors.encoder[self.device.axis0.encoder.error]),
KeyValue("axis0-motor-error",
oenums.errors.motor[self.device.axis0.motor.error]),
# KeyValue("axis0-controller-error", oenums.errors.controller[self.device.axis0.controller.error]),
KeyValue(
"axis0-ctrl-mode", oenums.control_modes[
self.device.axis0.controller.config.control_mode]),
KeyValue("axis0-state",
oenums.axis_states[self.device.axis0.current_state]),
KeyValue("axis1-error",
oenums.errors.axis[self.device.axis1.error]),
KeyValue(
"axis1-encoder-error",
oenums.errors.encoder[self.device.axis1.encoder.error]),
KeyValue("axis1-motor-error",
oenums.errors.motor[self.device.axis1.motor.error]),
# KeyValue("axis1-controller-error", oenums.errors.controller[self.device.axis1.controller.error]),
KeyValue(
"axis1-ctrl-mode", oenums.control_modes[
self.device.axis1.controller.config.control_mode]),
KeyValue("axis1-state",
oenums.axis_states[self.device.axis1.current_state]),
]
# print(repr(message))
return message
def cb_diagnostics_update(self):
tabpage_status = DiagnosticStatus()
tabpage_status.level = tabpage_status.OK
tabpage_status.name = self.handler_name + " Handler"
tabpage_status.hardware_id = self.ros_name
tabpage_status.message = "OK"
tabpage_status.values = []
tabpage_status.values.append(
KeyValue(key="Number of Clients",
value=str(len(self.osc_clients))))
return tabpage_status
def publish_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 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 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 joint_to_diag(js):
global has_warned_invalid
ds = DiagnosticStatus()
ds.level = DiagnosticStatus.OK
ds.message = 'OK'
# Hack to stop gripper joints from being "uncalibrated"
if not js.is_calibrated and js.name.find("float") < 0 and js.name.find(
"finger") < 0:
ds.level = DiagnosticStatus.WARN
ds.message = 'Uncalibrated'
if check_nan and (math.isnan(js.position) or math.isnan(js.velocity)
or math.isnan(js.measured_effort)
or math.isnan(js.commanded_effort)):
ds.level = DiagnosticStatus.ERROR
ds.message = 'NaN in joint data'
if not has_warned_invalid:
rospy.logerr(
"NaN value for joint data. controller_manager restart required."
)
has_warned_invalid = True
if check_nan and (math.isinf(js.position) or math.isinf(js.velocity)
or math.isinf(js.measured_effort)
or math.isinf(js.commanded_effort)):
ds.level = DiagnosticStatus.ERROR
ds.message = 'Inf in joint data'
if not has_warned_invalid:
rospy.logerr(
"Infinite value for joint data. controller_manager restart required."
)
has_warned_invalid = True
ds.name = "Joint (%s)" % js.name
ds.values = [
KeyValue('Position', str(js.position)),
KeyValue('Velocity', str(js.velocity)),
KeyValue('Measured Effort', str(js.measured_effort)),
KeyValue('Commanded Effort', str(js.commanded_effort)),
KeyValue('Calibrated', str(js.is_calibrated)),
KeyValue('Odometer', str(js.odometer)),
KeyValue('Max Position', str(js.max_position)),
KeyValue('Min Position', str(js.min_position)),
KeyValue('Max Abs. Velocity', str(js.max_abs_velocity)),
KeyValue('Max Abs. Effort', str(js.max_abs_effort)),
KeyValue('Limits Hit', str(js.violated_limits))
]
return ds
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 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 publish_status(self):
stat = DiagnosticStatus()
stat.name = self.name
is_connect_ok = self.check_connection()
if is_connect_ok:
stat.hardware_id = str(self._info_data['serial'])
model = str(self._info_data['model'])
top_temp = self.convertTemp(
self._diag_data['volt_temp']['top']['lm20_temp'])
bot_temp = self.convertTemp(
self._diag_data['volt_temp']['bot']['lm20_temp'])
i_out = self.convertAmp(
self._diag_data['volt_temp']['bot']['i_out'])
v_in = self.convertVolt(
self._diag_data['volt_temp']['bot']['pwr_v_in'])
stat.values = [
KeyValue(key='ConnectionStatus', value='OK'),
KeyValue(key='Model', value=str(model)),
KeyValue(key='TopTemp[DegC]', value=str(round(top_temp, 3))),
KeyValue(key='BottomTemp[DegC]', value=str(round(bot_temp,
3))),
KeyValue(key='Iout[V]', value=str(round(i_out, 3))),
KeyValue(key='Vin[V]', value=str(round(v_in, 3)))
]
self.judge_risk_level(top_temp, bot_temp, i_out, v_in, stat)
else:
stat.level = DiagnosticStatus.ERROR
stat.hardware_id = 'Velodyne'
stat.message = 'Connection Lost ' + self._ip
stat.values = [KeyValue(key='ConnectionStatus', value='N/A')]
msg = DiagnosticArray()
msg.header.stamp = rospy.get_rostime()
msg.status.append(stat)
self._pub.publish(msg)
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):
"""
Callback periodically called to update the diagnostics status of the
tabpage handler.
@return: A status message for the tabpage handler
@rtype: C{diagnostic_msgs/DiagnosticStatus}
"""
tabpage_status = DiagnosticStatus()
tabpage_status.level = tabpage_status.OK
tabpage_status.name = " ".join([self.handler_name,
"Handler"])
tabpage_status.hardware_id = self.parent.ros_name
tabpage_status.message = "OK"
tabpage_status.values = []
return tabpage_status
def joint_to_diag(js):
d = DiagnosticStatus()
d.level = 0
d.message = ''
if not js.is_calibrated:
d.level = 1
d.message = 'UNCALIBRATED'
d.name = "Joint (%s)" % js.name
d.values = [
KeyValue('Position', str(js.position)),
KeyValue('Velocity', str(js.velocity)),
KeyValue('Applied Effort', str(js.applied_effort)),
KeyValue('Commanded Effort', str(js.commanded_effort)),
KeyValue('Calibrated', str(js.is_calibrated))
]
return d
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 reset(self):
"""Resets the environment to an initial state and returns an initial observation.
Note that this function should not reset the environment's random
number generator(s); random variables in the environment's state should
be sampled independently between multiple calls to `reset()`. In other
words, each call of `reset()` should yield an environment suitable for
a new episode, independent of previous episodes.
Returns:
observation (object): the initial observation.
"""
if self._has_state():
# generate log
info = {
'episode': self.__episode,
'net_reward': self.__net_reward,
'duration': (rospy.Time.now() - self.__start_time).to_sec()
}
info.update(self._get_state()[3])
# send message
msg = DiagnosticStatus()
msg.level = DiagnosticStatus.OK
msg.name = 'ROS-Gym Interface'
msg.message = 'log of episode data'
msg.hardware_id = self.__LOG_ID
msg.values = [
KeyValue(key=key, value=str(value))
for key, value in info.items()
]
self.__log_pub.publish(msg)
# update variables (update time after reset)
self.__episode += 1
self.__net_reward = 0
# reset
if not self.__EVAL_MODE:
self._reset_env()
self._reset_self()
self.__step_time_and_wait_for_state(5)
self.__start_time = rospy.Time.now() # logging
return self._get_state()[0]
def check_ok(self):
with self._mutex:
stat = 0 if self._ok else 2
msg = '' if self._ok else 'Dropped Packets'
if rospy.get_time() - self._update_time > 3:
stat = 3
msg = 'Packet Data Stale'
if self._update_time == -1:
msg = 'No Packet Data'
diag = DiagnosticStatus()
diag.name = 'EC Stats Packet Data'
diag.level = stat
diag.message = msg
if diag.level == 0:
diag.message = 'OK'
diag.values = [
KeyValue(key='Has Link?', value=str(self._has_link)),
KeyValue(key='Dropped Since Reset', value=str(self._total_dropped - self._drop_count_at_reset)),
KeyValue(key='Total Drops', value=str(self._total_dropped)),
KeyValue(key='Lost Link Count', value=str(self._lost_link_count)),
KeyValue(key='Lost Links Since Reset', value=str(self._lost_link_count_since_reset)),
KeyValue(key='Number of Resets', value=str(self._reset_count)),
KeyValue(key='Time Since Last Reset', value=str(rospy.get_time() - self._time_at_last_reset)),
KeyValue(key='Drops at Last Reset', value=str(self._drop_count_at_reset)),
KeyValue(key='Max Device Count', value=str(self._max_device_count)),
KeyValue(key='Interval Drops', value=str(self._interval_dropped)),
KeyValue(key='Total Late Packets', value=str(self._total_late)),
KeyValue(key='Interval Late Packets', value=str(self._interval_late)),
KeyValue(key='Total Sent Packets', value=str(self._total_sent)),
KeyValue(key='Interval Sent Packets', value=str(self._interval_sent)),
KeyValue(key='Total Bandwidth', value=str(self._total_bandwidth)),
KeyValue(key='Interval Bandwidth', value=str(self._interval_bandwidth))
]
return stat, msg, [ diag ]
def __init__(self, argv=sys.argv):
rospy.init_node("ntp_monitor")
self.parse_args(argv)
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 = []
self.msg = DiagnosticArray()
self.msg.header.stamp = rospy.get_rostime()
self.msg.status = [stat]
self.update_diagnostics()
self.diag_pub = rospy.Publisher("/diagnostics",
DiagnosticArray,
queue_size=1)
self.diag_timer = rospy.Timer(rospy.Duration(1.0),
self.publish_diagnostics)
self.monitor_timer = rospy.Timer(rospy.Duration(60.0),
self.update_diagnostics)
def publish(self, sensor_state, gyro):
curr_time = sensor_state.header.stamp
# limit to 5hz
if (curr_time - self.last_diagnostics_time).to_sec() < 0.2:
return
self.last_diagnostics_time = curr_time
diag = DiagnosticArray()
diag.header.stamp = curr_time
stat = DiagnosticStatus()
#node status
stat = DiagnosticStatus(name="TurtleBot Node",
level=DiagnosticStatus.OK,
message="RUNNING")
diag.status.append(stat)
#mode info
stat = DiagnosticStatus(name="Operating Mode",
level=DiagnosticStatus.OK)
try:
stat.message = self.oi_mode[sensor_state.oi_mode]
except KeyError as ex:
stat.level = DiagnosticStatus.ERROR
stat.message = "Invalid OI Mode Reported %s" % ex
rospy.logwarn(stat.message)
diag.status.append(stat)
#battery info
stat = DiagnosticStatus(name="Battery",
level=DiagnosticStatus.OK,
message="OK")
values = stat.values
if sensor_state.charging_state == 5:
stat.level = DiagnosticStatus.ERROR
stat.message = "Charging Fault Condition"
values.append(
KeyValue("Charging State",
self.charging_state[sensor_state.charging_state]))
values.extend([
KeyValue("Voltage (V)", str(sensor_state.voltage / 1000.0)),
KeyValue("Current (A)", str(sensor_state.current / 1000.0)),
KeyValue("Temperature (C)", str(sensor_state.temperature)),
KeyValue("Charge (Ah)", str(sensor_state.charge / 1000.0)),
KeyValue("Capacity (Ah)", str(sensor_state.capacity / 1000.0))
])
diag.status.append(stat)
#charging source
stat = DiagnosticStatus(name="Charging Sources",
level=DiagnosticStatus.OK)
try:
stat.message = self.charging_source[
sensor_state.charging_sources_available]
except KeyError as ex:
stat.level = DiagnosticStatus.ERROR
stat.message = "Invalid Charging Source %s, actual value: %i" % (
ex, sensor_state.charging_sources_available)
rospy.logwarn(stat.message)
diag.status.append(stat)
#cliff sensors
stat = DiagnosticStatus(name="Cliff Sensor",
level=DiagnosticStatus.OK,
message="OK")
if sensor_state.cliff_left or sensor_state.cliff_front_left or sensor_state.cliff_right or sensor_state.cliff_front_right:
stat.level = DiagnosticStatus.WARN
if (sensor_state.current / 1000.0) > 0:
stat.message = "Near Cliff: While the irobot create is charging, the create thinks it's near a cliff."
stat.level = DiagnosticStatus.OK # We're OK here
else:
stat.message = "Near Cliff"
stat.values = [
KeyValue("Left", str(sensor_state.cliff_left)),
KeyValue("Left Signal", str(sensor_state.cliff_left_signal)),
KeyValue("Front Left", str(sensor_state.cliff_front_left)),
KeyValue("Front Left Signal",
str(sensor_state.cliff_front_left_signal)),
KeyValue("Front Right", str(sensor_state.cliff_right)),
KeyValue("Front Right Signal",
str(sensor_state.cliff_right_signal)),
KeyValue("Right", str(sensor_state.cliff_front_right)),
KeyValue("Right Signal",
str(sensor_state.cliff_front_right_signal))
]
diag.status.append(stat)
#Wall sensors
stat = DiagnosticStatus(name="Wall Sensor",
level=DiagnosticStatus.OK,
message="OK")
#wall always seems to be false???
if sensor_state.wall:
stat.level = DiagnosticStatus.ERROR
stat.message = "Near Wall"
stat.values = [
KeyValue("Wall", str(sensor_state.wall)),
KeyValue("Wall Signal", str(sensor_state.wall_signal)),
KeyValue("Virtual Wall", str(sensor_state.virtual_wall))
]
diag.status.append(stat)
#Gyro
stat = DiagnosticStatus(name="Gyro Sensor",
level=DiagnosticStatus.OK,
message="OK")
if gyro is None:
stat.level = DiagnosticStatus.WARN
stat.message = "Gyro Not Enabled: To enable the gyro set the has_gyro param in the turtlebot_node."
elif gyro.cal_offset < 100:
stat.level = DiagnosticStatus.ERROR
stat.message = "Gyro Power Problem: For more information visit http://answers.ros.org/question/2091/turtlebot-bad-gyro-calibration-also."
elif gyro.cal_offset > 575.0 or gyro.cal_offset < 425.0:
stat.level = DiagnosticStatus.ERROR
stat.message = "Bad Gyro Calibration Offset: The gyro average is outside the standard deviation."
if gyro is not None:
stat.values = [
KeyValue("Gyro Enabled", str(gyro is not None)),
KeyValue("Raw Gyro Rate", str(sensor_state.user_analog_input)),
KeyValue("Calibration Offset", str(gyro.cal_offset)),
KeyValue("Calibration Buffer", str(gyro.cal_buffer))
]
diag.status.append(stat)
#Digital IO
stat = DiagnosticStatus(name="Digital Outputs",
level=DiagnosticStatus.OK,
message="OK")
out_byte = sensor_state.user_digital_outputs
stat.values = [
KeyValue("Raw Byte", str(out_byte)),
KeyValue("Digital Out 2", self.digital_outputs[out_byte % 2]),
KeyValue("Digital Out 1",
self.digital_outputs[(out_byte >> 1) % 2]),
KeyValue("Digital Out 0",
self.digital_outputs[(out_byte >> 2) % 2])
]
diag.status.append(stat)
#publish
self.diag_pub.publish(diag)
def __init__(self):
rospy.init_node('system_node', log_level=rospy.DEBUG)
# self.load_publisher = rospy.Publisher('~load', msgs.CPUUsage, queue_size=1)
# self.cpu_publisher = rospy.Publisher('~cpu', msgs.CPUUsage, queue_size=1)
self.memory_publisher = rospy.Publisher('~memory',
msgs.MemoryUsage,
queue_size=1)
self.disk_publisher = rospy.Publisher('~disk',
msgs.DiskUsage,
queue_size=1)
self.diagnostics_pub = rospy.Publisher('/diagnostics',
DiagnosticArray,
queue_size=10)
self.debug_level_sub = rospy.Subscriber('~debug_level', Int16,
self.on_debug_level)
self.debug_level = ONLY_ERRORS
self.drive_path = '/dev/root'
self.rate = 1 #float(rospy.get_param("~rate", 1)) # hertz
rospy.loginfo('Ready')
r = rospy.Rate(self.rate)
while not rospy.is_shutdown():
# Find normalized system load.
# The ideal normalized system load less than or equal 1.0, meaning the processor is getting no more than the maximum number of tasks it can handle.
proc_count = multiprocessing.cpu_count()
load_1, load_5, load_15 = os.getloadavg()
normalized_load_1 = load_1 / float(proc_count)
normalized_load_5 = load_5 / float(proc_count)
normalized_load_15 = load_15 / float(proc_count)
normalized_load_level = OK
if normalized_load_level >= 4.0:
normalized_load_level = ERROR
elif normalized_load_level >= 2.0:
normalized_load_level = WARN
# Find CPU.
cpu_usage_percent = to_percent(
getoutput(
"grep 'cpu ' /proc/stat | awk '{usage=($2+$4)*100/($2+$4+$5)} END {print usage }'"
))
cpu_usage_percent_level = OK
if cpu_usage_percent >= c.CPU_USAGE_PERCENT_ERROR:
cpu_usage_percent_level = ERROR
elif cpu_usage_percent >= c.CPU_USAGE_PERCENT_WARN:
cpu_usage_percent_level = WARN
# msg = msgs.CPUUsage()
# msg.header.stamp = rospy.Time.now()
# msg.percent_used = cpu_usage_percent
# self.cpu_publisher.publish(msg)
# Find memory.
memory_usage_free_gbytes = to_float(
getoutput("free -m|grep -i 'Mem:'|awk '{print $4}'"))
memory_usage_used_gbytes = to_float(
getoutput("free -m|grep -i 'Mem:'|awk '{print $3}'"))
memory_usage_total_gbytes = to_float(
getoutput("free -m|grep -i 'Mem:'|awk '{print $2}'"))
memory_usage_percent = -1
if memory_usage_used_gbytes != -1 and memory_usage_total_gbytes != -1:
memory_usage_percent = memory_usage_used_gbytes / memory_usage_total_gbytes * 100
memory_usage_percent_level = OK
if memory_usage_percent >= c.MEMORY_USAGE_PERCENT_ERROR:
memory_usage_percent_level = ERROR
elif memory_usage_percent >= c.MEMORY_USAGE_PERCENT_WARN:
memory_usage_percent_level = WARN
msg = msgs.MemoryUsage()
msg.header.stamp = rospy.Time.now()
msg.used_gbytes = memory_usage_used_gbytes
msg.free_gbytes = memory_usage_free_gbytes
msg.total_gbytes = memory_usage_total_gbytes
msg.percent_used = memory_usage_percent
self.memory_publisher.publish(msg)
# Find disk.
disk_usage_percent = to_percent(
getoutput("df -H | grep -i " + self.drive_path +
" | awk '{print $5}'"))
disk_usage_free_gbytes = to_gbytes(
getoutput("df -H | grep -i " + self.drive_path +
" | awk '{print $4}'"))
disk_usage_used_gbytes = to_gbytes(
getoutput("df -H | grep -i " + self.drive_path +
" | awk '{print $3}'"))
disk_usage_total_gbytes = to_gbytes(
getoutput("df -H | grep -i " + self.drive_path +
" | awk '{print $2}'"))
disk_usage_level = OK
if disk_usage_percent >= c.DISK_USAGE_PERCENT_ERROR:
disk_usage_level = ERROR
if disk_usage_percent >= c.DISK_USAGE_PERCENT_WARN:
disk_usage_level = WARN
msg = msgs.DiskUsage()
msg.header.stamp = rospy.Time.now()
msg.used_gbytes = disk_usage_used_gbytes
msg.free_gbytes = disk_usage_free_gbytes
msg.total_gbytes = disk_usage_total_gbytes
msg.percent_used = disk_usage_percent
self.disk_publisher.publish(msg)
# Find CPU clock speed.
min_ghz = get_cpu_clock_speed_min() / 1000. / 1000.
max_ghz = get_cpu_clock_speed_max() / 1000. / 1000.
cpu_clock_speed = get_cpu_clock_speed()
# print('cpu_clock_speed:', cpu_clock_speed)
cpu_clock_speed_percent = get_cpu_clock_speed_percent()
# print('cpu_clock_speed_percent:', cpu_clock_speed_percent)
cpu_clock_speed_percent_level = OK
# if cpu_clock_speed_percent <= c.CPU_CLOCK_SPEED_PERCENT_ERROR:
# cpu_clock_speed_percent_level = ERROR
# elif cpu_clock_speed_percent <= c.CPU_CLOCK_SPEED_PERCENT_WARN:
# cpu_clock_speed_percent_level = WARN
# Find CPU temperature.
cpu_temp = get_cpu_temp()
cpu_temp_level = OK
if cpu_temp >= c.CPU_TEMP_ERROR:
cpu_temp_level = ERROR
elif cpu_temp >= c.CPU_TEMP_WARN:
cpu_temp_level = WARN
# Publish standard diagnostics.
array = DiagnosticArray()
statuses = []
normalized_load_status = DiagnosticStatus(
name='Normalized System Load',
level=normalized_load_level,
message=str(normalized_load_15))
normalized_load_status.values = [
KeyValue(key='normalized load 1',
value=str(normalized_load_1)),
KeyValue(key='normalized load 5',
value=str(normalized_load_5)),
KeyValue(key='normalized load 15',
value=str(normalized_load_15)),
]
if self.debug_level >= UP_TO_OK or normalized_load_level != OK:
statuses.append(normalized_load_status)
cpu_temperature_status = DiagnosticStatus(name='CPU Temperature',
level=cpu_temp_level,
message=str(cpu_temp))
cpu_temperature_status.values = [
KeyValue(key='celcius', value=str(cpu_temp)),
]
if self.debug_level >= UP_TO_OK or cpu_temp >= c.CPU_TEMP_ERROR:
statuses.append(cpu_temperature_status)
cpu_usage_status = DiagnosticStatus(name='CPU Usage',
level=cpu_usage_percent_level,
message=str(cpu_usage_percent))
cpu_usage_status.values = [
KeyValue(key='percent', value=str(cpu_usage_percent)),
]
if self.debug_level >= UP_TO_OK or cpu_usage_percent_level != OK:
statuses.append(cpu_usage_status)
cpu_clock_speed_status = DiagnosticStatus(
name='CPU Speed',
level=cpu_clock_speed_percent_level,
message=str(cpu_clock_speed_percent))
cpu_clock_speed_status.values = [
KeyValue(key='clock speed (GHz)', value=str(cpu_clock_speed)),
KeyValue(key='min clock speed (GHz)', value=str(min_ghz)),
KeyValue(key='max clock speed (GHz)', value=str(max_ghz)),
KeyValue(key='clock speed (percent)',
value=str(cpu_clock_speed_percent)),
]
if self.debug_level >= UP_TO_OK or cpu_clock_speed_percent_level != OK:
statuses.append(cpu_clock_speed_status)
disk_usage_status = DiagnosticStatus(
name='Disk Usage',
level=disk_usage_level,
message=str(disk_usage_percent))
disk_usage_status.values = [
KeyValue(key='percent', value=str(disk_usage_percent)),
KeyValue(key='free gb', value=str(disk_usage_free_gbytes)),
KeyValue(key='used gb', value=str(disk_usage_used_gbytes)),
KeyValue(key='total gb', value=str(disk_usage_total_gbytes)),
]
if self.debug_level >= UP_TO_OK or disk_usage_level != OK:
statuses.append(disk_usage_status)
memory_usage_status = DiagnosticStatus(
name='Memory Usage',
level=memory_usage_percent_level,
message=str(memory_usage_percent))
memory_usage_status.values = [
KeyValue(key='percent', value=str(memory_usage_percent)),
KeyValue(key='free gb', value=str(memory_usage_free_gbytes)),
KeyValue(key='used gb', value=str(memory_usage_used_gbytes)),
KeyValue(key='total gb', value=str(memory_usage_total_gbytes)),
]
if self.debug_level >= UP_TO_OK or memory_usage_percent_level != OK:
statuses.append(memory_usage_status)
if statuses:
array.status = statuses
# [
# normalized_load_status,
# cpu_temperature_status,
# cpu_usage_status,
# cpu_clock_speed_status,
# disk_usage_status,
# memory_usage_status,
# ]
self.diagnostics_pub.publish(array)
r.sleep()
def update(self, mech_state):
self._rx_count += 1
diag = DiagnosticStatus()
diag.level = 0 # Default the level to 'OK'
diag.name = "Trans. Monitor: %s" % self._joint
diag.message = "OK"
diag.values = []
diag.values.append(KeyValue("Joint", self._joint))
diag.values.append(KeyValue("Actuator", self._actuator))
diag.values.append(KeyValue("Up position", str(self._up_ref)))
diag.values.append(KeyValue("Down position", str(self._down_ref)))
diag.values.append(KeyValue("Wrap", str(self._wrap)))
diag.values.append(KeyValue("Max Limit", str(self._max)))
diag.values.append(KeyValue("Min Limit", str(self._min)))
diag.values.append(KeyValue("Deadband", str(self._deadband)))
diag.values.append(KeyValue("Mech State RX Count",
str(self._rx_count)))
# Check if we can find both the joint and actuator
act_names = [x.name for x in mech_state.actuator_states]
act_exists = self._actuator in act_names
act_exists_msg = 'Error'
if act_exists:
cal_reading = mech_state.actuator_states[act_names.index(
self._actuator)].calibration_reading
joint_names = [x.name for x in mech_state.joint_states]
joint_exists = self._joint in joint_names
jnt_exists_msg = 'Error'
if joint_exists:
position = mech_state.joint_states[joint_names.index(
self._joint)].position
calibrated = (mech_state.joint_states[joint_names.index(
self._joint)].is_calibrated == 1)
diag.values.append(KeyValue('Actuator Exists', str(act_exists)))
diag.values.append(KeyValue('Joint Exists', str(joint_exists)))
# First check existance of joint, actuator
if not (act_exists and joint_exists):
diag.level = 2
diag.message = 'Actuators, Joints missing'
self._ok = False
return diag, False
# Monitor current state
reading_msg = 'OK'
if not self.check_device(position, cal_reading, calibrated):
self._ok = False
self._num_errors += 1
self._num_errors_since_reset += 1
reading_msg = 'Broken'
# If we've had an error since the last reset, we're no good
if not self._ok:
diag.message = 'Broken'
diag.level = 2
diag.values.insert(0, KeyValue('Transmission Status', diag.message))
diag.values.insert(1, KeyValue('Current Reading', reading_msg))
diag.values.append(KeyValue('Is Calibrated', str(calibrated)))
diag.values.append(KeyValue('Calibration Reading', str(cal_reading)))
diag.values.append(KeyValue('Joint Position', str(position)))
diag.values.append(KeyValue('Total Errors', str(self._num_errors)))
diag.values.append(
KeyValue('Errors Since Reset', str(self._num_errors_since_reset)))
self._max_position = max(self._max_position, position)
diag.values.append(
KeyValue('Max Obs. Position', str(self._max_position)))
self._min_position = min(self._min_position, position)
diag.values.append(
KeyValue('Min Obs. Position', str(self._min_position)))
return self._ok, diag
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))
getTracking(status)
getSources(status)
pub.publish(array)
rate.sleep()
def publish(self, sensor_state, gyro):
curr_time = sensor_state.header.stamp
# limit to 5hz
if (curr_time - self.last_diagnostics_time).to_sec() < 0.2:
return
self.last_diagnostics_time = curr_time
diag = DiagnosticArray()
diag.header.stamp = curr_time
stat = DiagnosticStatus()
#node status
stat = DiagnosticStatus(name="TurtleBot Node", level=DiagnosticStatus.OK, message="RUNNING")
diag.status.append(stat)
#mode info
stat = DiagnosticStatus(name="Operating Mode", level=DiagnosticStatus.OK)
try:
stat.message = self.oi_mode[sensor_state.oi_mode]
except KeyError as ex:
stat.message = "Invalid OI Mode %s"%ex
rospy.logwarn("Invalid OI Mode %s"%ex)
diag.status.append(stat)
#battery info
stat = DiagnosticStatus(name="Battery", level=DiagnosticStatus.OK, message="OK")
values = stat.values
if sensor_state.charging_state == 5:
stat.level = DiagnosticStatus.ERROR
stat.message = "Charging Fault Condition"
values.append(KeyValue("Charging State", self.charging_state[sensor_state.charging_state]))
values.extend([KeyValue("Voltage (V)", str(sensor_state.voltage/1000.0)),
KeyValue("Current (A)", str(sensor_state.current/1000.0)),
KeyValue("Temperature (C)",str(sensor_state.temperature)),
KeyValue("Charge (Ah)", str(sensor_state.charge/1000.0)),
KeyValue("Capacity (Ah)", str(sensor_state.capacity/1000.0))])
diag.status.append(stat)
#charging source
stat = DiagnosticStatus(name="Charging Sources", level=DiagnosticStatus.OK)
try:
stat.message = self.charging_source[sensor_state.charging_sources_available]
except KeyError as ex:
stat.message = "Invalid Charging Source %s, actual value: %i"%(ex,sensor_state.charging_sources_available)
rospy.logwarn("Invalid Charging Source %s, actual value: %i"%(ex,sensor_state.charging_sources_available))
diag.status.append(stat)
#cliff sensors
stat = DiagnosticStatus(name="Cliff Sensor", level=DiagnosticStatus.OK, message="OK")
if sensor_state.cliff_left or sensor_state.cliff_front_left or sensor_state.cliff_right or sensor_state.cliff_front_right:
stat.level = DiagnosticStatus.WARN
if (sensor_state.current/1000.0)>0:
stat.message = "Near Cliff: While the irobot create is charging, the create thinks it's near a cliff."
else:
stat.message = "Near Cliff"
stat.values = [KeyValue("Left", str(sensor_state.cliff_left)),
KeyValue("Left Signal", str(sensor_state.cliff_left_signal)),
KeyValue("Front Left", str(sensor_state.cliff_front_left)),
KeyValue("Front Left Signal", str(sensor_state.cliff_front_left_signal)),
KeyValue("Front Right", str(sensor_state.cliff_right)),
KeyValue("Front Right Signal", str(sensor_state.cliff_right_signal)),
KeyValue("Right", str(sensor_state.cliff_front_right)),
KeyValue("Right Signal", str(sensor_state.cliff_front_right_signal))]
diag.status.append(stat)
#Wall sensors
stat = DiagnosticStatus(name="Wall Sensor", level=DiagnosticStatus.OK, message="OK")
#wall always seems to be false???
if sensor_state.wall:
stat.level = DiagnosticStatus.ERROR
stat.message = "Near Wall"
stat.values = [KeyValue("Wall", str(sensor_state.wall)),
KeyValue("Wall Signal", str(sensor_state.wall_signal)),
KeyValue("Virtual Wall", str(sensor_state.virtual_wall))]
diag.status.append(stat)
#Gyro
stat = DiagnosticStatus(name="Gyro Sensor", level = DiagnosticStatus.OK, message = "OK")
if gyro is None:
stat.level = DiagnosticStatus.WARN
stat.message = "Gyro Not Enabled: To enable the gyro set the has_gyro param in the turtlebot_node."
elif gyro.cal_offset < 100:
stat.level = DiagnosticStatus.ERROR
stat.message = "Gyro Power Problem: For more information visit http://answers.ros.org/question/2091/turtlebot-bad-gyro-calibration-also."
elif gyro.cal_offset > 575.0 or gyro.cal_offset < 425.0:
stat.level = DiagnosticStatus.ERROR
stat.message = "Bad Gyro Calibration Offset: The gyro average is outside the standard deviation."
if gyro is not None:
stat.values = [KeyValue("Gyro Enabled", str(gyro is not None)),
KeyValue("Raw Gyro Rate", str(sensor_state.user_analog_input)),
KeyValue("Calibration Offset", str(gyro.cal_offset)),
KeyValue("Calibration Buffer", str(gyro.cal_buffer))]
diag.status.append(stat)
#Digital IO
stat = DiagnosticStatus(name="Digital Outputs", level = DiagnosticStatus.OK, message = "OK")
out_byte = sensor_state.user_digital_outputs
stat.values = [KeyValue("Raw Byte", str(out_byte)),
KeyValue("Digital Out 2", self.digital_outputs[out_byte%2]),
KeyValue("Digital Out 1", self.digital_outputs[(out_byte >>1)%2]),
KeyValue("Digital Out 0", self.digital_outputs[(out_byte >>2)%2])]
diag.status.append(stat)
#publish
self.diag_pub.publish(diag)
def publish(self, sensor_state):
curr_time = sensor_state.header.stamp
# limit to 5hz
if (curr_time - self.last_diagnostics_time).to_sec() < 0.2:
return
self.last_diagnostics_time = curr_time
diag = DiagnosticArray()
diag.header.stamp = curr_time
stat = DiagnosticStatus()
#node status
stat = DiagnosticStatus(name="Robo50 Node", level=DiagnosticStatus.OK, message="RUNNING")
diag.status.append(stat)
#battery info
# stat = DiagnosticStatus(name="Battery", level=DiagnosticStatus.OK, message="OK")
# values = stat.values
# if sensor_state.charging_state == 5:
# stat.level = DiagnosticStatus.ERROR
# stat.message = "Charging Fault Condition"
# values.append(KeyValue("Charging State", self.charging_state[sensor_state.charging_state]))
# values.extend([KeyValue("Voltage (V)", str(sensor_state.voltage/1000.0)),
# KeyValue("Current (A)", str(sensor_state.current/1000.0)),
# KeyValue("Temperature (C)",str(sensor_state.temperature)),
# KeyValue("Charge (Ah)", str(sensor_state.charge/1000.0)),
# KeyValue("Capacity (Ah)", str(sensor_state.capacity/1000.0))])
# diag.status.append(stat)
# #cliff sensors
# stat = DiagnosticStatus(name="Cliff Sensor", level=DiagnosticStatus.OK, message="OK")
# if sensor_state.cliff_left or sensor_state.cliff_front_left or sensor_state.cliff_right or sensor_state.cliff_front_right:
# stat.level = DiagnosticStatus.WARN
# if (sensor_state.current/1000.0)>0:
# stat.message = "Near Cliff: While the irobot create is charging, the create thinks it's near a cliff."
# stat.level = DiagnosticStatus.OK # We're OK here
# else:
# stat.message = "Near Cliff"
# stat.values = [KeyValue("Left", str(sensor_state.cliff_left)),
# KeyValue("Left Signal", str(sensor_state.cliff_left_signal)),
# KeyValue("Front Left", str(sensor_state.cliff_front_left)),
# KeyValue("Front Left Signal", str(sensor_state.cliff_front_left_signal)),
# KeyValue("Front Right", str(sensor_state.cliff_right)),
# KeyValue("Front Right Signal", str(sensor_state.cliff_right_signal)),
# KeyValue("Right", str(sensor_state.cliff_front_right)),
# KeyValue("Right Signal", str(sensor_state.cliff_front_right_signal))]
# diag.status.append(stat)
# #Wall sensors
# stat = DiagnosticStatus(name="Wall Sensor", level=DiagnosticStatus.OK, message="OK")
# #wall always seems to be false???
# if sensor_state.wall:
# stat.level = DiagnosticStatus.ERROR
# stat.message = "Near Wall"
# stat.values = [KeyValue("Wall", str(sensor_state.wall)),
# KeyValue("Wall Signal", str(sensor_state.wall_signal)),
# KeyValue("Virtual Wall", str(sensor_state.virtual_wall))]
# diag.status.append(stat)
#Gyro
# stat = DiagnosticStatus(name="Gyro Sensor", level = DiagnosticStatus.OK, message = "OK")
# if gyro is None:
# stat.level = DiagnosticStatus.WARN
# stat.message = "Gyro Not Enabled: To enable the gyro set the has_gyro param in the Robo50_node."
# elif gyro.cal_offset < 100:
# stat.level = DiagnosticStatus.ERROR
# stat.message = "Gyro Power Problem: For more information visit http://answers.ros.org/question/2091/Robo50-bad-gyro-calibration-also."
# elif gyro.cal_offset > 575.0 or gyro.cal_offset < 425.0:
# stat.level = DiagnosticStatus.ERROR
# stat.message = "Bad Gyro Calibration Offset: The gyro average is outside the standard deviation."
#
# if gyro is not None:
# stat.values = [KeyValue("Gyro Enabled", str(gyro is not None)),
# KeyValue("Raw Gyro Rate", str(sensor_state.user_analog_input)),
# KeyValue("Calibration Offset", str(gyro.cal_offset)),
# KeyValue("Calibration Buffer", str(gyro.cal_buffer))]
# diag.status.append(stat)
#IMU
stat = DiagnosticStatus(name="IMU Sensor", level = DiagnosticStatus.OK, message = "OK")
stat.values = [KeyValue("Compass Heading", str(sensor_state.imu.heading)),
KeyValue("Gyro", str(sensor_state.imu.gyro)),
KeyValue("Accelero", str(sensor_state.imu.accelero))]
diag.status.append(stat)
#Encoder
stat = DiagnosticStatus(name="Encoder Counts", level = DiagnosticStatus.OK, message = "OK")
stat.values = [KeyValue("Left Encoder", str(sensor_state.encoder_counts_left)),
KeyValue("Right Encoder", str(sensor_state.encoder_counts_right)),
KeyValue("Distance Left", str(sensor_state.distance_left)),
KeyValue("Distance Right", str(sensor_state.distance_right)),
KeyValue("Distance", str(sensor_state.distance)),
KeyValue("Angle", str(sensor_state.angle))]
diag.status.append(stat)
#publish
self.diag_pub.publish(diag)
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) )
getTracking(status)
getSources(status)
pub.publish(array)
rate.sleep()
pub = rospy.Publisher("/diagnostics", DiagnosticArray, queue_size=10)
rospy.Subscriber("hw_monitor/diagnostics/motor_stall_l", Bool, motor_stall_l_callback)
rospy.Subscriber("hw_monitor/diagnostics/motor_stall_r", Bool, motor_stall_r_callback)
rospy.Subscriber("hw_monitor/diagnostics/batt_low", Bool, batt_low_callback)
rospy.Subscriber("hw_monitor/diagnostics/batt_high", Bool, batt_high_callback)
rospy.Subscriber("hw_monitor/diagnostics/controller", Bool, controller_callback)
rospy.Subscriber("hw_monitor/diagnostics/aux_lights", Bool, aux_lights_callback)
array = DiagnosticArray()
my_rate = rospy.Rate(1.0)
while not rospy.is_shutdown():
motor_l_stat = DiagnosticStatus(name="Motor Left", level=0, message="Running")
motor_l_stat.values = [KeyValue(key="Stall", value="False"), KeyValue(key="Controller Halt", value="False")]
motor_r_stat = DiagnosticStatus(name="Motor Right", level=0, message="Running")
motor_r_stat.values = [KeyValue(key="Stall", value="False"), KeyValue(key="Controller Halt", value="False")]
if controller == True:
motor_l_stat = DiagnosticStatus(name="Motor Left", level=0, message="Halted")
motor_l_stat.values = [KeyValue(key="Stall", value="False"), KeyValue(key="Controller Halt", value="True")]
motor_r_stat = DiagnosticStatus(name="Motor Right", level=0, message="Halted")
motor_r_stat.values = [KeyValue(key="Stall", value="False"), KeyValue(key="Controller Halt", value="True")]
if stall_l == True:
motor_l_stat = DiagnosticStatus(name="Motor Left", level=0, message="Stalled")
motor_l_stat.values = [KeyValue(key="Stall", value="True"), KeyValue(key="Controller Halt", value="False")]
if stall_l == True & controller == True:
from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
from time import sleep
def callback(msg):
print("Got msg")
return EmptyResponse()
if __name__ == '__main__':
rospy.init_node("test_node")
rospy.Service('self_test', Empty, callback)
pub = rospy.Publisher('/diagnostics', DiagnosticArray)
# Publish diagnostics to check runtime monitor, rxconsole
msg = DiagnosticArray()
stat = DiagnosticStatus()
stat.level = 0
stat.name = 'Test Node'
stat.message = 'OK'
stat.hardware_id = 'HW ID'
stat.values = [ KeyValue('Node Status', 'OK') ]
msg.status.append(stat)
while not rospy.is_shutdown():
msg.header.stamp = rospy.get_rostime()
pub.publish(msg)
rospy.loginfo('Test node is printing things')
sleep(1.0)
def ntp_monitor(namespace,
offset=500,
self_offset=500,
diag_hostname=None,
error_offset=5000000):
rospy.init_node(NAME, anonymous=True)
diag_updater = DiagnosticUpdater(
name=namespace + 'ntp',
display_name=diag_hostname + ' NTP',
)
hostname = socket.gethostname()
if diag_hostname is None:
diag_hostname = hostname
ntp_hostname = rospy.get_param('~reference_host', 'ntp.ubuntu.com')
offset = rospy.get_param('~offset_tolerance', 500)
error_offset = rospy.get_param('~error_offset_tolerance', 5000000)
stat = DiagnosticStatus()
stat.level = 0
stat.name = "NTP Offset"
stat.message = "OK"
stat.hardware_id = hostname
stat.values = []
stat_diagnostic = GenericDiagnostic('/offset')
stat_diagnostic.add_to_updater(diag_updater)
# 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)]:
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:
# Warning (not error), since this is a non-critical failure.
st.level = DiagnosticStatus.WARN
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)
]
# Convert from ROS diagnostics to mbot_diagnostics for publishing.
stat_diagnostic.set_status(Status(stat.level), stat.message)
for diag_val in stat.values:
stat_diagnostic.set_metric(diag_val.key, diag_val.value)
time.sleep(1)
def cb_diagnostics_update(self):
"""
Callback periodically called to update the diagnostics status of the
TouchOscInterface.
"""
msg = DiagnosticArray()
msg.header.stamp = rospy.Time.now()
msg.status = []
# If the callbacks DiagnosticStatus message hasn't been populated,
# do that now.
if not self._diagnostic_status_callbacks:
callback_status = DiagnosticStatus()
callback_status.level = callback_status.OK
callback_status.name = " ".join([self.ros_name,
"Registered Callbacks"])
callback_status.hardware_id = self.ros_name
callback_status.message = "OK"
callback_status.values = []
diags = [(k, v) for k, v in walk_node(self._osc_receiver)]
rospy.logdebug("Registered Callbacks:")
for (k, v) in diags:
rospy.logdebug('{0:<30}{1:<30}'.format(k, v))
callback_status.values.append(KeyValue(key=k, value=v))
self._diagnostic_status_callbacks = callback_status
msg.status.append(self._diagnostic_status_callbacks)
# Populate the clients DiagnosticStatus message
diagnostic_status_clients = DiagnosticStatus()
diagnostic_status_clients.level = diagnostic_status_clients.OK
diagnostic_status_clients.name = " ".join([self.ros_name,
"Client Status"])
diagnostic_status_clients.hardware_id = self.ros_name
diagnostic_status_clients.message = "Listening on %d" % self.osc_port
diagnostic_status_clients.values = []
#TODO: The clients property accessor is not thread-safe, as far as I can
#tell, but using a lock tends to make bonjour hang.
clients = self.clients
for client in clients.itervalues():
diagnostic_status_clients.values.append(KeyValue(
key=client.address,
value=client.servicename.split("[")[0]))
diagnostic_status_clients.values.append(KeyValue(
key=client.address + " Type",
value=client.client_type))
diagnostic_status_clients.values.append(KeyValue(
key=client.address + " Current",
value=client.active_tabpage))
diagnostic_status_clients.values.append(KeyValue(
key=client.address + " Tabpages",
value=", ".join(client.tabpages)))
if len(self.clients) == 0:
diagnostic_status_clients.message = "No clients detected"
msg.status.append(diagnostic_status_clients)
# For each registered tabpage handler, get a DiagnosticStatus message.
for tabpage in self.tabpage_handlers.itervalues():
msg.status.append(tabpage.cb_diagnostics_update())
# Publish
self.diagnostics_pub.publish(msg)
reactor.callLater(1.0, self.cb_diagnostics_update)
def publish_diagnostics_info(self):
"""
Publishes at a rate of 1Hz ( because thats the rate real kobuki does it )
For this first version we only publish battery status.
:return:
"""
battery_charge = self.get_simulated_battery_status()
msg = DiagnosticArray()
info_msg = DiagnosticStatus()
header = Header()
header.frame_id = ""
header.stamp = rospy.Time.now()
msg.header = header
msg.status.append(info_msg)
values = []
percent_value = KeyValue()
percent_value.key = "Percent"
percent_value.value = str(battery_charge)
# TODO: Add all the other values
voltage_value = KeyValue()
voltage_value.key = "Voltage (V)"
charge_value = KeyValue()
charge_value.key = "Charge (Ah)"
capacity_value = KeyValue()
capacity_value.key = "Capacity (Ah)"
source_value = KeyValue()
source_value.key = "Source"
charging_state_value = KeyValue()
charging_state_value.key = "Charging State"
current_value = KeyValue()
current_value.key = "Current (A)"
if battery_charge <= 10.0:
level = DiagnosticStatus.ERROR
message = "Empty Battery"
elif 10.0 < battery_charge <= 20.0:
level = DiagnosticStatus.WARN
message = "Warning Low Battery"
elif battery_charge >= 100.0:
level = DiagnosticStatus.OK
message = "Maximum"
else:
level = DiagnosticStatus.OK
message = "NormalLevel"
info_msg.name = "mobile_base_nodelet_manager: Battery"
info_msg.hardware_id = "Kobuki"
info_msg.message = message
info_msg.level = level
values.append(percent_value)
values.append(voltage_value)
values.append(charge_value)
values.append(capacity_value)
values.append(source_value)
values.append(charging_state_value)
values.append(current_value)
info_msg.values = values
msg.status.append(info_msg)
self._pub.publish(msg)
def cb_diagnostics_update(self):
"""
Callback periodically called to update the diagnostics status of the
TouchOscInterface.
"""
msg = DiagnosticArray()
msg.header.stamp = rospy.Time.now()
msg.status = []
# If the callbacks DiagnosticStatus message hasn't been populated,
# do that now.
if not self._diagnostic_status_callbacks:
callback_status = DiagnosticStatus()
callback_status.level = callback_status.OK
callback_status.name = " ".join([self.ros_name,
"Registered Callbacks"])
callback_status.hardware_id = self.ros_name
callback_status.message = "OK"
callback_status.values = []
diags = [(k, v) for k, v in walk_node(self._osc_receiver)]
rospy.logdebug("Registered Callbacks:")
for (k, v) in diags:
rospy.logdebug('{0:<30}{1:<30}'.format(k, v))
callback_status.values.append(KeyValue(key=k, value=v))
self._diagnostic_status_callbacks = callback_status
msg.status.append(self._diagnostic_status_callbacks)
# Populate the clients DiagnosticStatus message
diagnostic_status_clients = DiagnosticStatus()
diagnostic_status_clients.level = diagnostic_status_clients.OK
diagnostic_status_clients.name = " ".join([self.ros_name,
"Client Status"])
diagnostic_status_clients.hardware_id = self.ros_name
diagnostic_status_clients.message = "Listening on %d" % self.osc_port
diagnostic_status_clients.values = []
#TODO: The clients property accessor is not thread-safe, as far as I can
#tell, but using a lock tends to make bonjour hang.
clients = self.clients
for client in clients.itervalues():
diagnostic_status_clients.values.append(KeyValue(
key=client.address,
value=client.servicename.split("[")[0]))
diagnostic_status_clients.values.append(KeyValue(
key=client.address + " Type",
value=client.client_type))
diagnostic_status_clients.values.append(KeyValue(
key=client.address + " Current",
value=client.active_tabpage))
diagnostic_status_clients.values.append(KeyValue(
key=client.address + " Tabpages",
value=", ".join(client.tabpages)))
if len(self.clients) == 0:
diagnostic_status_clients.message = "No clients detected"
msg.status.append(diagnostic_status_clients)
# For each registered tabpage handler, get a DiagnosticStatus message.
for tabpage in self.tabpage_handlers.itervalues():
msg.status.append(tabpage.cb_diagnostics_update())
# Publish
self.diagnostics_pub.publish(msg)
reactor.callLater(1.0, self.cb_diagnostics_update)
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 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(self, sensor_state, gyro):
curr_time = sensor_state.header.stamp
# limit to 5hz
if (curr_time - self.last_diagnostics_time).to_sec() < 0.2:
return
self.last_diagnostics_time = curr_time
diag = DiagnosticArray()
diag.header.stamp = curr_time
stat = DiagnosticStatus()
#node status
stat = DiagnosticStatus(name="WheeledRobin Node", level=DiagnosticStatus.OK, message="RUNNING")
diag.status.append(stat)
#mode info
"""
stat = DiagnosticStatus(name="Operating Mode", level=DiagnosticStatus.OK)
try:
stat.message = self.mode[sensor_state.mode]
except KeyError as ex:
stat.level=DiagnosticStatus.ERROR
stat.message = "Invalid Mode Reported %s"%ex
rospy.logwarn(stat.message)
diag.status.append(stat)
"""
#cliff sensors
"""
stat = DiagnosticStatus(name="Cliff Sensor", level=DiagnosticStatus.OK, message="OK")
if sensor_state.cliff_left or sensor_state.cliff_front_left or sensor_state.cliff_right or sensor_state.cliff_front_right:
stat.level = DiagnosticStatus.WARN
if (sensor_state.current/1000.0)>0:
stat.message = "Near Cliff: While the irobot create is charging, the create thinks it's near a cliff."
stat.level = DiagnosticStatus.OK # We're OK here
else:
stat.message = "Near Cliff"
stat.values = [KeyValue("Left", str(sensor_state.cliff_left)),
KeyValue("Left Signal", str(sensor_state.cliff_left_signal)),
KeyValue("Front Left", str(sensor_state.cliff_front_left)),
KeyValue("Front Left Signal", str(sensor_state.cliff_front_left_signal)),
KeyValue("Front Right", str(sensor_state.cliff_right)),
KeyValue("Front Right Signal", str(sensor_state.cliff_right_signal)),
KeyValue("Right", str(sensor_state.cliff_front_right)),
KeyValue("Right Signal", str(sensor_state.cliff_front_right_signal))]
diag.status.append(stat)
#Wall sensors
stat = DiagnosticStatus(name="Wall Sensor", level=DiagnosticStatus.OK, message="OK")
#wall always seems to be false???
if sensor_state.wall:
stat.level = DiagnosticStatus.ERROR
stat.message = "Near Wall"
stat.values = [KeyValue("Wall", str(sensor_state.wall)),
KeyValue("Wall Signal", str(sensor_state.wall_signal)),
KeyValue("Virtual Wall", str(sensor_state.virtual_wall))]
diag.status.append(stat)
#Gyro
stat = DiagnosticStatus(name="Gyro Sensor", level = DiagnosticStatus.OK, message = "OK")
if gyro is None:
stat.level = DiagnosticStatus.WARN
stat.message = "Gyro Not Enabled: To enable the gyro set the has_gyro param in the wheeled_robin_node."
elif gyro.cal_offset < 100:
stat.level = DiagnosticStatus.ERROR
stat.message = "Gyro Power Problem: For more information visit http://answers.ros.org/question/2091/turtlebot-bad-gyro-calibration-also."
elif gyro.cal_offset > 575.0 or gyro.cal_offset < 425.0:
stat.level = DiagnosticStatus.ERROR
stat.message = "Bad Gyro Calibration Offset: The gyro average is outside the standard deviation."
if gyro is not None:
stat.values = [KeyValue("Gyro Enabled", str(gyro is not None)),
KeyValue("Raw Gyro Rate", str(sensor_state.user_analog_input)),
KeyValue("Calibration Offset", str(gyro.cal_offset)),
KeyValue("Calibration Buffer", str(gyro.cal_buffer))]
diag.status.append(stat)
"""
#Digital Outputs
stat = DiagnosticStatus(name="Digital Outputs", level = DiagnosticStatus.OK, message = "OK")
out_byte = sensor_state.digital_outputs
stat.values = [KeyValue("Raw Byte", str(out_byte)),
KeyValue("Digital Out 0", self.digital_outputs[(out_byte >>0)%2]),
KeyValue("Digital Out 1", self.digital_outputs[(out_byte >>1)%2]),
KeyValue("Digital Out 2", self.digital_outputs[(out_byte >>2)%2]),
KeyValue("Digital Out 3", self.digital_outputs[(out_byte >>3)%2]),
KeyValue("Digital Out 4", self.digital_outputs[(out_byte >>4)%2]),
KeyValue("Digital Out 5", self.digital_outputs[(out_byte >>5)%2]),
KeyValue("Digital Out 6", self.digital_outputs[(out_byte >>6)%2]),
KeyValue("Digital Out 7", self.digital_outputs[(out_byte >>7)%2])]
diag.status.append(stat)
#Digital Inputs
stat = DiagnosticStatus(name="Digital Inputs", level = DiagnosticStatus.OK, message = "OK")
in_byte = sensor_state.digital_inputs
stat.values = [KeyValue("Raw Byte", str(in_byte)),
KeyValue("Digital Input 0", self.digital_outputs[(in_byte >>0)%2]),
KeyValue("Digital Input 1", self.digital_outputs[(in_byte >>1)%2]),
KeyValue("Digital Input 2", self.digital_outputs[(in_byte >>2)%2]),
KeyValue("Digital Input 3", self.digital_outputs[(in_byte >>3)%2]),
KeyValue("Digital Input 4", self.digital_outputs[(in_byte >>4)%2]),
KeyValue("Digital Input 5", self.digital_outputs[(in_byte >>5)%2]),
KeyValue("Digital Input 6", self.digital_outputs[(in_byte >>6)%2]),
KeyValue("Digital Input 7", self.digital_outputs[(in_byte >>7)%2])]
diag.status.append(stat)
#publish
self.diag_pub.publish(diag)