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 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 _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 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 update_diagnostics(self):
da = DiagnosticArray()
ds = DiagnosticStatus()
ds.name = rospy.get_caller_id().lstrip('/') + ": Tasks"
in_progress = 0;
longest_interval = 0;
for updater in list(asynchronous_updaters):
(name, interval) = updater.getStatus()
if interval == 0:
msg = "Idle"
else:
in_progress = in_progress + 1
msg = "Update in progress (%i s)"%interval
longest_interval = max(interval, longest_interval)
ds.values.append(KeyValue(name, msg))
if in_progress == 0:
ds.message = "Idle"
else:
ds.message = "Updates in progress: %i"%in_progress
if longest_interval > 10:
ds.level = 1
ds.message = "Update is taking too long: %i"%in_progress
ds.hardware_id = "none"
da.status.append(ds)
da.header.stamp = rospy.get_rostime()
self.diagnostic_pub.publish(da)
def publish_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 _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 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 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 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 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 make_status_msg(count):
array = DiagnosticArray()
stat = DiagnosticStatus()
stat.level = 0
stat.message = 'OK'
stat.name = 'Unit Test'
stat.hardware_id = 'HW ID'
stat.values = [
KeyValue('Value A', str(count)),
KeyValue('Value B', str(count)),
KeyValue('Value C', str(count))]
array.status = [ stat ]
return array
def publish(pub, info):
diag = DiagnosticArray()
diag.header.stamp = rospy.Time.now()
status = DiagnosticStatus()
status.hardware_id = "wifi"
status.name = 'wifi_scan'
status.level = status.OK
status.message = pformat(info)
for k,v in info.items():
status.values.append(
KeyValue(k,str(v)),
)
diag.status = [status]
pub.publish(diag)
def 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 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 getEtherbotixDiagnostics(self, etherbotix):
msg = DiagnosticStatus()
msg.name = "etherbotix"
msg.level = DiagnosticStatus.OK
msg.message = "OK"
msg.hardware_id = etherbotix.getUniqueId()
# System Voltage
if etherbotix.system_voltage < 10.0:
msg.level = DiagnosticStatus.ERROR
msg.message = "Battery depleted!"
msg.values.append(KeyValue("Voltage", str(etherbotix.system_voltage)+"V"))
# Currents
msg.values.append(KeyValue("Servo Current", str(etherbotix.servo_current)+"A"))
msg.values.append(KeyValue("Aux. Current", str(etherbotix.aux_current)+"A"))
msg.values.append(KeyValue("Packets", str(etherbotix.packets_recv)))
msg.values.append(KeyValue("Packets Bad", str(etherbotix.packets_bad)))
return msg
def _laptop_charge_to_diag(laptop_msg):
rv = DiagnosticStatus()
rv.level = DiagnosticStatus.OK
rv.message = 'OK'
rv.name = 'Laptop Battery'
rv.hardware_id = socket.gethostname()
if not laptop_msg.present:
rv.level = DiagnosticStatus.ERROR
rv.message = 'Laptop battery missing'
rv.values.append(KeyValue('Voltage (V)', str(laptop_msg.voltage)))
rv.values.append(KeyValue('Current (A)', str(laptop_msg.rate)))
rv.values.append(KeyValue('Charge (Ah)', str(laptop_msg.charge)))
rv.values.append(KeyValue('Capacity (Ah)', str(laptop_msg.capacity)))
rv.values.append(KeyValue('Design Capacity (Ah)', str(laptop_msg.design_capacity)))
return rv
def gpu_status_to_diag(gpu_stat):
stat = DiagnosticStatus()
stat.name = 'GPU Status'
stat.message = 'OK'
stat.level = DiagnosticStatus.OK
stat.hardware_id = gpu_stat.pci_device_id
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))
# Check for valid data
if not gpu_stat.product_name or not gpu_stat.pci_device_id:
stat.level = DiagnosticStatus.ERROR
stat.message = 'No Device Data'
return stat
# Check load
if gpu_stat.gpu_usage > 98:
stat.level = max(stat.level, DiagnosticStatus.WARN)
stat.message = 'High Load'
# Check thresholds
if gpu_stat.temperature > 90:
stat.level = max(stat.level, DiagnosticStatus.WARN)
stat.message = 'High Temperature'
if gpu_stat.temperature > 95:
stat.level = max(stat.level, DiagnosticStatus.ERROR)
stat.message = 'Temperature Alarm'
# Check fan
if gpu_stat.fan_speed == 0:
stat.level = max(stat.level, DiagnosticStatus.ERROR)
stat.message = 'No Fan Speed'
return stat
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 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 create_nuc_status(self):
diag_status = DiagnosticStatus()
diag_status.name = "BHG: NUC Status"
diag_status.message = "BHG: Normal"
diag_status.hardware_id = socket.gethostname()
temperature_case_kv = KeyValue()
temperature_case_kv.key = 'temperature_case'
temperature_case_kv.value = str(self.temperature_case)
diag_status.values.append(temperature_case_kv)
disk_usage_kv = KeyValue()
self.get_disk_usage()
disk_usage_kv.key = 'disk_usage'
disk_usage_kv.value = str(self.disk_usage)
diag_status.values.append(disk_usage_kv)
is_recording_kv = KeyValue()
is_recording_kv.key = 'is_recording'
is_recording_kv.value = str(self.is_recording)
diag_status.values.append(is_recording_kv)
return diag_status
def _publish_diagnostic(self):
"""Publish diagnostics."""
diagnostic = DiagnosticArray()
diagnostic.header.stamp = rospy.Time.now()
status = DiagnosticStatus()
status.name = "LocalXY Origin"
status.hardware_id = "origin_publisher"
if self.origin is None:
status.level = DiagnosticStatus.ERROR
status.message = "No Origin"
else:
if self.origin_source == "gpsfix":
status.level = DiagnosticStatus.OK
status.message = "Has Origin (GPSFix)"
elif self.origin_source == "navsat":
status.level = DiagnosticStatus.OK
status.message = "Has Origin (NavSatFix)"
elif self.origin_source == "custom":
status.level = DiagnosticStatus.OK
status.message = "Has Origin (Custom Topic)"
else:
status.level = DiagnosticStatus.WARN
status.message = "Origin Was Set Manually"
frame_id = self.origin.header.frame_id
status.values.append(
KeyValue(key="Origin Frame ID", value=frame_id))
latitude = "%f" % self.origin.pose.position.y
status.values.append(KeyValue(key="Latitude", value=latitude))
longitude = "%f" % self.origin.pose.position.x
status.values.append(KeyValue(key="Longitude", value=longitude))
altitude = "%f" % self.origin.pose.position.z
status.values.append(KeyValue(key="Altitude", value=altitude))
diagnostic.status.append(status)
self.diagnostic_pub.publish(diagnostic)
def _laptop_charge_to_diag(laptop_msg):
rv = DiagnosticStatus()
rv.level = DiagnosticStatus.OK
rv.message = 'OK'
rv.name = 'Laptop Battery'
rv.hardware_id = socket.gethostname()
if not laptop_msg.present:
rv.level = DiagnosticStatus.ERROR
rv.message = 'Laptop battery missing'
rv.values.append(KeyValue('Voltage (V)', str(laptop_msg.voltage)))
rv.values.append(KeyValue('Current (A)', str(laptop_msg.current)))
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('Percentage (%)', str(laptop_msg.percentage)))
rv.values.append(
KeyValue('Charging state',
val_to_state[laptop_msg.power_supply_status]))
return rv
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 create_fcu_status(self):
diag_status = DiagnosticStatus()
diag_status.name = "BHG: FCU Status"
diag_status.message = "BHG: Normal"
diag_status.hardware_id = "Pixhawk"
gps_fix_type_kv = KeyValue()
gps_fix_type_kv.key = 'gps_fix_type'
gps_fix_type_kv.value = self.fixtype_convert(self.gps_fix_type)
diag_status.values.append(gps_fix_type_kv)
gps_num_sv_kv = KeyValue()
gps_num_sv_kv.key = 'gps_num_sv'
gps_num_sv_kv.value = str(self.gps_num_sv)
diag_status.values.append(gps_num_sv_kv)
fcu_mode_kv = KeyValue()
fcu_mode_kv.key = 'fcu_mode'
fcu_mode_kv.value = str(self.fcu_mode)
diag_status.values.append(fcu_mode_kv)
fcu_voltage_kv = KeyValue()
fcu_voltage_kv.key = 'fcu_voltage'
fcu_voltage_kv.value = str(self.fcu_voltage)
diag_status.values.append(fcu_voltage_kv)
fcu_amps_kv = KeyValue()
fcu_amps_kv.key = 'fcu_amps'
fcu_amps_kv.value = str(self.fcu_amps)
diag_status.values.append(fcu_amps_kv)
fcu_cpu_load_kv = KeyValue()
fcu_cpu_load_kv.key = 'fcu_cpu_load'
fcu_cpu_load_kv.value = str(self.fcu_cpu_load)
diag_status.values.append(fcu_cpu_load_kv)
return diag_status
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()
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 diagnostics(self):
ds = DiagnosticStatus()
ds.name = rospy.get_caller_id().lstrip('/') + ": " + "Network Watchdog"
ds.hardware_id = "none"
with self.mutex:
now = rospy.get_time()
time_to_timeout = self.timeout - (now - self.interruption_time)
elapsed_orig = now - self.interruption_time_orig
# Parameters
ds.values.append(KeyValue("Timeout (s)", "%.1f" % self.timeout))
ds.values.append(
KeyValue("Latency threshold (ms)",
str(int(self.max_latency * 1000))))
ds.values.append(
KeyValue("Loss threshold (%)", "%.1f" % (self.max_loss * 100)))
ds.values.append(KeyValue("Timeout command",
str(self.action_name)))
# Network outage stats
ds.values.append(
KeyValue("Network is down", str(self.outage_in_progress)))
ds.values.append(KeyValue("Outage count", str(self.outage_count)))
if self.outage_in_progress:
ds.values.append(
KeyValue("Time since outage start (s)",
"%.1f" % elapsed_orig))
if self.outage_length:
ds.values.append(
KeyValue("Latest outage length",
"%.1f" % self.outage_length))
ds.values.append(
KeyValue("Longest outage length (s)",
"%.1f" % self.longest_outage))
# Command stats
ds.values.append(
KeyValue("Timeout command in progress",
str(self.action_running)))
ds.values.append(KeyValue("Trigger count",
str(self.trigger_count)))
if self.outage_in_progress:
if not self.action_running:
ds.values.append(
KeyValue("Time to timeout (s)",
"%.1f" % (time_to_timeout)))
if self.action_running:
ds.values.append(
KeyValue("Time since trigger",
str(now - self.latest_trigger_time)))
if self.longest_action_time:
ds.values.append(
KeyValue("Longest timeout command run time",
"%.1f" % self.longest_action_duration))
# Stats
ds.values.append(
KeyValue("Node start time", time.ctime(self.start_time)))
if self.interruption_time_orig != inf:
ds.values.append(
KeyValue("Latest outage start time",
time.ctime(self.interruption_time_orig)))
if self.longest_outage_time:
ds.values.append(
KeyValue("Longest outage start time",
time.ctime(self.longest_outage_time)))
if self.latest_trigger_time:
ds.values.append(
KeyValue("Latest trigger time",
time.ctime(self.latest_trigger_time)))
if self.longest_action_time:
ds.values.append(
KeyValue("Time of longest timeout command",
time.ctime(self.longest_action_time)))
# Summary
if self.interruption_time == inf:
ds.message = "Network is up"
ds.level = DiagnosticStatus.OK
if self.action_running:
ds.message += "; Timeout command in progress (%.0f s)" % (
now - self.latest_trigger_time)
else:
if self.action_running:
ds.message = "Timeout command in progress (%.0f s; %.0f s)" % (
elapsed_orig, now - self.latest_trigger_time)
ds.level = DiagnosticStatus.ERROR
else:
ds.message = "Network is down (%.0f s; %.0f s)" % (
elapsed_orig, time_to_timeout)
ds.level = DiagnosticStatus.WARN
da = DiagnosticArray()
da.header.stamp = rospy.get_rostime()
da.status.append(ds)
self.diag_pub.publish(da)
def main():
rclpy.init(args=None)
node = Node('system_monitor')
pub = node.create_publisher(WorkloadMsg, 'system_workload', 1)
diagnostic_pub = node.create_publisher(DiagnosticArray, 'diagnostics', 1)
hostname = socket.gethostname()
diag_array = DiagnosticArray()
diag_cpu = DiagnosticStatus()
# start all names with "SYSTEM" for diagnostic analysesr
diag_cpu.name = "SYSTEMCPU"
diag_cpu.hardware_id = "CPU"
diag_mem = DiagnosticStatus()
diag_mem.name = "SYSTEMMemory"
diag_cpu.hardware_id = "Memory"
node.declare_parameter('update_frequency', 10.0)
node.declare_parameter('do_memory', True)
node.declare_parameter('do_cpu', True)
node.declare_parameter('cpu_load_percentage', 80.0)
node.declare_parameter('memoroy_load_percentage', 80.0)
node.declare_parameter('network_rate_received_errors', 10.0)
node.declare_parameter('network_rate_send_errors', 10.0)
rate = node.get_parameter('update_frequency').get_parameter_value().double_value
do_memory = node.get_parameter('do_memory').get_parameter_value().bool_value
do_cpu = node.get_parameter('do_cpu').get_parameter_value().bool_value
cpu_load_percentage = node.get_parameter('cpu_load_percentage').get_parameter_value().double_value
memoroy_load_percentage = node.get_parameter('memoroy_load_percentage').get_parameter_value().double_value
network_rate_received_errors = node.get_parameter(
'network_rate_received_errors').get_parameter_value().double_value
network_rate_send_errors = node.get_parameter('network_rate_send_errors').get_parameter_value().double_value
while rclpy.ok():
last_send_time = time.time()
running_processes, cpu_usages, overall_usage_percentage = cpus.collect_all() if do_cpu else (
-1, [], 0)
memory_available, memory_used, memory_total = memory.collect_all() if do_memory else (-1, -1, -1)
interfaces = network_interfaces.collect_all(node.get_clock())
msg = WorkloadMsg(
hostname=hostname,
cpus=cpu_usages,
running_processes=running_processes,
cpu_usage_overall=overall_usage_percentage,
memory_available=memory_available,
memory_used=memory_used,
memory_total=memory_total,
filesystems=[],
network_interfaces=interfaces
)
pub.publish(msg)
diag_array.status = []
# publish diagnostic message to show this in the rqt diagnostic viewer
diag_cpu.message = str(overall_usage_percentage) + "%"
if overall_usage_percentage >= cpu_load_percentage:
diag_cpu.level = DiagnosticStatus.WARN
else:
diag_cpu.level = DiagnosticStatus.OK
diag_array.status.append(diag_cpu)
memory_usage = round((memory_used / memory_total) * 100, 2)
diag_mem.message = str(memory_usage) + "%"
if memory_usage >= memoroy_load_percentage:
diag_mem.level = DiagnosticStatus.WARN
else:
diag_mem.level = DiagnosticStatus.OK
diag_array.status.append(diag_mem)
for interface in interfaces:
diag_net = DiagnosticStatus()
diag_net.name = "SYSTEM" + interface.name
diag_net.hardware_id = interface.name
if interface.rate_received_packets_errors >= network_rate_received_errors \
or interface.rate_sent_packets_errors >= network_rate_send_errors:
diag_net.level = DiagnosticStatus.WARN
else:
diag_net.level = DiagnosticStatus.OK
diag_array.status.append(diag_net)
diag_array.header.stamp = node.get_clock().now().to_msg()
diagnostic_pub.publish(diag_array)
# sleep to have correct rate. we dont use ROS time since we are interested in system things
dt = time.time() - last_send_time
time.sleep(max(0, (1 / rate) - dt))
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)
batteryVoltagePublisher = rospy.Publisher("/spencer/sensors/battery/voltage", Float32, latch=True, queue_size=1)
batteryPercentagePublisher = rospy.Publisher("/spencer/sensors/battery/percentage", Float32, latch=True, queue_size=1)
rospy.loginfo("Starting robot battery monitor...")
while not rospy.is_shutdown():
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)
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)
batteryPercentagePublisher = rospy.Publisher(
"/spencer/sensors/battery/percentage", Float32, latch=True, queue_size=1)
rospy.loginfo("Starting robot battery monitor...")
while not rospy.is_shutdown():
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)
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)
def __init__(self, coz, node_name):
"""
:type coz: cozmo.Robot
:param coz: The cozmo SDK robot handle (object).
:type node_name: String
:param node_name: Name for the node
"""
# initialize node
super().__init__(node_name)
# node timer
self._timer_rate = 10 # Hz
self.timer = self.create_timer(1 / self._timer_rate,
self.timer_callback)
# vars
self._cozmo = coz
self._lin_vel = .0
self._ang_vel = .0
self._cmd_lin_vel = .0
self._cmd_ang_vel = .0
self._last_pose = self._cozmo.pose
self._wheel_vel = (0, 0)
self._optical_frame_orientation = quaternion_from_euler(
-np.pi / 2., .0, -np.pi / 2.)
self._camera_info_manager = CameraInfoManager(
self, 'cozmo_camera', namespace='/cozmo_camera')
# tf
self._tfb = TransformBroadcaster(self)
# params
## TODO: use rosparam when rclpy supports it
#self._odom_frame = rospy.get_param('~odom_frame', 'odom')
self._odom_frame = 'odom'
#self._footprint_frame = rospy.get_param('~footprint_frame', 'base_footprint')
self._footprint_frame = 'base_footprint'
#self._base_frame = rospy.get_param('~base_frame', 'base_link')
self._base_frame = 'base_link'
#self._head_frame = rospy.get_param('~head_frame', 'head_link')
self._head_frame = 'head_link'
#self._camera_frame = rospy.get_param('~camera_frame', 'camera_link')
self._camera_frame = 'camera_link'
#self._camera_optical_frame = rospy.get_param('~camera_optical_frame', 'cozmo_camera')
self._camera_optical_frame = 'cozmo_camera'
#camera_info_url = rospy.get_param('~camera_info_url', '')
camera_info_url = 'file://' + os.path.dirname(
os.path.abspath(__file__)) + '/config/cozmo_camera.yaml'
# pubs
#self._joint_state_pub = rospy.Publisher('joint_states', JointState, queue_size=1)
self._joint_state_pub = self.create_publisher(JointState,
'joint_states')
#self._odom_pub = rospy.Publisher('odom', Odometry, queue_size=1)
self._odom_pub = self.create_publisher(Odometry, 'odom')
#self._imu_pub = rospy.Publisher('imu', Imu, queue_size=1)
self._imu_pub = self.create_publisher(Imu, 'imu')
#self._battery_pub = rospy.Publisher('battery', BatteryState, queue_size=1)
self._battery_pub = self.create_publisher(BatteryState, 'battery')
# Note: camera is published under global topic (preceding "/")
#self._image_pub = rospy.Publisher('/cozmo_camera/image', Image, queue_size=10)
self._image_pub = self.create_publisher(Image, '/cozmo_camera/image')
#self._camera_info_pub = rospy.Publisher('/cozmo_camera/camera_info', CameraInfo, queue_size=10)
self._camera_info_pub = self.create_publisher(
CameraInfo, '/cozmo_camera/camera_info')
# subs
#self._backpack_led_sub = rospy.Subscriber(
# 'backpack_led', ColorRGBA, self._set_backpack_led, queue_size=1)
self._backpack_led_sub = self.create_subscription(
ColorRGBA, 'backpack_led', self._set_backpack_led)
#self._twist_sub = rospy.Subscriber('cmd_vel', Twist, self._twist_callback, queue_size=1)
self._twist_sub = self.create_subscription(Twist, 'cmd_vel',
self._twist_callback)
#self._say_sub = rospy.Subscriber('say', String, self._say_callback, queue_size=1)
self._say_sub = self.create_subscription(String, 'say',
self._say_callback)
#self._head_sub = rospy.Subscriber('head_angle', Float64, self._move_head, queue_size=1)
self._head_sub = self.create_subscription(Float64, 'head_angle',
self._move_head)
#self._lift_sub = rospy.Subscriber('lift_height', Float64, self._move_lift, queue_size=1)
self._lift_sub = self.create_subscription(Float64, 'lift_height',
self._move_lift)
# diagnostics
self._diag_array = DiagnosticArray()
self._diag_array.header.frame_id = self._base_frame
diag_status = DiagnosticStatus()
diag_status.hardware_id = 'Cozmo Robot'
diag_status.name = 'Cozmo Status'
diag_status.values.append(KeyValue(key='Battery Voltage', value=''))
diag_status.values.append(KeyValue(key='Head Angle', value=''))
diag_status.values.append(KeyValue(key='Lift Height', value=''))
self._diag_array.status.append(diag_status)
#self._diag_pub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=1)
self._diag_pub = self.create_publisher(DiagnosticArray, '/diagnostics')
# camera info manager
self._camera_info_manager.setURL(camera_info_url)
self._camera_info_manager.loadCameraInfo()
def diagnostics(self):
ds = DiagnosticStatus()
ds.name = rospy.get_caller_id().lstrip('/') + ": " + "Network Watchdog"
ds.hardware_id = "none"
with self.mutex:
now = rospy.get_time()
time_to_timeout = self.timeout - (now - self.interruption_time)
elapsed_orig = now - self.interruption_time_orig
# Parameters
ds.values.append(KeyValue("Timeout (s)", "%.1f"%self.timeout))
ds.values.append(KeyValue("Latency threshold (ms)", str(int(self.max_latency * 1000))))
ds.values.append(KeyValue("Loss threshold (%)", "%.1f"%(self.max_loss * 100)))
ds.values.append(KeyValue("Timeout command", str(self.action_name)))
# Network outage stats
ds.values.append(KeyValue("Network is down", str(self.outage_in_progress)))
ds.values.append(KeyValue("Outage count", str(self.outage_count)))
if self.outage_in_progress:
ds.values.append(KeyValue("Time since outage start (s)", "%.1f"%elapsed_orig))
if self.outage_length:
ds.values.append(KeyValue("Latest outage length", "%.1f"%self.outage_length))
ds.values.append(KeyValue("Longest outage length (s)", "%.1f"%self.longest_outage))
# Command stats
ds.values.append(KeyValue("Timeout command in progress", str(self.action_running)))
ds.values.append(KeyValue("Trigger count", str(self.trigger_count)))
if self.outage_in_progress:
if not self.action_running:
ds.values.append(KeyValue("Time to timeout (s)", "%.1f"%(time_to_timeout)))
if self.action_running:
ds.values.append(KeyValue("Time since trigger", str(now - self.latest_trigger_time)))
if self.longest_action_time:
ds.values.append(KeyValue("Longest timeout command run time", "%.1f"%self.longest_action_duration))
# Stats
ds.values.append(KeyValue("Node start time", time.ctime(self.start_time)))
if self.interruption_time_orig != inf:
ds.values.append(KeyValue("Latest outage start time", time.ctime(self.interruption_time_orig)))
if self.longest_outage_time:
ds.values.append(KeyValue("Longest outage start time", time.ctime(self.longest_outage_time)))
if self.latest_trigger_time:
ds.values.append(KeyValue("Latest trigger time", time.ctime(self.latest_trigger_time)))
if self.longest_action_time:
ds.values.append(KeyValue("Time of longest timeout command", time.ctime(self.longest_action_time)))
# Summary
if self.interruption_time == inf:
ds.message = "Network is up"
ds.level = DiagnosticStatus.OK
if self.action_running:
ds.message += "; Timeout command in progress (%.0f s)"%(now - self.latest_trigger_time)
else:
if self.action_running:
ds.message = "Timeout command in progress (%.0f s; %.0f s)"%(elapsed_orig, now - self.latest_trigger_time)
ds.level = DiagnosticStatus.ERROR
else:
ds.message = "Network is down (%.0f s; %.0f s)"%(elapsed_orig, time_to_timeout)
ds.level = DiagnosticStatus.WARN
da = DiagnosticArray()
da.header.stamp = rospy.get_rostime()
da.status.append(ds)
self.diag_pub.publish(da)
averageDelay += delay
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))
while not rospy.is_shutdown():
if time.time() > nextQueryAt:
try:
batteryVoltage, batteryPercentage, laptopPluggedIn = getBatteryState()
nextQueryAt = time.time() + 15.0 # query interval
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()
import time
from water_sampler_ros.srv import *
# Initialize pumps object
pumps = Sampler()
# Diagnostic status for services
fill_pump_status = DiagnosticStatus()
check_pump_status = DiagnosticStatus()
empty_pump_status = DiagnosticStatus()
stop_pump_status = DiagnosticStatus()
fill_pump_status.name = 'sampler fill pump service'
check_pump_status.name = 'sampler check pump service'
empty_pump_status.name = 'sampler empty pump service'
stop_pump_status.name = 'sampler stop pump service'
fill_pump_status.hardware_id = 'sampler'
check_pump_status.hardware_id = 'sampler'
empty_pump_status.hardware_id = 'sampler'
stop_pump_status.hardware_id = 'sampler'
def check_status(status, srv):
# Check if service exists in ROS services
if any(srv in s for s in rosservice.get_service_list()):
status.level = DiagnosticStatus.OK
status.message = "OK"
status.values = [KeyValue(key="Update Status", value="OK")]
else:
status.level = DiagnosticStatus.ERROR
status.message = "Error"
status.values = [
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()
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(KeyValue('Average interval between messages', "%.3f sec" % averageInterval))
if hasHeader:
topicStatus.values.append(KeyValue('Maximum delay wrt. current ROS time', "%.3f sec" % maxDelay))
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 add_sentence(self, nmea_string, frame_id, timestamp=None):
if not check_nmea_checksum(nmea_string):
rospy.logwarn("Received a sentence with an invalid checksum. " +
"Sentence was: %s" % repr(nmea_string))
return False
parsed_sentence = libnmea_navsat_driver.parser.parse_nmea_sentence(
nmea_string)
if not parsed_sentence:
rospy.logdebug("Failed to parse NMEA sentence. Sentence was: %s" %
nmea_string)
return False
if timestamp:
current_time = timestamp
else:
current_time = rospy.get_rostime()
current_fix = NavSatFix()
current_fix.header.stamp = current_time
current_fix.header.frame_id = frame_id
current_pose_utm = GpsLocal()
current_pose_utm.header.stamp = current_time
current_pose_utm.header.frame_id = frame_id
current_time_ref = TimeReference()
current_time_ref.header.stamp = current_time
current_time_ref.header.frame_id = frame_id
if self.time_ref_source:
current_time_ref.source = self.time_ref_source
else:
current_time_ref.source = frame_id
if not self.use_RMC and 'GGA' in parsed_sentence:
self.seq = self.seq + 1
current_fix.position_covariance_type = \
NavSatFix.COVARIANCE_TYPE_APPROXIMATED
data = parsed_sentence['GGA']
gps_qual = data['fix_type']
try:
# Unpack the fix params for this quality value
current_fix.status.status, self.default_epe, self.fix_type = self.fix_mappings[
gps_qual]
except KeyError:
current_fix.status.status = NavSatStatus.STATUS_NO_FIX
self.fix_type = 'Unknown'
if gps_qual == 0:
current_fix.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN
self.using_receiver_epe = False
self.invalid_cnt += 1
current_fix.status.service = NavSatStatus.SERVICE_GPS
current_fix.header.stamp = current_time
latitude = data['latitude']
if data['latitude_direction'] == 'S':
latitude = -latitude
current_fix.latitude = latitude
longitude = data['longitude']
if data['longitude_direction'] == 'W':
longitude = -longitude
current_fix.longitude = longitude
# use default epe std_dev unless we've received a GST sentence with epes
if not self.using_receiver_epe or math.isnan(self.lon_std_dev):
self.lon_std_dev = self.default_epe
if not self.using_receiver_epe or math.isnan(self.lat_std_dev):
self.lat_std_dev = self.default_epe
if not self.using_receiver_epe or math.isnan(self.alt_std_dev):
self.alt_std_dev = self.default_epe * 2
hdop = data['hdop']
current_fix.position_covariance[0] = (hdop * self.lon_std_dev)**2
current_fix.position_covariance[4] = (hdop * self.lat_std_dev)**2
current_fix.position_covariance[8] = (hdop *
self.alt_std_dev)**2 # FIXME
# Altitude is above ellipsoid, so adjust for mean-sea-level
altitude = data['altitude'] + data['mean_sea_level']
current_fix.altitude = altitude
self.fix_pub.publish(current_fix)
if not math.isnan(latitude) and not math.isnan(longitude) and (
gps_qual == 5 or gps_qual == 4):
UTMNorthing, UTMEasting = LLtoUTM(latitude, longitude)[0:2]
current_pose_utm.position.x = UTMEasting
current_pose_utm.position.y = UTMNorthing
current_pose_utm.position.z = altitude
# Pose x/y/z covariance is whatever we decided h & v covariance is.
# Here is it the same as for ECEF coordinates
current_pose_utm.covariance[0] = (hdop * self.lon_std_dev)**2
current_pose_utm.covariance[4] = (hdop * self.lat_std_dev)**2
current_pose_utm.covariance[8] = (hdop * self.alt_std_dev)**2
current_pose_utm.rtk_fix = True if gps_qual == 4 else False
self.local_pub.publish(current_pose_utm)
if not math.isnan(data['utc_time']):
current_time_ref.time_ref = rospy.Time.from_sec(
data['utc_time'])
self.time_ref_pub.publish(current_time_ref)
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 = frame_id
diag_msg = DiagnosticStatus()
diag_msg.name = 'GPS_status'
diag_msg.hardware_id = 'GPS'
diag_msg.level = DiagnosticStatus.OK
diag_msg.message = 'Received GGA Fix'
diag_msg.values.append(
KeyValue('Sequence number', str(self.seq)))
diag_msg.values.append(
KeyValue('Invalid fix count', str(self.invalid_cnt)))
diag_msg.values.append(KeyValue('Latitude', str(latitude)))
diag_msg.values.append(KeyValue('Longitude', str(longitude)))
diag_msg.values.append(KeyValue('Altitude', str(altitude)))
diag_msg.values.append(KeyValue('GPS quality', str(gps_qual)))
diag_msg.values.append(KeyValue('Fix type', self.fix_type))
diag_msg.values.append(
KeyValue('Number of satellites',
str(data['num_satellites'])))
diag_msg.values.append(
KeyValue('Receiver providing accuracy',
str(self.using_receiver_epe)))
diag_msg.values.append(KeyValue('Hdop', str(hdop)))
diag_msg.values.append(
KeyValue('Latitude std dev', str(hdop * self.lat_std_dev)))
diag_msg.values.append(
KeyValue('Longitude std dev',
str(hdop * self.lon_std_dev)))
diag_msg.values.append(
KeyValue('Altitude std dev', str(hdop * self.alt_std_dev)))
diag_arr.status.append(diag_msg)
self.diag_pub.publish(diag_arr)
elif 'RMC' in parsed_sentence:
data = parsed_sentence['RMC']
# Only publish a fix from RMC if the use_RMC flag is set.
if self.use_RMC:
if data['fix_valid']:
current_fix.status.status = NavSatStatus.STATUS_FIX
else:
current_fix.status.status = NavSatStatus.STATUS_NO_FIX
current_fix.status.service = NavSatStatus.SERVICE_GPS
latitude = data['latitude']
if data['latitude_direction'] == 'S':
latitude = -latitude
current_fix.latitude = latitude
longitude = data['longitude']
if data['longitude_direction'] == 'W':
longitude = -longitude
current_fix.longitude = longitude
current_fix.altitude = float('NaN')
current_fix.position_covariance_type = \
NavSatFix.COVARIANCE_TYPE_UNKNOWN
self.fix_pub.publish(current_fix)
if not math.isnan(data['utc_time']):
current_time_ref.time_ref = rospy.Time.from_sec(
data['utc_time'])
self.time_ref_pub.publish(current_time_ref)
# Publish velocity from RMC regardless, since GGA doesn't provide it.
if data['fix_valid']:
current_vel = TwistStamped()
current_vel.header.stamp = current_time
current_vel.header.frame_id = frame_id
current_vel.twist.linear.x = data['speed'] * \
math.sin(data['true_course'])
current_vel.twist.linear.y = data['speed'] * \
math.cos(data['true_course'])
self.vel_pub.publish(current_vel)
elif 'GST' in parsed_sentence:
data = parsed_sentence['GST']
# Use receiver-provided error estimate if available
self.using_receiver_epe = True
self.lon_std_dev = data['lon_std_dev']
self.lat_std_dev = data['lat_std_dev']
self.alt_std_dev = data['alt_std_dev']
else:
return False
#!/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 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 __init__(self, coz):
"""
:type coz: cozmo.Robot
:param coz: The cozmo SDK robot handle (object).
"""
# vars
self._cozmo = coz
self._lin_vel = .0
self._ang_vel = .0
self._cmd_lin_vel = .0
self._cmd_ang_vel = .0
self._last_pose = self._cozmo.pose
self._wheel_vel = (0, 0)
self._optical_frame_orientation = quaternion_from_euler(-np.pi/2., .0, -np.pi/2.)
self._camera_info_manager = CameraInfoManager('cozmo_camera', namespace='/cozmo_camera')
self.loc1Found=False
self.loc2Found=False
self.loc3Found=False
self.rampFound=False
self.allLocFound=False
# tf
self._tfb = TransformBroadcaster()
# params
self._odom_frame = rospy.get_param('~odom_frame', 'odom')
self._footprint_frame = rospy.get_param('~footprint_frame', 'base_footprint')
self._base_frame = rospy.get_param('~base_frame', 'base_link')
self._head_frame = rospy.get_param('~head_frame', 'head_link')
self._camera_frame = rospy.get_param('~camera_frame', 'camera_link')
self._camera_optical_frame = rospy.get_param('~camera_optical_frame', 'cozmo_camera')
camera_info_url = rospy.get_param('~camera_info_url', '')
# pubs
self._joint_state_pub = rospy.Publisher('joint_states', JointState, queue_size=1)
self._odom_pub = rospy.Publisher('odom', Odometry, queue_size=1)
self._imu_pub = rospy.Publisher('imu', Imu, queue_size=1)
self._battery_pub = rospy.Publisher('battery', BatteryState, queue_size=1)
# Note: camera is published under global topic (preceding "/")
self._image_pub = rospy.Publisher('/cozmo_camera/image', Image, queue_size=10)
self._camera_info_pub = rospy.Publisher('/cozmo_camera/camera_info', CameraInfo, queue_size=10)
# subs
self._backpack_led_sub = rospy.Subscriber(
'backpack_led', ColorRGBA, self._set_backpack_led, queue_size=1)
self._twist_sub = rospy.Subscriber('cmd_vel', Twist, self._twist_callback, queue_size=1)
self._say_sub = rospy.Subscriber('say', String, self._say_callback, queue_size=1)
self._head_sub = rospy.Subscriber('head_angle', Float64, self._move_head, queue_size=1)
self._lift_sub = rospy.Subscriber('lift_height', Float64, self._move_lift, queue_size=1)
# diagnostics
self._diag_array = DiagnosticArray()
self._diag_array.header.frame_id = self._base_frame
diag_status = DiagnosticStatus()
diag_status.hardware_id = 'Cozmo Robot'
diag_status.name = 'Cozmo Status'
diag_status.values.append(KeyValue(key='Battery Voltage', value=''))
diag_status.values.append(KeyValue(key='Head Angle', value=''))
diag_status.values.append(KeyValue(key='Lift Height', value=''))
self._diag_array.status.append(diag_status)
self._diag_pub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=1)
# camera info manager
self._camera_info_manager.setURL(camera_info_url)
self._camera_info_manager.loadCameraInfo()
self.twist = Twist()
custom_objects(self._cozmo)
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)
res = p.wait()
(o,e) = p.communicate()
insock.bind(('',11111))
rospy.init_node('mbr_logging')
pubs = {}
diagnostic_pub = rospy.Publisher('/diagnostics',DiagnosticArray,queue_size=10)
while not rospy.is_shutdown():
data = insock.recv(1024)
values = data.decode('utf8').split(',')
sn = values[0]
if len(values) >= 24:
diag_array = DiagnosticArray()
diag_array.header.stamp = rospy.Time.now()
ds = DiagnosticStatus()
ds.name = 'mbr'
ds.hardware_id = values[0]
for i in range(len(fields)):
ds.values.append(KeyValue(fields[i],values[i]))
for i in (21,22):
if not sn+'/'+fields[i] in pubs:
pubs[sn+'/'+fields[i]] = rospy.Publisher('mbr/'+sn+'/'+fields[i],Float32, queue_size=10)
pubs[sn+'/'+fields[i]].publish(float(values[i]))
site_count = int(values[23])
for i in range(site_count):
base_index = 24+i*21
if len(values) >= base_index+21:
topic = 'mbr/'+sn+'/'+values[base_index+0]+'/mean_margin'
if not topic in pubs:
pubs[topic] = rospy.Publisher(topic, Float32, queue_size=10)
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,
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)
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 __init__(self, vec):
"""
:type vec: anki_vector.Robot
:param vec: The vector SDK robot handle (object).
"""
# vars
self._vector = vec
self._lin_vel = .0
self._ang_vel = .0
self._cmd_lin_vel = .0
self._cmd_ang_vel = .0
self._last_pose = self._vector.pose
self._wheel_vel = (0, 0)
self._optical_frame_orientation = quaternion_from_euler(-np.pi/2., .0, -np.pi/2.)
# self._camera_info_manager = CameraInfoManager('vector_camera', namespace='/vector_camera')
# tf
self._tfb = TransformBroadcaster()
# params
self._odom_frame = rospy.get_param('~odom_frame', 'odom')
self._footprint_frame = rospy.get_param('~footprint_frame', 'base_footprint')
self._base_frame = rospy.get_param('~base_frame', 'base_link')
self._head_frame = rospy.get_param('~head_frame', 'head_link')
self._camera_frame = rospy.get_param('~camera_frame', 'camera_link')
self._camera_optical_frame = rospy.get_param('~camera_optical_frame', 'vector_camera')
# camera_info_url = rospy.get_param('~camera_info_url', '')
# pubs
self._joint_state_pub = rospy.Publisher('joint_states', JointState, queue_size=1)
self._odom_pub = rospy.Publisher('odom', Odometry, queue_size=1)
self._imu_pub = rospy.Publisher('imu', Imu, queue_size=1)
self._battery_pub = rospy.Publisher('battery', BatteryState, queue_size=1)
self._touch_pub = rospy.Publisher('touch', Int16, queue_size=1)
self._laser_pub = rospy.Publisher('laser', Range, queue_size=50)
# Note: camera is published under global topic (preceding "/")
self._image_pub = rospy.Publisher('/vector_camera/image', Image, queue_size=10)
# self._camera_info_pub = rospy.Publisher('/vector_camera/camera_info', CameraInfo, queue_size=10)
# subs
# self._backpack_led_sub = rospy.Subscriber(
# 'backpack_led', ColorRGBA, self._set_backpack_led, queue_size=1)
self._twist_sub = rospy.Subscriber('cmd_vel', Twist, self._twist_callback, queue_size=1)
self._say_sub = rospy.Subscriber('say', String, self._say_callback, queue_size=1)
self._head_sub = rospy.Subscriber('head_angle', Float64, self._move_head, queue_size=1)
self._lift_sub = rospy.Subscriber('lift_height', Float64, self._move_lift, queue_size=1)
# diagnostics
self._diag_array = DiagnosticArray()
self._diag_array.header.frame_id = self._base_frame
diag_status = DiagnosticStatus()
diag_status.hardware_id = 'Vector Robot'
diag_status.name = 'Vector Status'
diag_status.values.append(KeyValue(key='Battery Voltage', value=''))
diag_status.values.append(KeyValue(key='Head Angle', value=''))
diag_status.values.append(KeyValue(key='Lift Height', value=''))
self._diag_array.status.append(diag_status)
self._diag_pub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=1)
def __init__(self, coz):
"""
:type coz: cozmo.Robot
:param coz: The cozmo SDK robot handle (object).
"""
# vars
self._cozmo = coz
self._lin_vel = .0
self._ang_vel = .0
self._cmd_lin_vel = .0
self._cmd_ang_vel = .0
self._last_pose = self._cozmo.pose
self._wheel_vel = (0, 0)
self._optical_frame_orientation = quaternion_from_euler(
-np.pi / 2., .0, -np.pi / 2.)
self._camera_info_manager = CameraInfoManager(
'cozmo_camera', namespace='/cozmo_camera')
# tf
self._tfb = TransformBroadcaster()
# params
self._odom_frame = rospy.get_param('~odom_frame', 'odom')
self._footprint_frame = rospy.get_param('~footprint_frame',
'base_footprint')
self._base_frame = rospy.get_param('~base_frame', 'base_link')
self._head_frame = rospy.get_param('~head_frame', 'head_link')
self._camera_frame = rospy.get_param('~camera_frame', 'camera_link')
self._camera_optical_frame = rospy.get_param('~camera_optical_frame',
'cozmo_camera')
camera_info_url = rospy.get_param('~camera_info_url', '')
# pubs
self._joint_state_pub = rospy.Publisher('joint_states',
JointState,
queue_size=1)
self._odom_pub = rospy.Publisher('odom', Odometry, queue_size=1)
self._imu_pub = rospy.Publisher('imu', Imu, queue_size=1)
self._battery_pub = rospy.Publisher('battery',
BatteryState,
queue_size=1)
# Note: camera is published under global topic (preceding "/")
self._image_pub = rospy.Publisher('/cozmo_camera/image',
Image,
queue_size=10)
self._camera_info_pub = rospy.Publisher('/cozmo_camera/camera_info',
CameraInfo,
queue_size=10)
# subs
self._backpack_led_sub = rospy.Subscriber('backpack_led',
ColorRGBA,
self._set_backpack_led,
queue_size=1)
self._twist_sub = rospy.Subscriber('cmd_vel',
Twist,
self._twist_callback,
queue_size=1)
self._say_sub = rospy.Subscriber('say',
String,
self._say_callback,
queue_size=1)
self._head_sub = rospy.Subscriber('head_angle',
Float64,
self._move_head,
queue_size=1)
self._lift_sub = rospy.Subscriber('lift_height',
Float64,
self._move_lift,
queue_size=1)
# diagnostics
self._diag_array = DiagnosticArray()
self._diag_array.header.frame_id = self._base_frame
diag_status = DiagnosticStatus()
diag_status.hardware_id = 'Cozmo Robot'
diag_status.name = 'Cozmo Status'
diag_status.values.append(KeyValue(key='Battery Voltage', value=''))
diag_status.values.append(KeyValue(key='Head Angle', value=''))
diag_status.values.append(KeyValue(key='Lift Height', value=''))
self._diag_array.status.append(diag_status)
self._diag_pub = rospy.Publisher('/diagnostics',
DiagnosticArray,
queue_size=1)
# camera info manager
self._camera_info_manager.setURL(camera_info_url)
self._camera_info_manager.loadCameraInfo()
# Raise the head
self.move_head(10) # level the head
self.move_lift(0) # raise the lift
# Start the tasks
self.task = 0
self.goal_pose = self._cozmo.pose
self.action = None
self._cozmo.add_event_handler(cozmo.objects.EvtObjectAppeared,
self.handle_object_appeared)
self._cozmo.add_event_handler(cozmo.objects.EvtObjectDisappeared,
self.handle_object_disappeared)
self.front_wall = self._cozmo.world.define_custom_wall(
CustomObjectTypes.CustomType00, CustomObjectMarkers.Hexagons2, 300,
44, 63, 63, True)
self.ramp_bottom = self._cozmo.world.define_custom_wall(
CustomObjectTypes.CustomType01, CustomObjectMarkers.Triangles5,
100, 100, 63, 63, True)
self.ramp_top = self._cozmo.world.define_custom_wall(
CustomObjectTypes.CustomType02, CustomObjectMarkers.Diamonds2, 5,
5, 44, 44, True)
self.drop_spot = self._cozmo.world.define_custom_wall(
CustomObjectTypes.CustomType03, CustomObjectMarkers.Circles4, 70,
70, 63, 63, True)
self.back_wall = self._cozmo.world.define_custom_wall(
CustomObjectTypes.CustomType04, CustomObjectMarkers.Diamonds4, 300,
50, 63, 63, True)
self.drop_target = self._cozmo.world.define_custom_wall(
CustomObjectTypes.CustomType05, CustomObjectMarkers.Hexagons5, 5,
5, 32, 32, True)
self.drop_clue = self._cozmo.world.define_custom_wall(
CustomObjectTypes.CustomType06, CustomObjectMarkers.Triangles3, 5,
5, 32, 32, True)
self.front_wall_pose = None
self.ramp_bottom_pose = None
self.drop_spot_pose = None
self.back_wall_pose = None
self.drop_target_pose = None
self.drop_clue_pose = None
self.cube_found = False
self.target_cube = None
self.is_up_top = False # Is Cozmo on the platform
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 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):
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 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)
rospy.init_node('hwboard')
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 = "hcboard_channel " + str(send_channel)
elif read_id == self.eye_sensor_param:
status_object.name = "Eye Camera Temperature"
status_object.hardware_id = "hcboard_channel = " + str(send_channel)
elif read_id == self.torso_module_sensor_param:
status_object.name = "Torso Module Temperature"
status_object.hardware_id = "hcboard_channel =" + str(send_channel)
elif read_id == self.torso_sensor_param:
status_object.name = "Torso Temperature"
status_object.hardware_id = "hcboard_channel =" + str(send_channel)
elif read_id == self.pc_sensor_param:
status_object.name = "PC Temperature"
status_object.hardware_id = "hcboard_channel =" + str(send_channel)
elif read_id == self.engine_sensor_param:
status_object.name = "Engine Temperature"
status_object.hardware_id = "hcboard_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 = "hcboard_channel = 0"
elif send_channel == 1:
status_object.name = "Torso Engine Voltage"
status_object.hardware_id = "hcboard_channel = 1"
elif send_channel == 2:
status_object.name = "Torso Logic Voltage"
status_object.hardware_id = "hcboard_channel = 2"
elif send_channel == 3:
status_object.name = "Tray Logic Voltage"
status_object.hardware_id = "hcboard_channel = 3"
elif send_channel == 6:
status_object.name = "Arm Engine Voltage"
status_object.hardware_id = "hcboard_channel = 6"
elif send_channel == 7:
status_object.name = "Tray Engine Voltage"
status_object.hardware_id = "hcboard_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 = "hcboard_channel = 1"
elif send_channel == 2:
status_object.name = "Torso Logic Current"
status_object.hardware_id = "hcboard_channel = 2"
elif send_channel == 3:
status_object.name = "Tray Logic Current"
status_object.hardware_id = "hcboard_channel = 3"
elif send_channel == 6:
status_object.name = "Arm Engine Current"
status_object.hardware_id = "hcboard_channel = 6"
elif send_channel == 7:
status_object.name = "Tray Engine Current"
status_object.hardware_id = "hcboard_channel = 7"
# evaluate error detection
if error_while_reading == 1:
level = 1
status_object.message = "detected error while receiving answer from hardware control board"
# 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()
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()
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:',
