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 publish_diagnostic(self):
try:
da = DiagnosticArray()
ds = DiagnosticStatus()
ds.name = rospy.get_caller_id().lstrip('/') + ': Status'
ds.hardware_id = self.ifname
if not self.iwconfig.is_enabled():
ds.level = DiagnosticStatus.STALE
ds.message = "Device not found"
elif not self.iwconfig.is_connected():
ds.level = DiagnosticStatus.ERROR
ds.message = "No connection"
else:
if extract_number(self.iwconfig.status["Link Quality"]) < self.warn_quality:
ds.level = DiagnosticStatus.WARN
ds.message = "Connected, but bad quality"
else:
ds.level = DiagnosticStatus.OK
ds.message = "Connected"
for key, val in self.iwconfig.status.items():
ds.values.append(KeyValue(key, val))
da.status.append(ds)
da.header.stamp = rospy.Time.now()
self.diagnostic_pub.publish(da)
except Exception as e:
rospy.logerr('Failed to publish diagnostic: %s' % str(e))
rospy.logerr(traceback.format_exc())
def diagnosticsCallback (event):
array = DiagnosticArray()
# Set the timestamp for diagnostics
array.header.stamp = rospy.Time.now()
motors_message = DiagnosticStatus(name = 'PhidgetMotors', level = 0,message = 'initialized', hardware_id='Phidget')
if (motorsError == 1):
motors_message.level = 2
motors_message.message = "Phidget Motor controller can't be initialized"
motors_message.values = [ KeyValue(key = 'Recommendation', value = motorControllerDisconnected)]
if (motorsError == 2):
motors_message.level = 2
motors_message.message = "Can't set up motor speed"
motors_message.values = [ KeyValue(key = 'Recommendation', value = motorSpeedError)]
encoders_message = DiagnosticStatus(name = 'PhidgetEncoders', level = 0,message = 'initialized', hardware_id='Phidget')
if (encodersError == 1):
encoders_message.level = 2
encoders_message.message = "Phidget Encoder board can't be initialized"
encoders_message.values = [ KeyValue(key = 'Recommendation', value = encoderBoardDisconnected)]
if (encodersError == 2):
encoders_message.level = 2
encoders_message.message = "Can't get encoder value"
encoders_message.values = [ KeyValue(key = 'Recommendation', value = encoderValueError)]
array.status = [ motors_message, encoders_message ]
diagnosticPub.publish(array)
def send_diags():
# See diagnostics with: rosrun rqt_runtime_monitor rqt_runtime_monitor
msg = DiagnosticArray()
msg.status = []
msg.header.stamp = rospy.Time.now()
for gripper in grippers:
for servo in gripper.servos:
status = DiagnosticStatus()
status.name = "Gripper '%s' servo %d"%(gripper.name, servo.servo_id)
status.hardware_id = '%s'%servo.servo_id
temperature = servo.read_temperature()
status.values.append(KeyValue('Temperature', str(temperature)))
status.values.append(KeyValue('Voltage', str(servo.read_voltage())))
if temperature >= 70:
status.level = DiagnosticStatus.ERROR
status.message = 'OVERHEATING'
elif temperature >= 65:
status.level = DiagnosticStatus.WARN
status.message = 'HOT'
else:
status.level = DiagnosticStatus.OK
status.message = 'OK'
msg.status.append(status)
diagnostics_pub.publish(msg)
def _publish_diagnostic(self):
"""Publish diagnostics."""
diagnostic = DiagnosticArray()
diagnostic.header.stamp = rospy.Time.now()
status = DiagnosticStatus()
status.name = "LocalXY Origin"
status.hardware_id = "origin_publisher"
if self.origin is None:
status.level = DiagnosticStatus.ERROR
status.message = "No Origin"
else:
if self.origin_source == "gpsfix":
status.level = DiagnosticStatus.OK
status.message = "Has Origin (GPSFix)"
elif self.origin_source == "navsat":
status.level = DiagnosticStatus.OK
status.message = "Has Origin (NavSatFix)"
else:
status.level = DiagnosticStatus.WARN
status.message = "Origin Was Set Manually"
frame_id = self.origin.header.frame_id
status.values.append(KeyValue(key="Origin Frame ID", value=frame_id))
latitude = "%f" % self.origin.pose.position.y
status.values.append(KeyValue(key="Latitude", value=latitude))
longitude = "%f" % self.origin.pose.position.x
status.values.append(KeyValue(key="Longitude", value=longitude))
altitude = "%f" % self.origin.pose.position.z
status.values.append(KeyValue(key="Altitude", value=altitude))
diagnostic.status.append(status)
self.diagnostic_pub.publish(diagnostic)
def _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 _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 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 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 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 publish(self, state):
STATE_INDEX_CHARGING = 0
STATE_INDEX_BATTERY = 1
STATE_INDEX_CONNECTION = 2
## timed gate: limit to 1 Hz
curr_time = rospy.get_rostime()
if (curr_time - self.last_diagnostics_time).to_sec() < 1.0:
return
self.last_diagnostics_time = curr_time
## compose diagnostics message
diag = DiagnosticArray()
diag.header.stamp = curr_time
# battery info
stat = DiagnosticStatus(name="Battery", level=DiagnosticStatus.OK, message="OK")
try:
battery_state_code = state[STATE_INDEX_BATTERY]
stat.message = self.STATE_TEXTS_BATTERY[battery_state_code]
if battery_state_code < 3:
stat.level = DiagnosticStatus.WARN
if battery_state_code < 1:
stat.level = DiagnosticStatus.ERROR
stat.message = "Please Recharge Battery (%s)." % self.STATE_TEXTS_BATTERY[battery_state_code]
except KeyError as ex:
stat.message = "Invalid Battery State %s"%ex
rospy.logwarn("Invalid Battery State %s"%ex)
stat.level = DiagnosticStatus.ERROR
diag.status.append(stat)
# connection info
stat = DiagnosticStatus(name='ps3joy'": Connection Type", level=DiagnosticStatus.OK, message="OK")
try:
stat.message = self.STATE_TEXTS_CONNECTION[state[STATE_INDEX_CONNECTION]]
except KeyError as ex:
stat.message = "Invalid Connection State %s"%ex
rospy.logwarn("Invalid Connection State %s"%ex)
stat.level = DiagnosticStatus.ERROR
diag.status.append(stat)
# charging info
stat = DiagnosticStatus(name='ps3joy'": Charging State", level=DiagnosticStatus.OK, message="OK")
try:
stat.message = self.STATE_TEXTS_CHARGING[state[STATE_INDEX_CHARGING]]
except KeyError as ex:
stat.message = "Invalid Charging State %s"%ex
rospy.logwarn("Invalid Charging State %s"%ex)
stat.level = DiagnosticStatus.ERROR
diag.status.append(stat)
# publish message
self.diag_pub.publish(diag)
def 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 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 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 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 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 _laptop_charge_to_diag(laptop_msg):
rv = DiagnosticStatus()
rv.level = DiagnosticStatus.OK
rv.message = 'OK'
rv.name = 'Laptop Battery'
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 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 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 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 publish_diagnostics(self, time):
diag = DiagnosticArray()
diag.header.stamp = time
values = []
values.append(KeyValue(key="voltage", value=str(self.batt_voltage)))
values.append(KeyValue(key="numCharges", value=str(self.num_charges)))
values.append(KeyValue(key="timeSinceCharge", value=str(self.time_since_chg)))
stat = DiagnosticStatus(name="Battery Status", level=DiagnosticStatus.OK, message=self.power_state_msg, values=values)
if self.power_state == 3:
stat.level=DiagnosticStatus.WARN
if self.power_state == 4:
stat.level=DiagnosticStatus.ERROR
diag.status.append(stat)
self.diag_pub.publish(diag)
def _publish_diag(self):
ok = False
with self._mutex:
ok = self._ok
msg = DiagnosticArray()
stat = DiagnosticStatus()
msg.status.append(stat)
# Simulate pr2_etherCAT diagnostics
stat.level = 0
if not ok:
stat.level = 2
stat.name = 'EtherCAT Master'
stat.message = 'OK'
stat.values.append(KeyValue('Dropped Packets', '0'))
stat.values.append(KeyValue('RX Late Packet', '0'))
# Check for encoder errors
mcb_stat = DiagnosticStatus()
mcb_stat.name = 'EtherCAT Device (my_motor)'
mcb_stat.level = 0
mcb_stat.values.append(KeyValue('Num encoder_errors', '0'))
# Test camera listener
stat_cam = DiagnosticStatus()
stat_cam.level = 0
stat_cam.name = 'wge100: Frequency Status'
stat_cam.message = 'OK'
# Test HK listener
stat_hk = DiagnosticStatus()
stat_hk.level = 0
stat_hk.name = 'tilt_hokuyo_node: Frequency Status'
stat_hk.message = 'OK'
msg.status.append(stat_cam)
msg.status.append(mcb_stat)
msg.status.append(stat_hk)
msg.header.stamp = rospy.get_rostime()
self.diag_pub.publish(msg)
self._last_diag_pub = rospy.get_time()
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 publish_diagnostics(self, err=None):
'''
Publishes current kill board state in ROS diagnostics package, making it easy to use in GUIs and other ROS tools
'''
msg = DiagnosticArray()
msg.header.stamp = rospy.Time.now()
status = DiagnosticStatus()
status.name = 'kill_board'
status.hardware_id = self.port
if not self.connected:
status.level = DiagnosticStatus.ERROR
status.message = 'Cannot connect to kill board. Retrying every second.'
status.values.append(KeyValue("Exception", str(err)))
else:
status.level = DiagnosticStatus.OK
for key, value in self.board_status.items():
status.values.append(KeyValue(key, str(value)))
msg.status.append(status)
self.diagnostics_pub.publish(msg)
def 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 _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 gpu_status_to_diag(gpu_stat):
stat = DiagnosticStatus()
stat.name = socket.gethostname() + ' GPU Status'
stat.message = ''
stat.level = DiagnosticStatus.OK
stat.hardware_id = socket.gethostname() + "/" + gpu_stat.product_name
stat.values.append(KeyValue(key='Product Name', value = gpu_stat.product_name))
#stat.values.append(KeyValue(key='PCI Device/Vendor ID', value = gpu_stat.pci_device_id))
#stat.values.append(KeyValue(key='PCI Location ID', value = gpu_stat.pci_location))
#stat.values.append(KeyValue(key='Display', value = gpu_stat.display))
stat.values.append(KeyValue(key='Driver Version', value = gpu_stat.driver_version))
stat.values.append(KeyValue(key='Temperature (C)', value = '%.0f' % gpu_stat.temperature))
#stat.values.append(KeyValue(key='Fan Speed (RPM)', value = '%.0f' % _rads_to_rpm(gpu_stat.fan_speed)))
#stat.values.append(KeyValue(key='Usage (%)', value = '%.0f' % gpu_stat.gpu_usage))
stat.values.append(KeyValue(key='Memory (%)', value = '%.0f' % gpu_stat.memory_usage))
errors = []
# Check for valid data
if not gpu_stat.product_name:
stat.level = DiagnosticStatus.ERROR
errors.append('No Device Data')
else:
# Check load
if gpu_stat.gpu_usage > 95:
stat.level = max(stat.level, DiagnosticStatus.WARN)
errors.append('High Load')
# Check thresholds
if gpu_stat.temperature > 95:
stat.level = max(stat.level, DiagnosticStatus.ERROR)
errors.append('Temperature Alarm')
elif gpu_stat.temperature > 90:
stat.level = max(stat.level, DiagnosticStatus.WARN)
errors.append('High Temperature')
# Check memory usage
if gpu_stat.memory_usage > 95:
stat.level = max(stat.level, DiagnosticStatus.ERROR)
errors.append('Memory critical')
elif gpu_stat.memory_usage > 90:
stat.level = max(stat.level, DiagnosticStatus.WARN)
errors.append('Low Memory')
# Check fan
#if gpu_stat.fan_speed == 0:
# stat.level = max(stat.level, DiagnosticStatus.ERROR)
# stat.message = 'No Fan Speed'
if not errors:
stat.message = 'OK'
else:
stat.message = ', '.join(errors)
return stat
def get_status(self):
stat = 0
if not self._ok:
stat = 2
if rospy.get_time() - self._update_time > 3:
stat = 3
diag = DiagnosticStatus()
diag.level = stat
diag.name = 'Caster Slip Listener'
diag.message = 'OK'
if diag.level == 2:
diag.message = 'Caster Slipping'
if diag.level == 3:
diag.message = 'Caster Stale'
diag.level = 2
diag.values.append(KeyValue("Turret", str(TURRET_NAME)))
diag.values.append(KeyValue("R Wheel", str(R_WHEEL_NAME)))
diag.values.append(KeyValue("L Wheel", str(L_WHEEL_NAME)))
diag.values.append(KeyValue("Turret Position", str(self.last_position.turret)))
diag.values.append(KeyValue("R Wheel Position", str(self.last_position.r_wheel)))
diag.values.append(KeyValue("L Wheel Position", str(self.last_position.l_wheel)))
diag.values.append(KeyValue("Max Pos. Left Slip", str(self._max_l_err_pos)))
diag.values.append(KeyValue("Max Neg. Left Slip", str(self._max_l_err_neg)))
diag.values.append(KeyValue("Max Pos. Right Slip", str(self._max_r_err_pos)))
diag.values.append(KeyValue("Max Neg. Right Slip", str(self._max_r_err_neg)))
diag.values.append(KeyValue("Max Pos. Left Slip (Reset)", str(self._max_l_err_pos_reset)))
diag.values.append(KeyValue("Max Neg. Left Slip (Reset)", str(self._max_l_err_neg_reset)))
diag.values.append(KeyValue("Max Pos. Right Slip (Reset)", str(self._max_r_err_pos_reset)))
diag.values.append(KeyValue("Max Neg. Right Slip (Reset)", str(self._max_r_err_neg_reset)))
diag.values.append(KeyValue("Wheel Offset", str(WHEEL_OFFSET)))
diag.values.append(KeyValue("Wheel Diameter", str(WHEEL_RADIUS)))
diag.values.append(KeyValue("Allowed Slip", str(ALLOWED_SLIP)))
diag.values.append(KeyValue("Update Interval", str(UPDATE_INTERVAL)))
diag.values.append(KeyValue("Total Errors", str(self._num_errors)))
diag.values.append(KeyValue("Errors Since Reset", str(self._num_errors_since_reset)))
return diag
def sensor_status_to_diag(sensorList, hostname='localhost', ignore_fans=False):
stat = DiagnosticStatus()
stat.name = '%s Sensor Status'%hostname
stat.message = 'OK'
stat.level = DiagnosticStatus.OK
stat.hardware_id = hostname
for sensor in sensorList:
if sensor.getType() == "Temperature":
if sensor.getInput() > sensor.getCrit():
stat.level = max(stat.level, DiagnosticStatus.ERROR)
stat.message = "Critical Temperature"
elif sensor.getInput() > sensor.getHigh():
stat.level = max(stat.level, DiagnosticStatus.WARN)
stat.message = "High Temperature"
stat.values.append(KeyValue(key=" ".join([sensor.getName(), sensor.getType()]),
value=str(sensor.getInput())))
elif sensor.getType() == "Voltage":
if sensor.getInput() < sensor.getMin():
stat.level = max(stat.level, DiagnosticStatus.ERROR)
stat.message = "Low Voltage"
elif sensor.getInput() > sensor.getMax():
stat.level = max(stat.level, DiagnosticStatus.ERROR)
stat.message = "High Voltage"
stat.values.append(KeyValue(key=" ".join([sensor.getName(), sensor.getType()]),
value=str(sensor.getInput())))
elif sensor.getType() == "Speed":
if not ignore_fans:
if sensor.getInput() < sensor.getMin():
stat.level = max(stat.level, DiagnosticStatus.ERROR)
stat.message = "No Fan Speed"
stat.values.append(KeyValue(key=" ".join([sensor.getName(), sensor.getType()]),
value=str(sensor.getInput())))
return 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
pub_diagnostics = rospy.Publisher('/diagnostics',
DiagnosticArray,
queue_size=1)
rospy.loginfo("fake diagnostics for %s running listening to %s",
diagnostics_name, topic_name)
rate = rospy.Rate(1)
while not rospy.is_shutdown():
# if no topic_name is set, we assume that we received a
if topic_name == None:
last_received_ = rospy.Time.now()
# only publish ok if message received recently
if rospy.Time.now() - last_received_ <= rospy.Duration(10.0):
status = DiagnosticStatus()
status.level = 0
status.name = diagnostics_name
status.message = diagnostics_name + " running"
diagnostics = DiagnosticArray()
diagnostics.header.stamp = rospy.Time.now()
diagnostics.status.append(status)
else:
status = DiagnosticStatus()
status.level = 2
status.name = diagnostics_name
status.message = "no message received on " + topic_name
diagnostics = DiagnosticArray()
diagnostics.header.stamp = rospy.Time.now()
diagnostics.status.append(status)
pub_diagnostics.publish(diagnostics)
try:
def hwboard(self):
# initialize local variables
send_channel = 0
read_channel = 0
send_specifier = 0
read_specifier = 0
read_status = 0
read_data = 0
read_id = 0
read_crc = 0
# init ros-node
pub = rospy.Publisher('diagnostics', DiagnosticArray)
while not rospy.is_shutdown():
# init publisher message
pub_message = DiagnosticArray()
# init array for storing data
status_array = []
# init local variable for error detection
error_while_reading = 0
for send_specifier in range(0, 7, 3):
if send_specifier == 0:
count_range = range(6)
elif send_specifier == 3:
count_range = [0, 1, 2, 3, 6, 7]
else:
count_range = [1, 2, 3, 6, 7]
for send_channel in count_range:
# init message and local variables
send_buff_array = [
send_channel, send_specifier, 0x00, 0x00, 0x00
]
message = ""
preamble_bytes = 4
preamble_error = 1
crc_error = 1
retry = 0
# calculate crc
crc = 0x00
for i in range(4):
data = send_buff_array[i]
for k in range(8):
feedback_bit = (crc ^ data) & 0x80
feedback_bit = (feedback_bit >> 7) & 0xFF
if feedback_bit == 1:
crc = (crc << 1) & 0xFF
crc = crc ^ 0x31
else:
crc = (crc << 1) & 0xFF
data = (data << 1) & 0xFF
send_buff_array[4] = crc
# send message
while (preamble_error == 1
or crc_error == 1) and retry < 8:
message = ""
for i in range(preamble_bytes):
message += chr(0x55)
for i in send_buff_array:
message += chr(i)
self.s.write(message)
# receive message
# check for first preamble byte of reveiced message
read_buff_array = []
buff = self.s.read(1)
preamble_count = 0
for i in buff:
read_buff_array.append(ord(i))
if read_buff_array[0] == 0x55:
# check for following preamble bytes
while read_buff_array[
0] == 0x55 and preamble_count < 10:
read_buff_array = []
buff = self.s.read(1)
for i in buff:
read_buff_array.append(ord(i))
preamble_count = preamble_count + 1
buff = self.s.read(13)
# check preamble length
if preamble_count > 6:
preamble_error = 1
preamble_bytes = preamble_bytes + 1
retry = retry + 1
if preamble_bytes == 7:
preamble_bytes = 2
elif preamble_count < 2:
preamble_error = 1
preamble_bytes = preamble_bytes + 1
retry = retry + 1
if preamble_bytes == 7:
preamble_bytes = 2
else:
# preamble ok. evaluate message
preamble_error = 0
# get remaining message
for i in buff:
read_buff_array.append(ord(i))
#check crc
crc = 0x00
for i in range(14):
data = read_buff_array[i]
for k in range(8):
feedback_bit = (crc ^ data) & 0x80
feedback_bit = (
feedback_bit >> 7) & 0xFF
if feedback_bit == 1:
crc = (crc << 1) & 0xFF
crc = crc ^ 0x31
else:
crc = (crc << 1) & 0xFF
data = (data << 1) & 0xFF
if crc != 0:
crc_error = 1
preamble_bytes = preamble_bytes + 1
retry = retry + 1
if preamble_bytes == 7:
preamble_bytes = 2
else:
crc_error = 0
# no preamble detected
else:
buff = s.read(14)
preamble_error = 1
preamble_bytes = preamble_bytes + 1
retry = retry + 1
if preamble_bytes == 7:
preamble_bytes = 2
# get channel byte
read_channel = int(read_buff_array[0])
# get specifier byte
read_specifier = int(read_buff_array[1])
# get status byte
read_status = int(read_buff_array[2])
# get data bytes
read_data = 256 * int(read_buff_array[3])
read_data = read_data + int(read_buff_array[4])
# get id bytes
read_id = read_buff_array[5] << 8
read_id = (read_id | read_buff_array[6]) << 8
read_id = (read_id | read_buff_array[7]) << 8
read_id = read_id | read_buff_array[8]
# evaluate recieved message
if read_channel == send_channel:
if read_specifier == send_specifier:
if read_status == 0 or read_status == 8:
if send_specifier == 0:
read_data = read_data / 10.0
else:
read_data = read_data / 1000.0
erro_while_reading = 0
else:
read_data = 0
error_while_reading = 1
else:
read_data = 0
error_while_reading = 1
else:
read_data = 0
error_while_reading = 1
#prepare status object for publishing
# init sensor object
status_object = DiagnosticStatus()
# init local variable for data
key_value = KeyValue()
# set values for temperature parameters
if send_specifier == 0:
if read_data == 85:
level = 1
status_object.message = "sensor damaged"
elif read_data > 50:
level = 2
status_object.message = "temperature critical"
elif read_data > 40:
level = 1
status_object.message = "temperature high"
elif read_data > 10:
level = 0
status_object.message = "temperature ok"
elif read_data > -1:
level = 1
status_object.message = "temperature low"
else:
level = 2
status_object.message = "temperature critical"
# mapping for temperature sensors
if read_id == self.head_sensor_param:
status_object.name = "Head Temperature"
status_object.hardware_id = "hwboard_channel " + str(
send_channel)
elif read_id == self.eye_sensor_param:
status_object.name = "Eye Camera Temperature"
status_object.hardware_id = "hwboard_channel = " + str(
send_channel)
elif read_id == self.torso_module_sensor_param:
status_object.name = "Torso Module Temperature"
status_object.hardware_id = "hwboard_channel =" + str(
send_channel)
elif read_id == self.torso_sensor_param:
status_object.name = "Torso Temperature"
status_object.hardware_id = "hwboard_channel =" + str(
send_channel)
elif read_id == self.pc_sensor_param:
status_object.name = "PC Temperature"
status_object.hardware_id = "hwboard_channel =" + str(
send_channel)
elif read_id == self.engine_sensor_param:
status_object.name = "Engine Temperature"
status_object.hardware_id = "hwboard_channel = " + str(
send_channel)
else:
level = 1
status_object.message = "cannot map if from yaml file to temperature sensor"
# set values for voltage parameters
elif send_specifier == 3:
if send_channel == 0:
if read_data > 58:
level = 2
status_object.message = "voltage critical"
elif read_data > 56:
level = 1
status_object.message = "voltage high"
elif read_data > 44:
level = 0
status_object.message = "voltage ok"
elif read_data > 42:
level = 1
status_object.message = "voltage low"
else:
level = 2
status_object.message = "voltage critical"
else:
if read_data > 27:
level = 2
status_object.message = "voltage critical"
elif read_data > 25:
level = 1
status_object.message = "voltage_high"
elif read_data > 23:
level = 0
status_object.message = "voltage ok"
elif read_data > 19:
level = 1
status_object.message = "voltage low"
else:
level = 2
status_object.message = "voltage critical"
if send_channel == 0:
status_object.name = "Akku Voltage"
status_object.hardware_id = "hwboard_channel = 0"
elif send_channel == 1:
status_object.name = "Torso Engine Voltage"
status_object.hardware_id = "hwboard_channel = 1"
elif send_channel == 2:
status_object.name = "Torso Logic Voltage"
status_object.hardware_id = "hwboard_channel = 2"
elif send_channel == 3:
status_object.name = "Tray Logic Voltage"
status_object.hardware_id = "hwboard_channel = 3"
elif send_channel == 6:
status_object.name = "Arm Engine Voltage"
status_object.hardware_id = "hwboard_channel = 6"
elif send_channel == 7:
status_object.name = "Tray Engine Voltage"
status_object.hardware_id = "hwboard_channel = 7"
# set values for current parameters
else:
if read_data > 15:
level = 2
status_object.message = "current critical"
elif read_data > 10:
level = 1
status_object.message = "current high"
elif read_data < 0:
level = 2
status_object.message = "current critical"
else:
level = 0
status_object.message = "current ok"
if send_channel == 1:
status_object.name = "Torso Engine Current"
status_object.hardware_id = "hwboard_channel = 1"
elif send_channel == 2:
status_object.name = "Torso Logic Current"
status_object.hardware_id = "hwboard_channel = 2"
elif send_channel == 3:
status_object.name = "Tray Logic Current"
status_object.hardware_id = "hwboard_channel = 3"
elif send_channel == 6:
status_object.name = "Arm Engine Current"
status_object.hardware_id = "hwboard_channel = 6"
elif send_channel == 7:
status_object.name = "Tray Engine Current"
status_object.hardware_id = "hwboard_channel = 7"
# evaluate error detection
if error_while_reading == 1:
level = 1
status_object.message = "detected error while receiving answer from hardware"
# append status object to publishing message
status_object.level = level
key_value.value = str(read_data)
status_object.values.append(key_value)
pub_message.status.append(status_object)
# publish message
pub.publish(pub_message)
rospy.sleep(1.0)
def __publish_diagnostic_information(self):
diag_msg = DiagnosticArray()
rate = rospy.Rate(self.diagnostics_rate)
while not rospy.is_shutdown() and self.running:
diag_msg.status = []
diag_msg.header.stamp = rospy.Time.now()
status = DiagnosticStatus()
status.name = 'Dynamixel Serial Bus (%s)' % self.port_namespace
status.hardware_id = 'Dynamixel Serial Bus on port %s' % self.port_name
status.values.append(KeyValue('Baud Rate', str(self.baud_rate)))
status.values.append(
KeyValue('Min Motor ID', str(self.min_motor_id)))
status.values.append(
KeyValue('Max Motor ID', str(self.max_motor_id)))
status.values.append(
KeyValue('Desired Update Rate', str(self.update_rate)))
status.values.append(
KeyValue('Actual Update Rate', str(self.actual_rate)))
status.values.append(
KeyValue('# Non Fatal Errors',
str(self.error_counts['non_fatal'])))
status.values.append(
KeyValue('# Checksum Errors',
str(self.error_counts['checksum'])))
status.values.append(
KeyValue('# Dropped Packet Errors',
str(self.error_counts['dropped'])))
status.level = DiagnosticStatus.OK
status.message = 'OK'
if self.actual_rate - self.update_rate < -5:
status.level = DiagnosticStatus.WARN
status.message = 'Actual update rate is lower than desired'
diag_msg.status.append(status)
for motor_state in self.current_state.motor_states:
mid = motor_state.id
status = DiagnosticStatus()
status.name = 'Robotis Dynamixel Motor %d on port %s' % (
mid, self.port_namespace)
status.hardware_id = 'DXL-%d@%s' % (motor_state.id,
self.port_namespace)
status.values.append(
KeyValue('Model Name',
str(self.motor_static_info[mid]['model'])))
status.values.append(
KeyValue('Firmware Version',
str(self.motor_static_info[mid]['firmware'])))
status.values.append(
KeyValue('Return Delay Time',
str(self.motor_static_info[mid]['delay'])))
status.values.append(
KeyValue('Minimum Voltage',
str(self.motor_static_info[mid]['min_voltage'])))
status.values.append(
KeyValue('Maximum Voltage',
str(self.motor_static_info[mid]['max_voltage'])))
status.values.append(
KeyValue('Minimum Position (CW)',
str(self.motor_static_info[mid]['min_angle'])))
status.values.append(
KeyValue('Maximum Position (CCW)',
str(self.motor_static_info[mid]['max_angle'])))
status.values.append(KeyValue('Goal', str(motor_state.goal)))
status.values.append(
KeyValue('Position', str(motor_state.position)))
status.values.append(KeyValue('Error', str(motor_state.error)))
status.values.append(
KeyValue('Velocity', str(motor_state.speed)))
status.values.append(KeyValue('Load', str(motor_state.load)))
status.values.append(
KeyValue('Voltage', str(motor_state.voltage)))
status.values.append(
KeyValue('Temperature', str(motor_state.temperature)))
status.values.append(
KeyValue('Moving', str(motor_state.moving)))
if motor_state.temperature >= self.error_level_temp:
status.level = DiagnosticStatus.ERROR
status.message = 'OVERHEATING'
elif motor_state.temperature >= self.warn_level_temp:
status.level = DiagnosticStatus.WARN
status.message = 'VERY HOT'
else:
status.level = DiagnosticStatus.OK
status.message = 'OK'
diag_msg.status.append(status)
self.diagnostics_pub.publish(diag_msg)
rate.sleep()
#!/usr/bin/env python
import os
import rospy
from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
if __name__ == '__main__':
node_name = os.path.splitext(os.path.basename(__file__))[0]
rospy.init_node(node_name)
publish_rate = rospy.get_param('~publish_rate', 1.0)
frame_id = rospy.get_param('~frame_id', 'base_link')
ip_address = rospy.get_param('~ip_address', '192.168.0.12')
pub = rospy.Publisher('/ft_sensor/diagnostics',
DiagnosticArray,
queue_size=3)
msg = DiagnosticArray()
msg.header.frame_id = frame_id
status = DiagnosticStatus()
status.level = DiagnosticStatus.OK
status.name = 'NetFT RDT Driver'
status.message = 'OK'
status.hardware_id = 'ATI Gamma'
status.values.append(KeyValue('IP Address', ip_address))
msg.status.append(status)
rate = rospy.Rate(publish_rate)
while not rospy.is_shutdown():
msg.header.stamp = rospy.Time.now()
pub.publish(msg)
rate.sleep()
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)
proxy = Connection(("ant", 1234))
proxy.login("Master", "robox")
data = proxy.inspect(StringArray(["Safety:batteryPower,batteryVoltage"]))
diagnosticArray = DiagnosticArray()
diagnosticArray.header.stamp = rospy.Time.now()
batteryStatus = DiagnosticStatus()
batteryStatus.name = "battery"
batteryStatus.hardware_id = socket.gethostname()
batteryStatus.message = "%.3fV %.1f%%" % (data.Safety.batteryVoltage,
data.Safety.batteryPower)
if data.Safety.batteryPower < 10:
batteryStatus.message += "; Critically low power, recharge NOW!"
batteryStatus.level = DiagnosticStatus.ERROR
elif data.Safety.batteryPower < 20:
batteryStatus.message += "; Low power, recharge soon!"
batteryStatus.level = DiagnosticStatus.WARN
else:
batteryStatus.message += "; Power level good"
batteryStatus.level = DiagnosticStatus.OK
diagnosticArray.status.append(batteryStatus)
publisher.publish(diagnosticArray)
batteryVoltagePublisher.publish(data.Safety.batteryVoltage)
batteryPercentagePublisher.publish(data.Safety.batteryPower)
rate.sleep()
print "Closing battery monitor"
proxy.close()
def initialize_origin():
rospy.init_node('initialize_origin', anonymous=True)
global _origin_pub
global _local_xy_frame
_origin_pub = rospy.Publisher('/local_xy_origin', GPSFix, latch=True)
diagnostic_pub = rospy.Publisher('/diagnostics', DiagnosticArray)
local_xy_origin = rospy.get_param('~local_xy_origin', 'auto')
_local_xy_frame = rospy.get_param('~local_xy_frame', 'map')
if local_xy_origin == "auto":
global _sub
_sub = rospy.Subscriber("gps", GPSFix, gps_callback)
else:
parse_origin(local_xy_origin)
hw_id = rospy.get_param('~hw_id', 'none')
while not rospy.is_shutdown():
if _gps_fix == None:
diagnostic = DiagnosticArray()
diagnostic.header.stamp = rospy.Time.now()
status = DiagnosticStatus()
status.name = "LocalXY Origin"
status.hardware_id = hw_id
status.level = DiagnosticStatus.ERROR
status.message = "No Origin"
diagnostic.status.append(status)
diagnostic_pub.publish(diagnostic)
else:
_origin_pub.publish(
_gps_fix) # Publish this at 1Hz for bag convenience
diagnostic = DiagnosticArray()
diagnostic.header.stamp = rospy.Time.now()
status = DiagnosticStatus()
status.name = "LocalXY Origin"
status.hardware_id = hw_id
if _gps_fix.status.header.frame_id == 'auto':
status.level = DiagnosticStatus.OK
status.message = "Has Origin (auto)"
else:
status.level = DiagnosticStatus.WARN
status.message = "Origin is static (non-auto)"
value0 = KeyValue()
value0.key = "Origin"
value0.value = _gps_fix.status.header.frame_id
status.values.append(value0)
value1 = KeyValue()
value1.key = "Latitude"
value1.value = "%f" % _gps_fix.latitude
status.values.append(value1)
value2 = KeyValue()
value2.key = "Longitude"
value2.value = "%f" % _gps_fix.longitude
status.values.append(value2)
value3 = KeyValue()
value3.key = "Altitude"
value3.value = "%f" % _gps_fix.altitude
status.values.append(value3)
diagnostic.status.append(status)
diagnostic_pub.publish(diagnostic)
rospy.sleep(1.0)
mag_msg.magnetic_field.x = float(st.unpack('h', st.pack('BB', buf[6], buf[7]))[0]) / mag_fact
mag_msg.magnetic_field.y = float(st.unpack('h', st.pack('BB', buf[8], buf[9]))[0]) / mag_fact
mag_msg.magnetic_field.z = float(st.unpack('h', st.pack('BB', buf[10], buf[11]))[0]) / mag_fact
pub_mag.publish(mag_msg)
# Publish temperature
if len(buf) > 45:
temperature_msg.header.stamp = rospy.Time.now()
temperature_msg.header.frame_id = frame_id
temperature_msg.header.seq = seq
temperature_msg.temperature = buf[44]
pub_temp.publish(temperature_msg)
# Publish diagnostic status
if len(buf) > 51:
status_msg.level = 0
status_msg.name = "BNO055"
status_msg.message = ""
calib_stat = KeyValue(key='calib_stat',value=str(buf[45]))
selftest_result = KeyValue(key='selftest_result',value=str(buf[46]))
intr_stat = KeyValue(key='intr_stat',value=str(buf[47]))
sys_clk_stat = KeyValue(key='sys_clk_stat',value=str(buf[48]))
sys_stat = KeyValue(key='sys_stat',value=str(buf[49]))
sys_err = KeyValue(key='sys_err',value=str(buf[50]))
status_msg.values = [calib_stat,selftest_result,intr_stat,sys_clk_stat,sys_stat,sys_err]
pub_status.publish(status_msg)
seq = seq + 1
rate.sleep()
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 initialize_origin():
global _origin
rospy.init_node('initialize_origin', anonymous=True)
ros_distro = os.environ.get('ROS_DISTRO')
if not ros_distro:
rospy.logerror('ROS_DISTRO environment variable was not set.')
exit(1)
if ros_distro == 'indigo':
# ROS Indigo uses the GPSFix message
global _origin_pub
global _local_xy_frame
_origin_pub = rospy.Publisher('/local_xy_origin', GPSFix, latch=True, queue_size=2)
diagnostic_pub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=2)
local_xy_origin = rospy.get_param('~local_xy_origin', 'auto')
_local_xy_frame = rospy.get_param('~local_xy_frame', 'map')
_local_xy_frame_identity = rospy.get_param('~local_xy_frame_identity', _local_xy_frame + "__identity")
if local_xy_origin == "auto":
global _sub
_sub = rospy.Subscriber("gps", GPSFix, gps_callback)
else:
parse_origin(local_xy_origin)
if len(_local_xy_frame):
tf_broadcaster = tf.TransformBroadcaster()
else:
tf_broadcaster = None
hw_id = rospy.get_param('~hw_id', 'none')
while not rospy.is_shutdown():
if tf_broadcaster:
# Publish transform involving map (to an anonymous unused
# frame) so that TransformManager can support /tf<->/wgs84
# conversions without requiring additional nodes.
tf_broadcaster.sendTransform(
(0, 0, 0),
(0, 0, 0, 1),
rospy.Time.now(),
_local_xy_frame_identity, _local_xy_frame)
if _gps_fix == None:
diagnostic = DiagnosticArray()
diagnostic.header.stamp = rospy.Time.now()
status = DiagnosticStatus()
status.name = "LocalXY Origin"
status.hardware_id = hw_id
status.level = DiagnosticStatus.ERROR
status.message = "No Origin"
diagnostic.status.append(status)
diagnostic_pub.publish(diagnostic)
else:
_origin_pub.publish(_gps_fix) # Publish this at 1Hz for bag convenience
diagnostic = DiagnosticArray()
diagnostic.header.stamp = rospy.Time.now()
status = DiagnosticStatus()
status.name = "LocalXY Origin"
status.hardware_id = hw_id
if local_xy_origin == 'auto':
status.level = DiagnosticStatus.OK
status.message = "Has Origin (auto)"
else:
status.level = DiagnosticStatus.WARN
status.message = "Origin is static (non-auto)"
value0 = KeyValue()
value0.key = "Origin"
value0.value = _gps_fix.status.header.frame_id
status.values.append(value0)
value1 = KeyValue()
value1.key = "Latitude"
value1.value = "%f" % _gps_fix.latitude
status.values.append(value1)
value2 = KeyValue()
value2.key = "Longitude"
value2.value = "%f" % _gps_fix.longitude
status.values.append(value2)
value3 = KeyValue()
value3.key = "Altitude"
value3.value = "%f" % _gps_fix.altitude
status.values.append(value3)
diagnostic.status.append(status)
diagnostic_pub.publish(diagnostic)
rospy.sleep(1.0)
else:
# ROS distros later than Indigo use NavSatFix
origin_pub = rospy.Publisher('/local_xy_origin', PoseStamped, latch=True, queue_size=2)
diagnostic_pub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=2)
origin_name = rospy.get_param('~local_xy_origin', 'auto')
rospy.loginfo('Local XY origin is "' + origin_name + '"')
origin_frame_id = rospy.get_param(rospy.search_param('local_xy_frame'), 'map')
origin_frame_identity = rospy.get_param('~local_xy_frame_identity', origin_frame_id + "__identity")
rospy.loginfo('Local XY frame ID is "' + origin_frame_id + '"')
if len(origin_frame_id):
tf_broadcaster = tf.TransformBroadcaster()
else:
tf_broadcaster = None
if origin_name != "auto":
origin_list = rospy.get_param('~local_xy_origins', [])
_origin = make_origin_from_list(origin_frame_id, origin_name, origin_list)
if _origin is not None:
origin_pub.publish(_origin)
else:
origin_name = "auto"
if origin_name == "auto":
sub = rospy.Subscriber("gps", NavSatFix)
sub.impl.add_callback(navsatfix_callback, (origin_frame_id, origin_pub, sub))
rospy.loginfo('Subscribed to NavSat on ' + sub.resolved_name)
while not rospy.is_shutdown():
if tf_broadcaster:
# Publish transform involving map (to an anonymous unused
# frame) so that TransformManager can support /tf<->/wgs84
# conversions without requiring additional nodes.
tf_broadcaster.sendTransform(
(0, 0, 0),
(0, 0, 0, 1),
rospy.Time.now(),
origin_frame_identity, origin_frame_id)
diagnostic_pub.publish(make_diagnostic(_origin, origin_name != "auto"))
rospy.sleep(1.0)
def ntp_monitor(ntp_hostname,
offset=500,
self_offset=500,
diag_hostname=None,
error_offset=5000000,
ignore_self=False):
pub = rospy.Publisher("/diagnostics", DiagnosticArray, queue_size=10)
rospy.init_node(NAME, anonymous=True)
hostname = socket.gethostname()
if diag_hostname is None:
diag_hostname = hostname
ntp_checks = []
stat = DiagnosticStatus()
stat.level = 0
stat.name = "NTP offset from " + diag_hostname + " to " + ntp_hostname
stat.message = "OK"
stat.hardware_id = hostname
stat.values = []
ntp_checks.append((stat, ntp_hostname, offset))
if not ignore_self:
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 = []
ntp_checks.append((self_stat, hostname, self_offset))
while not rospy.is_shutdown():
msg = DiagnosticArray()
for st, host, off in ntp_checks:
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.status.append(st)
msg.header.stamp = rospy.get_rostime()
pub.publish(msg)
time.sleep(1)
def publish_diagnostics(self, rt_HZ_store = []):
# set desired rates
if self.hzerror:
if isinstance(self.hzerror, float) or isinstance(self.hzerror, int):
min_rate = self.hz - self.hzerror
max_rate = self.hz + self.hzerror
else:
rospy.logerr("hzerror not float or int")
sys.exit(1)
else:
min_rate = None
max_rate = None
# create diagnostic message
array = DiagnosticArray()
array.header.stamp = rospy.Time.now()
hz_status = DiagnosticStatus()
hz_status.name = self.diagnostics_name
hz_status.values.append(KeyValue("topics", str(self.topics)))
hz_status.values.append(KeyValue("desired_rate", str(self.hz)))
hz_status.values.append(KeyValue("min_rate", str(min_rate)))
hz_status.values.append(KeyValue("max_rate", str(max_rate)))
hz_status.values.append(KeyValue("window_size", str(self.window_size)))
publishing_rate_error = False
rates = []
consolidated_error_messages = {} ## store and display consolidated erros messages for all the topics
consolidated_error_messages.setdefault("never received message for topics", [])
consolidated_error_messages.setdefault("no messages anymore for topics", [])
consolidated_error_messages.setdefault("publishing rate is too low for topics", [])
consolidated_error_messages.setdefault("publishing rate is too high for topics", [])
if len(rt_HZ_store) != len(self.topics):
hz_status.level = DiagnosticStatus.WARN
hz_status.message = "could not determine type for topics %s. Probably never published on that topics."%self.missing_topics
array.status.append(hz_status)
self.pub_diagnostics.publish(array)
return
# calculate actual rates
for rt, topic in zip(rt_HZ_store, self.topics):
if not rt or not rt.times:
hz_status.level = DiagnosticStatus.ERROR
publishing_rate_error = True
rates.append(0.0)
consolidated_error_messages["never received message for topics"].append(topic)
elif rt.msg_tn == rt.last_printed_tn:
hz_status.level = DiagnosticStatus.ERROR
publishing_rate_error = True
rates.append(0.0)
consolidated_error_messages["no messages anymore for topics"].append(topic)
else:
with rt.lock: # calculation taken from /opt/ros/indigo/lib/python2.7/dist-packages/rostopic/__init__.py
n = len(rt.times)
mean = sum(rt.times) / n
rate = 1./mean if mean > 0. else 0
rt.last_printed_tn = rt.msg_tn
rates.append(round(rate, 2))
if min_rate and rate < min_rate:
hz_status.level = DiagnosticStatus.WARN
publishing_rate_error = True
consolidated_error_messages["publishing rate is too low for topics"].append(topic)
elif max_rate and rate > max_rate:
hz_status.level = DiagnosticStatus.WARN
publishing_rate_error = True
consolidated_error_messages["publishing rate is too high for topics"].append(topic)
else:
if not publishing_rate_error:
hz_status.level = DiagnosticStatus.OK
hz_status.message = 'all publishing rates are ok'
if publishing_rate_error:
message = ""
key = "never received message for topics"
if len(consolidated_error_messages[key]) > 0:
message += key + " " + str(consolidated_error_messages[key]) + ", "
key = "no messages anymore for topics"
if len(consolidated_error_messages[key]) > 0:
message += key + " " + str(consolidated_error_messages[key]) + ", "
key = "publishing rate is too low for topics"
if len(consolidated_error_messages[key]) > 0:
message += key + " " + str(consolidated_error_messages[key]) + ", "
key = "publishing rate is too high for topics"
if len(consolidated_error_messages[key]) > 0:
message += key + " " + str(consolidated_error_messages[key])
hz_status.message = message
hz_status.values.append(KeyValue("rates", str(rates)))
array.status.append(hz_status)
self.pub_diagnostics.publish(array)
if previousTimestamp is not None:
interval = (receivedTimestamp - previousTimestamp).to_sec()
averageInterval += interval
maxInterval = max(interval, maxInterval)
previousTimestamp = receivedTimestamp
# Prepare diagnostics message
diagnosticArray = DiagnosticArray()
diagnosticArray.header.stamp = rospy.Time.now()
topicStatus = DiagnosticStatus()
topicStatus.name = "Topic status: " + topic
topicStatus.hardware_id = hostname
topicStatus.level = DiagnosticStatus.WARN if lastMessageAt == rospy.Time(
0
) else DiagnosticStatus.ERROR if hasTimeout else DiagnosticStatus.OK
averageRate = 0.0
if len(timestampBuffer) > 1:
# Finish averaging
averageDelay /= float(len(timestampBuffer))
averageInterval /= float(len(timestampBuffer) - 1)
averageRate = 1.0 / max(averageInterval, 1.0 / 10000)
topicStatus.values.append(
KeyValue('Last received at', str(lastMessageAt.to_sec())))
topicStatus.values.append(
KeyValue('Observation window size:',
"%d messages" % len(timestampBuffer)))
topicStatus.values.append(
imuMsg.orientation.y = q[1]
imuMsg.orientation.z = q[2]
imuMsg.orientation.w = q[3]
imuMsg.header.stamp = rospy.Time.now()
imuMsg.header.frame_id = 'base_imu_link'
imuMsg.header.seq = seq
seq = seq + 1
pub.publish(imuMsg)
if (diag_pub_time < rospy.get_time()):
diag_pub_time += 1
diag_arr = DiagnosticArray()
diag_arr.header.stamp = rospy.get_rostime()
diag_arr.header.frame_id = '1'
diag_msg = DiagnosticStatus()
diag_msg.name = 'Razor_Imu'
diag_msg.level = DiagnosticStatus.OK
diag_msg.message = 'Received AHRS measurement'
diag_msg.values.append(
KeyValue('roll (deg)', str(roll * (180.0 / math.pi))))
diag_msg.values.append(
KeyValue('pitch (deg)', str(pitch * (180.0 / math.pi))))
diag_msg.values.append(
KeyValue('yaw (deg)', str(yaw * (180.0 / math.pi))))
diag_msg.values.append(KeyValue('sequence number', str(seq)))
diag_arr.status.append(diag_msg)
diag_pub.publish(diag_arr)
ser.close
#f.close
maxDelay = max(delay, maxDelay)
#if previousTimestamp is not None:
# interval = (receivedTimestamp - previousTimestamp).to_sec()
# averageInterval += interval
# maxInterval = max(interval, maxInterval)
previousTimestamp = receivedTimestamp
# Prepare diagnostics message
diagnosticArray = DiagnosticArray()
diagnosticArray.header.stamp = rospy.Time.now()
tfStatus = DiagnosticStatus()
tfStatus.name = "TF link status: %s --> %s" % (source_frame, target_frame)
tfStatus.hardware_id = hostname
tfStatus.level = DiagnosticStatus.WARN if lastTransformAt == rospy.Time(0) else DiagnosticStatus.ERROR if hasTimeout else DiagnosticStatus.OK
averageRate = 0.0
if len(timestampBuffer) > 1:
# Finish averaging
averageDelay /= float(len(timestampBuffer))
# averageInterval /= float(len(timestampBuffer) - 1)
# averageRate = 1.0 / averageInterval
tfStatus.values.append(KeyValue('Last received at', str(lastTransformAt.to_sec())))
tfStatus.values.append(KeyValue('Observation window size:', "%d messages" % len(timestampBuffer)))
#tfStatus.values.append(KeyValue('Average interval between messages', "%.3f sec" % averageInterval))
tfStatus.values.append(KeyValue('Maximum delay wrt. current ROS time', "%.3f sec" % maxDelay))
tfStatus.values.append(KeyValue('Average delay wrt. current ROS time', "%.3f sec" % averageDelay))
if lastTransformAt == rospy.Time(0):
def publish_diagnostics(self, event):
# set desired rates
if isinstance(self.hzerror, float) or isinstance(self.hzerror, int):
if self.hzerror < 0:
rospy.logerr("hzerror cannot be negative")
sys.exit(1)
min_rate = self.hz - self.hzerror
max_rate = self.hz + self.hzerror
if min_rate < 0 or max_rate < 0:
rospy.logerr("min_rate/max_rate cannot be negative")
sys.exit(1)
min_duration = 1 / min_rate if min_rate > 0 else float('inf')
max_duration = 1 / max_rate if max_rate > 0 else float('inf')
else:
rospy.logerr("hzerror not float or int")
sys.exit(1)
# create diagnostic message
array = DiagnosticArray()
array.header.stamp = rospy.Time.now()
hz_status = DiagnosticStatus()
hz_status.name = self.diagnostics_name
hz_status.level = DiagnosticStatus.OK
hz_status.message = 'all publishing rates are ok'
hz_status.values.append(KeyValue("topics", str(self.topics)))
hz_status.values.append(KeyValue("desired_rate", str(self.hz)))
hz_status.values.append(KeyValue("min_rate", str(min_rate)))
hz_status.values.append(KeyValue("max_rate", str(max_rate)))
hz_status.values.append(KeyValue("window_size", str(self.window_size)))
rates = []
consolidated_error_messages = {
} ## store and display consolidated erros messages for all the topics
consolidated_error_messages.setdefault(
"never received message for topics", [])
consolidated_error_messages.setdefault(
"no messages anymore for topics", [])
consolidated_error_messages.setdefault(
"publishing rate is too low for topics", [])
consolidated_error_messages.setdefault(
"publishing rate is too high for topics", [])
if not all(k in self.rt_HZ_store for k in self.topics):
hz_status.level = DiagnosticStatus.WARN
hz_status.message = "could not determine type for topics {}. Probably never published on that topics.".format(
self.missing_topics)
array.status.append(hz_status)
self.pub_diagnostics.publish(array)
return
# calculate actual rates
for topic, rt in self.rt_HZ_store.items():
#print("rt.times {}".format(rt.times))
#print("rt.msg_tn {}".format(rt.msg_tn))
#print("rt.last_printed_tn {}".format(rt.last_printed_tn))
#print("rt.delta: {}".format(rt.msg_tn - rt.last_printed_tn))
#print("event.current_real: {}".format(event.current_real.to_sec()))
#print("event.last_real: {}".format(event.last_real.to_sec()))
#print("event.delta: {}".format((event.current_real - event.last_real).to_sec()))
if not rt or not rt.times:
hz_status.level = DiagnosticStatus.ERROR
rates.append(0.0)
consolidated_error_messages[
"never received message for topics"].append(topic)
elif rt.msg_tn == rt.last_printed_tn \
and not (event.current_real.to_sec() < rt.msg_tn + min_duration): # condition to prevent WARN for topics with hz<diagnostics_rate, allow 1/min_rte to pass before reporting error
hz_status.level = DiagnosticStatus.ERROR
rates.append(0.0)
consolidated_error_messages[
"no messages anymore for topics"].append(topic)
else:
with rt.lock: # calculation taken from /opt/ros/indigo/lib/python2.7/dist-packages/rostopic/__init__.py
n = len(rt.times)
mean = sum(rt.times) / n
rate = 1. / mean if mean > 0. else 0
rt.last_printed_tn = rt.msg_tn
rates.append(round(rate, 2))
if min_rate and rate < min_rate:
hz_status.level = DiagnosticStatus.WARN
consolidated_error_messages[
"publishing rate is too low for topics"].append(topic)
if max_rate and rate > max_rate:
hz_status.level = DiagnosticStatus.WARN
consolidated_error_messages[
"publishing rate is too high for topics"].append(topic)
if hz_status.level != DiagnosticStatus.OK:
message = ""
key = "never received message for topics"
if len(consolidated_error_messages[key]) > 0:
message += key + " " + str(
consolidated_error_messages[key]) + ", "
key = "no messages anymore for topics"
if len(consolidated_error_messages[key]) > 0:
message += key + " " + str(
consolidated_error_messages[key]) + ", "
key = "publishing rate is too low for topics"
if len(consolidated_error_messages[key]) > 0:
message += key + " " + str(
consolidated_error_messages[key]) + ", "
key = "publishing rate is too high for topics"
if len(consolidated_error_messages[key]) > 0:
message += key + " " + str(consolidated_error_messages[key])
hz_status.message = message
hz_status.values.append(KeyValue("rates", str(rates)))
array.status.append(hz_status)
self.pub_diagnostics.publish(array)
except ValueError:
rospy.loginfo("Waiting for laptop battery state to become available...")
nextQueryAt = time.time() + 15.0
continue
diagnosticArray = DiagnosticArray()
diagnosticArray.header.stamp = rospy.Time.now()
batteryStatus = DiagnosticStatus()
batteryStatus.name = hostname + " laptop battery power"
batteryStatus.hardware_id = hostname
batteryStatus.message="%.1f%% (%.3fV)" % (batteryPercentage, batteryVoltage)
if batteryPercentage < 20:
batteryStatus.message += "; Critically low power, recharge NOW!"
batteryStatus.level = DiagnosticStatus.ERROR
elif batteryPercentage < 40:
batteryStatus.message += "; Low power, recharge soon!"
batteryStatus.level = DiagnosticStatus.WARN
else:
batteryStatus.message += "; Power level good"
batteryStatus.level = DiagnosticStatus.OK
diagnosticArray.status.append(batteryStatus)
laptopPluggedStatus = DiagnosticStatus()
laptopPluggedStatus.name = hostname + " power supply"
laptopPluggedStatus.hardware_id = hostname
laptopPluggedStatus.message = "Plugged in" if laptopPluggedIn else "Disconnected!"
laptopPluggedStatus.level = DiagnosticStatus.OK if laptopPluggedIn else DiagnosticStatus.WARN
def ntp_monitor(offset=500, self_offset=500, diag_hostname = None, error_offset = 5000000):
pub = rospy.Publisher("/diagnostics", DiagnosticArray, queue_size = 100)
rospy.init_node(NAME, anonymous=True)
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 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)]:
try:
p = Popen(["ntpdate", "-q", host], stdout=PIPE, stdin=PIPE, stderr=PIPE, universal_newlines=True)
res = p.wait()
(o,e) = p.communicate()
except OSError as e:
(errno, msg) = e.args
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]
pub.publish(msg)
time.sleep(1)
def run(self):
"""This function recieves data from the socket and published a IMU message to ROS"""
while not rospy.is_shutdown():
line = self.recv_all()
if line.startswith("#YPRAG="):
line = line.replace("#YPRAG=", "") # Delete "#YPRAG="
else:
continue
#f.write(line) # Write to the output log file
words = string.split(line, ",") # Fields split
#Try to convert words from string to float, if a value doesn't work, continue to the next loop
try:
words = [float(word) for word in words]
except ValueError:
print "Failed to parse data into 9 values.. No big concern"
continue
if len(words) == 9:
#in AHRS firmware z axis points down, in ROS z axis points up (see REP 103)
yaw_deg = -words[0]
yaw_deg = yaw_deg + imu_yaw_calibration
if yaw_deg > 180.0:
yaw_deg = yaw_deg - 360.0
if yaw_deg < -180.0:
yaw_deg = yaw_deg + 360.0
self.yaw = yaw_deg * degrees2rad
#in AHRS firmware y axis points right, in ROS y axis points left (see REP 103)
self.pitch = -words[1] * degrees2rad
self.roll = words[2] * degrees2rad
# AHRS firmware accelerations are negated
# This means y and z are correct for ROS, but x needs reversing
self.imuMsg.linear_acceleration.x = -words[
3] * self.accel_factor
self.imuMsg.linear_acceleration.y = words[4] * self.accel_factor
self.imuMsg.linear_acceleration.z = words[5] * self.accel_factor
self.imuMsg.angular_velocity.x = words[6]
#in AHRS firmware y axis points right, in ROS y axis points left (see REP 103)
self.imuMsg.angular_velocity.y = -words[7]
#in AHRS firmware z axis points down, in ROS z axis points up (see REP 103)
self.imuMsg.angular_velocity.z = -words[8]
q = quaternion_from_euler(self.roll, self.pitch, self.yaw)
self.imuMsg.orientation.x = q[0]
self.imuMsg.orientation.y = q[1]
self.imuMsg.orientation.z = q[2]
self.imuMsg.orientation.w = q[3]
self.imuMsg.header.stamp = rospy.Time.now()
self.imuMsg.header.frame_id = 'base_imu_link'
self.imuMsg.header.seq = self.seq
self.seq = self.seq + 1
#Publish message
self.pub.publish(self.imuMsg)
else:
print "Recieved less than 9 values from socket, not publishing"
if (self.diag_pub_time < rospy.get_time()):
self.diag_pub_time += 1
diag_arr = DiagnosticArray()
diag_arr.header.stamp = rospy.get_rostime()
diag_arr.header.frame_id = '1'
diag_msg = DiagnosticStatus()
diag_msg.name = 'Razor_Imu'
diag_msg.level = DiagnosticStatus.OK
diag_msg.message = 'Received AHRS measurement'
diag_msg.values.append(
KeyValue('roll (deg)', str(self.roll * (180.0 / math.pi))))
diag_msg.values.append(
KeyValue('pitch (deg)',
str(self.pitch * (180.0 / math.pi))))
diag_msg.values.append(
KeyValue('yaw (deg)', str(self.yaw * (180.0 / math.pi))))
diag_msg.values.append(
KeyValue('sequence number', str(self.seq)))
diag_arr.status.append(diag_msg)
self.diag_pub.publish(diag_arr)
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)
NAME = 'ntp_monitor'
def ntp_monitor(offset=500, self_offset=500, diag_hostname = None as error_offset = 5000000):
pub = rospy.Publisher("/diagnostics", DiagnosticArray, queue_size = 100)
rospy.init_node(NAME, anonymous=True)
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 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)]:
try:
def bno08x_node():
# Initialize ROS node
raw_pub = rospy.Publisher('bno08x/raw', Imu, queue_size=10)
mag_pub = rospy.Publisher('bno08x/mag', MagneticField, queue_size=10)
status_pub = rospy.Publisher('bno08x/status',
DiagnosticStatus,
queue_size=10)
rospy.init_node('bno08x')
rate = rospy.Rate(100) # frequency in Hz
rospy.loginfo(rospy.get_caller_id() + " bno08x node launched.")
i2c = busio.I2C(board.SCL, board.SDA, frequency=800000)
bno = BNO08X_I2C(i2c, address=0x4b) # BNO080 (0x4b) BNO085 (0x4a)
bno.enable_feature(BNO_REPORT_ACCELEROMETER)
bno.enable_feature(BNO_REPORT_GYROSCOPE)
bno.enable_feature(BNO_REPORT_MAGNETOMETER)
bno.enable_feature(BNO_REPORT_ROTATION_VECTOR)
time.sleep(0.5) # ensure IMU is initialized
while True:
raw_msg = Imu()
raw_msg.header.stamp = rospy.Time.now()
accel_x, accel_y, accel_z = bno.acceleration
raw_msg.linear_acceleration.x = accel_x
raw_msg.linear_acceleration.y = accel_y
raw_msg.linear_acceleration.z = accel_z
gyro_x, gyro_y, gyro_z = bno.gyro
raw_msg.angular_velocity.x = gyro_x
raw_msg.angular_velocity.y = gyro_y
raw_msg.angular_velocity.z = gyro_z
quat_i, quat_j, quat_k, quat_real = bno.quaternion
raw_msg.orientation.w = quat_i
raw_msg.orientation.x = quat_j
raw_msg.orientation.y = quat_k
raw_msg.orientation.z = quat_real
raw_msg.orientation_covariance[0] = -1
raw_msg.linear_acceleration_covariance[0] = -1
raw_msg.angular_velocity_covariance[0] = -1
raw_pub.publish(raw_msg)
mag_msg = MagneticField()
mag_x, mag_y, mag_z = bno.magnetic
mag_msg.header.stamp = rospy.Time.now()
mag_msg.magnetic_field.x = mag_x
mag_msg.magnetic_field.y = mag_y
mag_msg.magnetic_field.z = mag_z
mag_msg.magnetic_field_covariance[0] = -1
mag_pub.publish(mag_msg)
status_msg = DiagnosticStatus()
status_msg.level = 0
status_msg.name = "bno08x IMU"
status_msg.message = ""
status_pub.publish(status_msg)
rate.sleep()
rospy.loginfo(rospy.get_caller_id() + " bno08x node finished")
def _update_diagnostics_state(self):
md5_warnings = {}
ttype_warnings = {}
for mname, mth in self.masters.items():
warnings = mth.get_md5warnigs()
if warnings:
md5_warnings[mname] = warnings
twarnings = mth.get_topic_type_warnings()
if twarnings:
ttype_warnings[mname] = twarnings
level = 0
if md5_warnings or ttype_warnings:
level = 1
if self._current_diagnistic_level != level:
da = DiagnosticArray()
if md5_warnings or ttype_warnings:
# add warnings for all hosts with topic types with different md5sum
for mname, warnings in md5_warnings.items():
diag_state = DiagnosticStatus()
diag_state.level = level
diag_state.name = rospy.get_name()
diag_state.message = 'Wrong topic md5sum @ %s and %s' % (
mname, self._localname)
diag_state.hardware_id = self.hostname
for (topicname, _node,
_nodeuri), tmtype in warnings.items():
if isinstance(tmtype, tuple):
(md5sum, ttype) = tmtype
if md5sum is not None:
key = KeyValue()
key.key = topicname
key.value = str(ttype)
diag_state.values.append(key)
da.status.append(diag_state)
# add warnings if a topic with different type is synchrinozied to local host
for mname, warnings in ttype_warnings.items():
diag_state = DiagnosticStatus()
diag_state.level = level
diag_state.name = rospy.get_name()
diag_state.message = 'Wrong topics type @ %s and %s' % (
mname, self._localname)
diag_state.hardware_id = self.hostname
for (topicname, _node,
_nodeuri), tmtype in warnings.items():
ttype = tmtype
if isinstance(tmtype, tuple):
(md5sum, ttype) = tmtype
key = KeyValue()
key.key = topicname
key.value = str(ttype)
diag_state.values.append(key)
da.status.append(diag_state)
else:
# clear all warnings
diag_state = DiagnosticStatus()
diag_state.level = 0
diag_state.name = rospy.get_name()
diag_state.message = ""
diag_state.hardware_id = self.hostname
da.status.append(diag_state)
da.header.stamp = rospy.Time.now()
self.pub_diag.publish(da)
self._current_diagnistic_level = level
