def _recursive_create_widget_items(self, parent, topic_name, type_name, message, is_editable=True):
item = QTreeWidgetItem(parent)
if is_editable:
item.setFlags(item.flags() | Qt.ItemIsEditable)
else:
item.setFlags(item.flags() & (~Qt.ItemIsEditable))
if parent is None:
# show full topic name with preceding namespace on toplevel item
topic_text = topic_name
else:
topic_text = topic_name.split('/')[-1]
item.setText(self._column_index['service'], topic_text)
item.setText(self._column_index['type'], type_name)
item.setData(0, Qt.UserRole, topic_name)
if hasattr(message, '__slots__') and hasattr(message, '_slot_types'):
for slot_name, type_name in zip(message.__slots__, message._slot_types):
self._recursive_create_widget_items(item, topic_name + '/' + slot_name, type_name, getattr(message, slot_name), is_editable)
elif type(message) in (list, tuple) and (len(message) > 0) and hasattr(message[0], '__slots__'):
type_name = type_name.split('[', 1)[0]
for index, slot in enumerate(message):
self._recursive_create_widget_items(item, topic_name + '[%d]' % index, type_name, slot, is_editable)
else:
item.setText(self._column_index['expression'], repr(message))
return item
def _recursive_create_widget_items(self, parent, topic_name, type_name,
message):
if parent is self.topics_tree_widget:
# show full topic name with preceding namespace on toplevel item
topic_text = topic_name
else:
topic_text = topic_name.split('/')[-1]
if '[' in topic_text:
topic_text = topic_text[topic_text.index('['):]
item = QTreeWidgetItem(parent)
item.setText(self._column_index['topic'], topic_text)
item.setText(self._column_index['type'], type_name)
item.setData(0, Qt.UserRole, topic_name)
self._tree_items[topic_name] = item
if hasattr(message, '__slots__') and hasattr(message, '_slot_types'):
for slot_name, type_name in zip(message.__slots__,
message._slot_types):
self._recursive_create_widget_items(
item, topic_name + '/' + slot_name, type_name,
getattr(message, slot_name))
else:
base_type_str, array_size = self._extract_array_info(type_name)
try:
base_instance = roslib.message.get_message_class(
base_type_str)()
except ValueError:
base_instance = None
if array_size is not None and hasattr(base_instance, '__slots__'):
for index in range(array_size):
self._recursive_create_widget_items(
item, topic_name + '[%d]' % index, base_type_str,
base_instance)
return item
def _recursive_create_widget_items(self,
parent,
topic_name,
type_name,
message,
is_editable=True):
item = QTreeWidgetItem(parent)
if is_editable:
item.setFlags(item.flags() | Qt.ItemIsEditable)
else:
item.setFlags(item.flags() & (~Qt.ItemIsEditable))
if parent is None:
# show full topic name with preceding namespace on toplevel item
topic_text = topic_name
else:
topic_text = topic_name.split('/')[-1]
item.setText(self._column_index['service'], topic_text)
item.setText(self._column_index['type'], type_name)
item.setData(0, Qt.UserRole, topic_name)
if hasattr(message, '__slots__') and hasattr(message, '_slot_types'):
for slot_name, type_name in zip(message.__slots__,
message._slot_types):
self._recursive_create_widget_items(
item, topic_name + '/' + slot_name, type_name,
getattr(message, slot_name), is_editable)
elif type(message) in (list, tuple) and (len(message) > 0) and hasattr(
message[0], '__slots__'):
type_name = type_name.split('[', 1)[0]
for index, slot in enumerate(message):
self._recursive_create_widget_items(
item, topic_name + '[%d]' % index, type_name, slot,
is_editable)
else:
item.setText(self._column_index['expression'], repr(message))
return item
def add_sensor(self, name, topic, ttype, freq):
sensor = QTreeWidgetItem()
sensor.setData(0, Qt.DisplayRole, name)
sensor.setData(1, Qt.DisplayRole, topic)
sensor.setData(2, Qt.DisplayRole, "0/" + str(freq))
sensor.setData(3, Qt.DecorationRole, self.abnormal_image)
self.sensors[name] = sensor
self._widget.sensor_list.addTopLevelItem(sensor)
self.ts_list[name] = []
self.freq[name] = freq
self.has_msg[name] = False
rospy.Subscriber(topic,
ttype,
self.callback, (name, topic),
queue_size=10)
def _add_msg_object(self, parent, path, name, obj, obj_type):
label = name
if hasattr(obj, '__slots__'):
subobjs = [(slot, getattr(obj, slot)) for slot in obj.__slots__]
elif type(obj) in [list, tuple]:
len_obj = len(obj)
if len_obj == 0:
subobjs = []
else:
w = int(math.ceil(math.log10(len_obj)))
subobjs = [('[%*d]' % (w, i), subobj)
for (i, subobj) in enumerate(obj)]
else:
subobjs = []
if type(obj) in [int, long, float]:
if type(obj) == float:
obj_repr = '%.6f' % obj
else:
obj_repr = str(obj)
if obj_repr[0] == '-':
label += ': %s' % obj_repr
else:
label += ': %s' % obj_repr
elif type(obj) in [str, bool, int, long, float, complex, rospy.Time]:
# Ignore any binary data
obj_repr = codecs.utf_8_decode(str(obj), 'ignore')[0]
# Truncate long representations
if len(obj_repr) >= 50:
obj_repr = obj_repr[:50] + '...'
label += ': ' + obj_repr
item = QTreeWidgetItem([label])
if name == 'msg':
pass
elif path.find('.') == -1 and path.find('[') == -1:
self.addTopLevelItem(item)
else:
parent.addChild(item)
item.setData(0, Qt.UserRole, (path, obj_type))
for subobj_name, subobj in subobjs:
if subobj is None:
continue
if path == '':
subpath = subobj_name # root field
elif subobj_name.startswith('['):
subpath = '%s%s' % (path, subobj_name) # list, dict, or tuple
else:
subpath = '%s.%s' % (path, subobj_name
) # attribute (prefix with '.')
if hasattr(subobj, '_type'):
subobj_type = subobj._type
else:
subobj_type = type(subobj).__name__
self._add_msg_object(item, subpath, subobj_name, subobj,
subobj_type)
def _add_msg_object(self, parent, path, name, obj, obj_type):
label = name
if hasattr(obj, '__slots__'):
subobjs = [(slot, getattr(obj, slot)) for slot in obj.__slots__]
elif type(obj) in [list, tuple]:
len_obj = len(obj)
if len_obj == 0:
subobjs = []
else:
w = int(math.ceil(math.log10(len_obj)))
subobjs = [('[%*d]' % (w, i), subobj) for (i, subobj) in enumerate(obj)]
else:
subobjs = []
plotitem=False
if type(obj) in [int, long, float]:
plotitem=True
if type(obj) == float:
obj_repr = '%.6f' % obj
else:
obj_repr = str(obj)
if obj_repr[0] == '-':
label += ': %s' % obj_repr
else:
label += ': %s' % obj_repr
elif type(obj) in [str, bool, int, long, float, complex, rospy.Time]:
# Ignore any binary data
obj_repr = codecs.utf_8_decode(str(obj), 'ignore')[0]
# Truncate long representations
if len(obj_repr) >= 50:
obj_repr = obj_repr[:50] + '...'
label += ': ' + obj_repr
item = QTreeWidgetItem([label])
if name == '':
pass
elif path.find('.') == -1 and path.find('[') == -1:
self.addTopLevelItem(item)
else:
parent.addChild(item)
if plotitem == True:
if path.replace(' ', '') in self._checked_states:
item.setCheckState (0, Qt.Checked)
else:
item.setCheckState (0, Qt.Unchecked)
item.setData(0, Qt.UserRole, (path, obj_type))
for subobj_name, subobj in subobjs:
if subobj is None:
continue
if path == '':
subpath = subobj_name # root field
elif subobj_name.startswith('['):
subpath = '%s%s' % (path, subobj_name) # list, dict, or tuple
else:
subpath = '%s.%s' % (path, subobj_name) # attribute (prefix with '.')
if hasattr(subobj, '_type'):
subobj_type = subobj._type
else:
subobj_type = type(subobj).__name__
self._add_msg_object(item, subpath, subobj_name, subobj, subobj_type)
def refresh_ctrlers(self):
if self.cm_namespace_combo.count() == 0:
# no controller managers found so there are no controllers to update
# remove old controllers
for old_ctrler_name in self._ctrlers.keys():
self.remove_ctrler_from_list(old_ctrler_name)
return
ctrlman_ns = self.cm_namespace_combo.currentText()
if self.ctrlman_ns_cur != ctrlman_ns:
# new controller manager selected
# remove old controllers from list from last CM
for old_ctrler_name in self._ctrlers.keys():
self.remove_ctrler_from_list(old_ctrler_name)
self.ctrlman_ns_cur = ctrlman_ns
rospy.wait_for_service(ctrlman_ns + '/controller_manager/list_controllers', 0.2)
try:
resp = self.list_ctrlers[ctrlman_ns].call(ListControllersRequest())
except rospy.ServiceException as e:
# TODO: display warning somehow
return
controller_list = resp.controller
new_ctrlers = {}
for c in controller_list:
if c.name not in self._ctrlers:
# new controller
item = QTreeWidgetItem(self.ctrl_list_tree_widget)
item.setData(0, Qt.UserRole, c.name)
ctrler = {'item' : item,
'state' : c.state,
'type' : c.type,
'hw_iface' : c.hardware_interface,
'resources' : "[" + ", ".join(c.resources) + "]"}
ctrler['item'].setText(self._column_index['name'], c.name)
update_type = True
update_state = True
else:
# controller already in list
ctrler = self._ctrlers[c.name]
update_type = False
update_state = False
if ctrler['type'] != c.type or ctrler['hw_iface'] != c.hardware_interface:
# total controller change
ctrler['state'] = c.state
ctrler['type'] = c.type
ctrler['hw_iface'] = c.hardware_interface
ctrler['resources'] = "[" + ", ".join(c.resources) + "]"
update_type = True
if ctrler['state'] != c.state:
# state change
ctrler['state'] = c.state
update_state = True
# update entries if needed
if update_type:
ctrler['item'].setText(self._column_index['type'], ctrler['type'])
ctrler['item'].setText(self._column_index['hw_iface'], ctrler['hw_iface'])
ctrler['item'].setText(self._column_index['resources'], ctrler['resources'])
if update_state or update_type:
ctrler['item'].setText(self._column_index['state'], ctrler['state'])
new_ctrlers[c.name] = ctrler
# remove missing controllers
for old_ctrler_name in self._ctrlers.keys():
if old_ctrler_name not in new_ctrlers:
self.remove_ctrler_from_list(old_ctrler_name)
# update current controllers
self._ctrlers = new_ctrlers
def refresh_ctrlers(self):
if self.cm_namespace_combo.count() == 0:
# no controller managers found so there are no controllers to update
# remove old controllers
for old_ctrler_name in self._ctrlers.keys():
self.remove_ctrler_from_list(old_ctrler_name)
return
ctrlman_ns = self.cm_namespace_combo.currentText()
if self.ctrlman_ns_cur != ctrlman_ns:
# new controller manager selected
# remove old controllers from list from last CM
for old_ctrler_name in self._ctrlers.keys():
self.remove_ctrler_from_list(old_ctrler_name)
self.ctrlman_ns_cur = ctrlman_ns
rospy.wait_for_service(
ctrlman_ns + '/controller_manager/list_controllers', 0.2)
try:
resp = self.list_ctrlers[ctrlman_ns].call(ListControllersRequest())
except rospy.ServiceException as e:
# TODO: display warning somehow
return
controller_list = resp.controller
new_ctrlers = {}
for c in controller_list:
if c.name not in self._ctrlers:
# new controller
item = QTreeWidgetItem(self.ctrl_list_tree_widget)
item.setData(0, Qt.UserRole, c.name)
ctrler = {
'item': item,
'state': c.state,
'type': c.type,
'hw_iface': c.hardware_interface,
'resources': "[" + ", ".join(c.resources) + "]"
}
ctrler['item'].setText(self._column_index['name'], c.name)
update_type = True
update_state = True
else:
# controller already in list
ctrler = self._ctrlers[c.name]
update_type = False
update_state = False
if ctrler['type'] != c.type or ctrler[
'hw_iface'] != c.hardware_interface:
# total controller change
ctrler['state'] = c.state
ctrler['type'] = c.type
ctrler['hw_iface'] = c.hardware_interface
ctrler['resources'] = "[" + ", ".join(c.resources) + "]"
update_type = True
if ctrler['state'] != c.state:
# state change
ctrler['state'] = c.state
update_state = True
# update entries if needed
if update_type:
ctrler['item'].setText(self._column_index['type'],
ctrler['type'])
ctrler['item'].setText(self._column_index['hw_iface'],
ctrler['hw_iface'])
ctrler['item'].setText(self._column_index['resources'],
ctrler['resources'])
if update_state or update_type:
ctrler['item'].setText(self._column_index['state'],
ctrler['state'])
new_ctrlers[c.name] = ctrler
# remove missing controllers
for old_ctrler_name in self._ctrlers.keys():
if old_ctrler_name not in new_ctrlers:
self.remove_ctrler_from_list(old_ctrler_name)
# update current controllers
self._ctrlers = new_ctrlers
