def _camera_diag(level = 0):
array = DiagnosticArray()
stat = DiagnosticStatus()
stat.name = 'wge100: Driver Status'
stat.level = level
stat.message = 'OK'
motor_stat = DiagnosticStatus()
motor_stat.name = 'EtherCAT Master'
motor_stat.level = 0
motor_stat.values = [
KeyValue(key='Dropped Packets', value='0'),
KeyValue(key='RX Late Packet', value='0')]
mcb_stat = DiagnosticStatus()
mcb_stat.name = 'EtherCAT Device (my_motor)'
mcb_stat.level = 0
mcb_stat.values.append(KeyValue('Num encoder_errors', '0'))
array.header.stamp = rospy.get_rostime()
array.status.append(stat)
array.status.append(motor_stat)
array.status.append(mcb_stat)
return array
def 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 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 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 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 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 _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 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 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):
"""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 _publish_diag(self):
ok = False
with self._mutex:
ok = self._ok
msg = DiagnosticArray()
stat = DiagnosticStatus()
msg.status.append(stat)
# Simulate pr2_etherCAT diagnostics
stat.level = 0
if not ok:
stat.level = 2
stat.name = 'EtherCAT Master'
stat.message = 'OK'
stat.values.append(KeyValue('Dropped Packets', '0'))
stat.values.append(KeyValue('RX Late Packet', '0'))
# Check for encoder errors
mcb_stat = DiagnosticStatus()
mcb_stat.name = 'EtherCAT Device (my_motor)'
mcb_stat.level = 0
mcb_stat.values.append(KeyValue('Num encoder_errors', '0'))
# Test camera listener
stat_cam = DiagnosticStatus()
stat_cam.level = 0
stat_cam.name = 'wge100: Frequency Status'
stat_cam.message = 'OK'
# Test HK listener
stat_hk = DiagnosticStatus()
stat_hk.level = 0
stat_hk.name = 'tilt_hokuyo_node: Frequency Status'
stat_hk.message = 'OK'
msg.status.append(stat_cam)
msg.status.append(mcb_stat)
msg.status.append(stat_hk)
msg.header.stamp = rospy.get_rostime()
self.diag_pub.publish(msg)
self._last_diag_pub = rospy.get_time()
def 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 _laptop_charge_to_diag(laptop_msg):
rv = DiagnosticStatus()
rv.level = DiagnosticStatus.OK
rv.message = 'OK'
rv.name = rospy.get_name()
if rv.name.startswith('/'):
rv.name = rv.name[1:]
if not laptop_msg.present:
rv.level = DiagnosticStatus.ERROR
rv.message = 'Laptop battery missing'
rv.values.append(KeyValue('Voltage (V)', str(laptop_msg.voltage)))
rv.values.append(KeyValue('Current (A)', str(laptop_msg.rate)))
rv.values.append(KeyValue('Charge (Ah)', str(laptop_msg.charge)))
rv.values.append(KeyValue('Capacity (Ah)', str(laptop_msg.capacity)))
rv.values.append(KeyValue('Design Capacity (Ah)', str(laptop_msg.design_capacity)))
return rv
def _ecat_diag(enc_errors = 0):
array = DiagnosticArray()
stat = DiagnosticStatus()
stat.name = 'EtherCAT Master'
stat.level = 0
stat.message = 'OK'
stat.values.append(KeyValue('Dropped Packets', '0'))
stat.values.append(KeyValue('RX Late Packet', '0'))
mcb_stat = DiagnosticStatus()
mcb_stat.name = 'EtherCAT Device (my_motor)'
mcb_stat.level = 0
mcb_stat.values.append(KeyValue('Num encoder_errors', str(enc_errors)))
array.header.stamp = rospy.get_rostime()
array.status.append(stat)
array.status.append(mcb_stat)
return array
def _camera_diag(level = 0):
array = DiagnosticArray()
stat = DiagnosticStatus()
stat.name = 'wge100: Driver Status'
stat.level = level
stat.message = 'OK'
array.header.stamp = rospy.get_rostime()
array.status.append(stat)
return array
def test_DiagnosticStatus():
'''
PUBLISHER METHODE: DiagnosticStatus
'''
pub_DiagnosticStatus = rospy.Publisher('diagnostic', DiagnosticStatus, queue_size=10)
message = DiagnosticStatus()
message.name = "ROB"
message.message = "RUNNING"
print "DiagnosticStatus: " + message.name + " , " + message.message
pub_DiagnosticStatus.publish(message)
def 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 mark_diag_stale(diag_stat = None, error = False):
if not diag_stat:
diag_stat = DiagnosticStatus()
diag_stat.message = 'No Updates'
diag_stat.name = DIAG_NAME
else:
diag_stat.message = 'Updates Stale'
diag_stat.level = DiagnosticStatus.WARN
if error:
diag_stat.level = DiagnosticStatus.ERROR
return diag_stat
def publish_diag(self, level, choice):
msg = DiagnosticArray()
stat = DiagnosticStatus()
msg.status.append(stat)
stat.level = level
stat.name = 'EtherCAT Master'
stat.message = choice
stat.values.append(KeyValue(key='Dropped Packets', value='0'))
stat.values.append(KeyValue(key='RX Late Packet', value='0'))
mcb_stat = DiagnosticStatus()
mcb_stat.level = 0
mcb_stat.name = 'EtherCAT Device (cont_motor)'
mcb_stat.message = 'OK'
mcb_stat.values.append(KeyValue(key='Num encoder_errors', value='0'))
msg.status.append(mcb_stat)
self.diag_pub.publish(msg)
halted = Bool()
halted.data = level != 0
self.motors_pub.publish(halted)
def diagnostics(level, msg_short, msg_long):
if level == 0:
rospy.loginfo(msg_long)
elif level == 1:
rospy.logwarn(msg_long)
elif level == 2:
rospy.logerr(msg_long)
d = DiagnosticArray()
d.header.stamp = rospy.Time.now()
ds = DiagnosticStatus()
ds.level = level
ds.message = msg_long
ds.name = msg_short
d.status = [ds]
pub_diag.publish(d)
def transform(self, msg):
statuses = msg.status
for status in statuses:
if not status.name.startswith('/'):
status.name = '/' + status.name
def top_only(status): return status.name.count("/") < 2
filtered_statuses = filter(top_only, statuses)
levels = [status.level for status in filtered_statuses]
new_message = DiagnosticStatus()
new_message.name = "Robot Status"
new_message.level = max(levels)
return new_message
def publish(pub, info):
diag = DiagnosticArray()
diag.header.stamp = rospy.Time.now()
status = DiagnosticStatus()
status.hardware_id = "wifi"
status.name = 'wifi_scan'
status.level = status.OK
status.message = pformat(info)
for k,v in info.items():
status.values.append(
KeyValue(k,str(v)),
)
diag.status = [status]
pub.publish(diag)
def 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 odom_cb(self, msg):
with self.lock:
dist = msg.distance + (msg.angle * 0.25)
# check if base moved
if dist > self.dist + EPS:
self.start_time = rospy.Time.now()
self.start_angle = self.last_angle
self.dist = dist
# do imu test if possible
if rospy.Time.now() > self.start_time + rospy.Duration(10.0):
self.drift = fabs(self.start_angle - self.last_angle)*180/(pi*10)
self.start_time = rospy.Time.now()
self.start_angle = self.last_angle
self.last_measured = rospy.Time.now()
# publish diagnostics
d = DiagnosticArray()
d.header.stamp = rospy.Time.now()
ds = DiagnosticStatus()
ds.name = "imu_node: Imu Drift Monitor"
if self.drift < 0.5:
ds.level = DiagnosticStatus.OK
ds.message = 'OK'
elif self.drift < 1.0:
ds.level = DiagnosticStatus.WARN
ds.message = 'Drifting'
else:
ds.level = DiagnosticStatus.ERROR
ds.message = 'Drifting'
drift = self.drift
if self.drift < 0:
last_measured = 'No measurements yet, waiting for base to stop moving before measuring'
drift = 'N/A'
else:
age = (rospy.Time.now() - self.last_measured).to_sec()
if age < 60:
last_measured = '%f seconds ago'%age
elif age < 3600:
last_measured = '%f minutes ago'%(age/60)
else:
last_measured = '%f hours ago'%(age/3600)
ds.values = [
KeyValue('Last measured', last_measured),
KeyValue('Drift (deg/sec)', str(drift)) ]
d.status = [ds]
self.pub_diag.publish(d)
def cb_diagnostics_update(self):
"""
Callback periodically called to update the diagnostics status of the
tabpage handler.
@return: A status message for the tabpage handler
@rtype: C{diagnostic_msgs/DiagnosticStatus}
"""
tabpage_status = DiagnosticStatus()
tabpage_status.level = tabpage_status.OK
tabpage_status.name = " ".join([self.handler_name,
"Handler"])
tabpage_status.hardware_id = self.parent.ros_name
tabpage_status.message = "OK"
tabpage_status.values = []
return tabpage_status
def callback(self, _msg):
msg = DiagnosticArray()
statuses = []
for _status in _msg.status:
status = DiagnosticStatus()
status.level = _status.level
status.name = _status.name
if _status.code == 0:
status.message = ""
else:
status.message = self.status_codes_by_module[_status.name].get(
_status.code, "Unknown error"
)
statuses.append(status)
msg.status = statuses
self.pub.publish(msg)
def laptop_charge_to_diag(self, laptop_msg):
rv = DiagnosticStatus()
rv.level = DiagnosticStatus.OK
rv.message = 'OK'
rv.name = 'Laptop Battery'
if not laptop_msg.present:
rv.level = DiagnosticStatus.ERROR
rv.message = 'Laptop battery missing'
rv.values.append(KeyValue('Voltage (V)', str(laptop_msg.voltage)))
rv.values.append(KeyValue('Current (A)', str(laptop_msg.rate)))
rv.values.append(KeyValue('Charge (Ah)', str(laptop_msg.charge)))
rv.values.append(KeyValue('Capacity (Ah)', str(laptop_msg.capacity)))
rv.values.append(KeyValue('Design Capacity (Ah)', str(laptop_msg.design_capacity)))
return rv
def getEtherbotixDiagnostics(self, etherbotix):
msg = DiagnosticStatus()
msg.name = "etherbotix"
msg.level = DiagnosticStatus.OK
msg.message = "OK"
msg.hardware_id = etherbotix.getUniqueId()
# System Voltage
if etherbotix.system_voltage < 10.0:
msg.level = DiagnosticStatus.ERROR
msg.message = "Battery depleted!"
msg.values.append(KeyValue("Voltage", str(etherbotix.system_voltage)+"V"))
# Currents
msg.values.append(KeyValue("Servo Current", str(etherbotix.servo_current)+"A"))
msg.values.append(KeyValue("Aux. Current", str(etherbotix.aux_current)+"A"))
msg.values.append(KeyValue("Packets", str(etherbotix.packets_recv)))
msg.values.append(KeyValue("Packets Bad", str(etherbotix.packets_bad)))
return msg
def _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 controller_to_diag(c):
d = DiagnosticStatus()
d.name = 'Controller (' + c.name + ')'
d.level = 0
if (c.running):
d.message = 'Running'
else:
d.message = 'Stopped'
if (not use_sim_time and c.num_control_loop_overruns > 0):
d.message += ' !!! Broke Realtime, used ' + \
str(int(c.max_time.to_sec() * 1e6)) + \
' micro seconds in update loop'
if c.time_last_control_loop_overrun + rospy.Duration(
30.0) > rospy.Time.now():
d.level = 1
d.values.append(
KeyValue('Avg Time in Update Loop (usec)',
str(int(c.mean_time.to_sec() * 1e6))))
d.values.append(
KeyValue('Max Time in update Loop (usec)',
str(int(c.max_time.to_sec() * 1e6))))
d.values.append(
KeyValue('Variance on Time in Update Loop',
str(int(c.variance_time.to_sec() * 1e6))))
d.values.append(
KeyValue('Percent of Cycle Time Used',
str(int(c.mean_time.to_sec() / 0.00001))))
d.values.append(
KeyValue('Number of Control Loop Overruns',
str(int(c.num_control_loop_overruns))))
d.values.append(
KeyValue('Timestamp of Last Control Loop Overrun (sec)',
str(int(c.time_last_control_loop_overrun.to_sec()))))
return d
def diagnostics(self, state):
try:
da = DiagnosticArray()
ds = DiagnosticStatus()
ds.name = rospy.get_caller_id().lstrip('/') + ": Node State"
if state == 0:
ds.level = DiagnosticStatus.OK
ds.message = "%i sounds playing"%self.active_sounds
ds.values.append(KeyValue("Active sounds", str(self.active_sounds)))
ds.values.append(KeyValue("Allocated sound channels", str(self.num_channels)))
ds.values.append(KeyValue("Buffered builtin sounds", str(len(self.builtinsounds))))
ds.values.append(KeyValue("Buffered wave sounds", str(len(self.filesounds))))
ds.values.append(KeyValue("Buffered voice sounds", str(len(self.voicesounds))))
elif state == 1:
ds.level = DiagnosticStatus.WARN
ds.message = "Sound device not open yet."
else:
ds.level = DiagnosticStatus.ERROR
ds.message = "Can't open sound device. See http://wiki.ros.org/sound_play/Troubleshooting"
da.status.append(ds)
da.header.stamp = rospy.get_rostime()
self.diagnostic_pub.publish(da)
except Exception, e:
rospy.loginfo('Exception in diagnostics: %s'%str(e))
def 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_status(self):
stat = DiagnosticStatus()
stat.name = self.name
is_connect_ok = self.check_connection()
if is_connect_ok:
stat.hardware_id = str(self._info_data['serial'])
model = str(self._info_data['model'])
top_temp = self.convertTemp(
self._diag_data['volt_temp']['top']['lm20_temp'])
bot_temp = self.convertTemp(
self._diag_data['volt_temp']['bot']['lm20_temp'])
i_out = self.convertAmp(
self._diag_data['volt_temp']['bot']['i_out'])
v_in = self.convertVolt(
self._diag_data['volt_temp']['bot']['pwr_v_in'])
stat.values = [
KeyValue(key='ConnectionStatus', value='OK'),
KeyValue(key='Model', value=str(model)),
KeyValue(key='TopTemp[DegC]', value=str(round(top_temp, 3))),
KeyValue(key='BottomTemp[DegC]', value=str(round(bot_temp,
3))),
KeyValue(key='Iout[V]', value=str(round(i_out, 3))),
KeyValue(key='Vin[V]', value=str(round(v_in, 3)))
]
self.judge_risk_level(top_temp, bot_temp, i_out, v_in, stat)
else:
stat.level = DiagnosticStatus.ERROR
stat.hardware_id = 'Velodyne'
stat.message = 'Connection Lost ' + self._ip
stat.values = [KeyValue(key='ConnectionStatus', value='N/A')]
msg = DiagnosticArray()
msg.header.stamp = rospy.get_rostime()
msg.status.append(stat)
self._pub.publish(msg)
def pub_diagnostics(self, msg):
# Pull out the percent charge from the message
percent_charge = msg.percentage
# Initialize the diagnostics array
diag_arr = DiagnosticArray()
# Time stamp the message with the incoming stamp
diag_arr.header.stamp = msg.header.stamp
# Initialize the status message
diag_msg = DiagnosticStatus()
# Make the name field descriptive of what we are measuring
diag_msg.name = "Laptop Charge"
# Add a key-value pair so we can drill down to the percent charge
diag_msg.values.append(KeyValue('percent_charge', str(percent_charge)))
# Set the diagnostics level based on the current charge and the threshold
# parameters
if percent_charge < self.error_percent:
diag_msg.level = DiagnosticStatus.ERROR
diag_msg.message = 'Battery needs recharging'
elif percent_charge < self.warn_percent:
diag_msg.level = DiagnosticStatus.WARN
diag_msg.message = 'Battery is below 50%'
else:
diag_msg.level = DiagnosticStatus.OK
diag_msg.message = 'Battery charge is OK'
# Append the status message to the diagnostic array
diag_arr.status.append(diag_msg)
# Publish the array
self.diag_pub.publish(diag_arr)
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 __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()
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)
def update(self, msg):
removed = []
added = []
items = {}
# fill items from new msg, creating new StatusItems for any that don't already exist,
# and keeping track of those that have been added new
for s in msg.status:
# DiagnosticStatus messages without a name are invalid #3806
if not s.name and not self._has_warned_no_name:
rospy.logwarn(
'DiagnosticStatus message with no "name". Unable to add to robot monitor. Message: %s, hardware ID: %s, level: %d'
% (s.message, s.hardware_id, s.level))
self._has_warned_no_name = True
if not s.name:
continue
if (len(s.name) > 0 and s.name[0] != '/'):
s.name = '/' + s.name
if (s.name not in self._items):
i = StatusItem(s)
added.append(i)
items[s.name] = i
else:
i = self._items[s.name]
i.update(s)
items[s.name] = i
# find anything without a parent already in the items, and add it as a dummy
# item
to_add = []
dummy_names = []
for i in items.itervalues():
parent = i.status.name
while (len(parent) != 0):
parent = get_parent_name(parent)
if (len(parent) > 0 and parent not in items
and parent not in dummy_names):
pi = None
if (parent not in self._items):
s = DiagnosticStatus()
s.name = parent
s.message = ""
pi = StatusItem(s)
else:
pi = self._items[parent]
to_add.append(pi)
dummy_names.append(pi.status.name)
for a in to_add:
if (a.status.name not in items):
items[a.status.name] = a
if (a.status.name not in self._items):
added.append(a)
for i in self._items.itervalues():
# determine removed items
if (i.status.name not in items):
removed.append(i)
# remove removed items
for r in removed:
del self._items[r.status.name]
self._items = items
self._msg = msg
# sort so that parents are always added before children
added.sort(cmp=lambda l, r: cmp(l.status.name, r.status.name))
# sort so that children are always removed before parents
removed.sort(cmp=lambda l, r: cmp(l.status.name, r.status.name),
reverse=True)
#added_keys = [a.status.name for a in added]
#if len(added_keys) > 0: print "Added: ", added_keys
#removed_keys = [r.status.name for r in removed]
#if (len(removed_keys) > 0): print "Removed: ", removed_keys
return (added, removed, self._items)
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 __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 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 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 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)
insock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
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:
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 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, 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
def update(self, mech_state):
self._rx_count += 1
diag = DiagnosticStatus()
diag.level = 0 # Default the level to 'OK'
diag.name = "Trans. Monitor: %s" % self._joint
diag.message = "OK"
diag.values = []
diag.values.append(KeyValue("Joint", self._joint))
diag.values.append(KeyValue("Actuator", self._actuator))
diag.values.append(KeyValue("Up position", str(self._up_ref)))
diag.values.append(KeyValue("Down position", str(self._down_ref)))
diag.values.append(KeyValue("Wrap", str(self._wrap)))
diag.values.append(KeyValue("Max Limit", str(self._max)))
diag.values.append(KeyValue("Min Limit", str(self._min)))
diag.values.append(KeyValue("Deadband", str(self._deadband)))
diag.values.append(KeyValue("Mech State RX Count",
str(self._rx_count)))
# Check if we can find both the joint and actuator
act_names = [x.name for x in mech_state.actuator_states]
act_exists = self._actuator in act_names
act_exists_msg = 'Error'
if act_exists:
cal_reading = mech_state.actuator_states[act_names.index(
self._actuator)].calibration_reading
joint_names = [x.name for x in mech_state.joint_states]
joint_exists = self._joint in joint_names
jnt_exists_msg = 'Error'
if joint_exists:
position = mech_state.joint_states[joint_names.index(
self._joint)].position
calibrated = (mech_state.joint_states[joint_names.index(
self._joint)].is_calibrated == 1)
diag.values.append(KeyValue('Actuator Exists', str(act_exists)))
diag.values.append(KeyValue('Joint Exists', str(joint_exists)))
# First check existance of joint, actuator
if not (act_exists and joint_exists):
diag.level = 2
diag.message = 'Actuators, Joints missing'
self._ok = False
return diag, False
# Monitor current state
reading_msg = 'OK'
if not self.check_device(position, cal_reading, calibrated):
self._ok = False
self._num_errors += 1
self._num_errors_since_reset += 1
reading_msg = 'Broken'
# If we've had an error since the last reset, we're no good
if not self._ok:
diag.message = 'Broken'
diag.level = 2
diag.values.insert(0, KeyValue('Transmission Status', diag.message))
diag.values.insert(1, KeyValue('Current Reading', reading_msg))
diag.values.append(KeyValue('Is Calibrated', str(calibrated)))
diag.values.append(KeyValue('Calibration Reading', str(cal_reading)))
diag.values.append(KeyValue('Joint Position', str(position)))
diag.values.append(KeyValue('Total Errors', str(self._num_errors)))
diag.values.append(
KeyValue('Errors Since Reset', str(self._num_errors_since_reset)))
self._max_position = max(self._max_position, position)
diag.values.append(
KeyValue('Max Obs. Position', str(self._max_position)))
self._min_position = min(self._min_position, position)
diag.values.append(
KeyValue('Min Obs. Position', str(self._min_position)))
return self._ok, diag
from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
from PumpControl import Sampler
import rosservice
import rospy
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")]
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 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
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 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)
pub_diagnostics = rospy.Publisher('/diagnostics', DiagnosticArray)
rospy.loginfo("fake diagnostics for %s running listening to %s",
diagnostics_name, topic_name)
rate = rospy.Rate(1)
while not rospy.is_shutdown():
# if no topic_name is set, we assume that we received a
if topic_name == None:
last_received_ = rospy.Time.now()
# only publish ok if message received recently
if rospy.Time.now() - last_received_ <= rospy.Duration(10.0):
status = DiagnosticStatus()
status.level = 0
status.name = diagnostics_name
status.message = diagnostics_name + " running"
diagnostics = DiagnosticArray()
diagnostics.header.stamp = rospy.Time.now()
diagnostics.status.append(status)
else:
status = DiagnosticStatus()
status.level = 2
status.name = diagnostics_name
status.message = "no message received on " + topic_name
diagnostics = DiagnosticArray()
diagnostics.header.stamp = rospy.Time.now()
diagnostics.status.append(status)
pub_diagnostics.publish(diagnostics)
rate.sleep()
#!/usr/bin/env python
import os
import rospy
from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
if __name__ == '__main__':
node_name = os.path.splitext(os.path.basename(__file__))[0]
rospy.init_node(node_name)
publish_rate = rospy.get_param('~publish_rate', 1.0)
frame_id = rospy.get_param('~frame_id', 'base_link')
ip_address = rospy.get_param('~ip_address', '192.168.0.12')
pub = rospy.Publisher('/ft_sensor/diagnostics',
DiagnosticArray,
queue_size=3)
msg = DiagnosticArray()
msg.header.frame_id = frame_id
status = DiagnosticStatus()
status.level = DiagnosticStatus.OK
status.name = 'NetFT RDT Driver'
status.message = 'OK'
status.hardware_id = 'ATI Gamma'
status.values.append(KeyValue('IP Address', ip_address))
msg.status.append(status)
rate = rospy.Rate(publish_rate)
while not rospy.is_shutdown():
msg.header.stamp = rospy.Time.now()
pub.publish(msg)
rate.sleep()
imuMsg.orientation.y = q[1]
imuMsg.orientation.z = q[2]
imuMsg.orientation.w = q[3]
imuMsg.header.stamp = rospy.Time.now()
imuMsg.header.frame_id = link_name
imuMsg.header.seq = seq
seq = seq + 1
pub.publish(imuMsg)
if (diag_pub_time < rospy.get_time()):
diag_pub_time += 1
diag_arr = DiagnosticArray()
diag_arr.header.stamp = rospy.get_rostime()
diag_arr.header.frame_id = '1'
diag_msg = DiagnosticStatus()
diag_msg.name = 'Razor_Imu'
diag_msg.level = DiagnosticStatus.OK
diag_msg.message = 'Received AHRS measurement'
diag_msg.values.append(
KeyValue('roll (deg)', str(roll * (180.0 / math.pi))))
diag_msg.values.append(
KeyValue('pitch (deg)', str(pitch * (180.0 / math.pi))))
diag_msg.values.append(
KeyValue('yaw (deg)', str(yaw * (180.0 / math.pi))))
diag_msg.values.append(KeyValue('sequence number', str(seq)))
diag_arr.status.append(diag_msg)
diag_pub.publish(diag_arr)
ser.close
#f.close
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)
