def create_task_tree(self, task_id, processor):
self.task_tree_widget.clear()
self.task_tree_widget.setColumnCount(2)
self.task_tree_widget.hideColumn(1)
item = QTreeWidgetItem(self.task_tree_widget, ["Task {}".format(task_id), str(task_id)])
item.setExpanded(True)
return item
def _add_frequently_used_skill(self, s):
self.last_executed_skill = s.name
fu = self.skill_tree_widget.findItems("fu", Qt.MatchRecursive | Qt.MatchFixedString, 1)[0]
for i in range(0, fu.childCount()): # avoid adding same node multiple times
if s.name == fu.child(i).data(1, 0):
return
skill = QTreeWidgetItem(fu, [s.name, s.name])
skill.setData(2, 0, s)
def on_call_service_button_clicked(self):
self.response_tree_widget.clear()
request = self._service_info['service_class']._request_class()
self.fill_message_slots(request, self._service_info['service_name'],
self._service_info['expressions'],
self._service_info['counter'])
try:
response = self._service_info['service_proxy'](request)
except rospy.ServiceException as e:
qWarning(
'ServiceCaller.on_call_service_button_clicked(): request:\n%r'
% (request))
qWarning(
'ServiceCaller.on_call_service_button_clicked(): error calling service "%s":\n%s'
% (self._service_info['service_name'], e))
top_level_item = QTreeWidgetItem()
top_level_item.setText(self._column_index['service'], 'ERROR')
top_level_item.setText(self._column_index['type'],
'rospy.ServiceException')
top_level_item.setText(self._column_index['expression'], str(e))
else:
#qDebug('ServiceCaller.on_call_service_button_clicked(): response: %r' % (response))
top_level_item = self._recursive_create_widget_items(
None, '/', response._type, response, is_editable=False)
self.response_tree_widget.addTopLevelItem(top_level_item)
# resize columns
self.response_tree_widget.expandAll()
for i in range(self.response_tree_widget.columnCount()):
self.response_tree_widget.resizeColumnToContents(i)
def _recursive_create_widget_items(self,
parent,
topic_name,
type_name,
message,
leaf=True):
#print("Topic name is ",topic_name)
if parent is self._widget.topics_tree_widget:
topic_text = topic_name
self._topic_list.append(topic_name)
else:
topic_text = topic_name.split('/')[-1]
if '[' in topic_text:
topic_text = topic_text[topic_text.index('['):]
if leaf:
item = TreeWidgetItem(self._toggle_selection, topic_name, parent)
else:
item = QTreeWidgetItem(parent)
item.setText(self._column_index['topic'], topic_text)
item.setText(self._column_index['type'], type_name)
item.setText(self._column_index['buffer_size'], "1")
item.setData(0, Qt.UserRole, topic_name)
self._tree_items[topic_name] = item
if hasattr(message, '__slots__') and hasattr(message, '_slot_types'):
#print("This thing has slots and slot_types")
for slot_name, type_name in zip(message.__slots__,
message._slot_types):
#print("S/T: ", slot_name, "/", type_name)
self._recursive_create_widget_items(
item, topic_name + '/' + slot_name, type_name,
getattr(message, slot_name))
else:
#print("Trying to extract array info or complex type info")
base_type_str, array_size = self._extract_array_info(type_name)
try:
base_instance = roslib.message.get_message_class(
base_type_str)()
except (ValueError, TypeError):
base_instance = None
if array_size is not None and hasattr(base_instance, '__slots__'):
for index in range(array_size):
print("Recursing into array") # NEVER HAPPENS!
self._recursive_create_widget_items(
item, topic_name + '[%d]' % index, base_type_str,
base_instance)
elif hasattr(base_instance, '__slots__') and hasattr(
base_instance, '_slot_types'):
#print("Recursing into complex type")
for b_slot_name, b_type_name in zip(base_instance.__slots__,
base_instance._slot_types):
#print("Complex::S/T: ", b_slot_name, "/", b_type_name)
self._recursive_create_widget_items(
item, topic_name + '/' + b_slot_name, b_type_name,
getattr(base_instance, b_slot_name))
return item
def _create_item(self, status, select, expand_if_error):
if (status.level == DiagnosticStatus.OK):
parent_node = self._ok_node
elif (status.level == DiagnosticStatus.WARN):
parent_node = self._warning_node
elif (status.level == -1):
parent_node = self._stale_node
else: # ERROR
parent_node = self._error_node
item = TreeItem(status, QTreeWidgetItem(parent_node, [status.name + ": " + status.message]))
item.tree_node.setData(0, Qt.UserRole, item)
parent_node.addChild(item.tree_node)
self._name_to_item[status.name] = item
parent_node.sortChildren(0, Qt.AscendingOrder)
if (select):
item.tree_node.setSelected(True)
if (expand_if_error and (status.level > 1 or status.level == -1)):
parent_node.setExpanded(True)
item.mark = True
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
item = TreeWidgetItem(self._toggle_monitoring, topic_name, parent)
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, TypeError):
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 __init__(self, topic="diagnostics"):
super(RuntimeMonitorWidget, self).__init__()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_runtime_monitor'), 'resource', 'runtime_monitor_widget.ui')
loadUi(ui_file, self)
self.setObjectName('RuntimeMonitorWidget')
self._mutex = threading.Lock()
self._error_icon = QIcon.fromTheme('dialog-error')
self._warning_icon = QIcon.fromTheme('dialog-warning')
self._ok_icon = QIcon.fromTheme('dialog-information')
self._stale_node = QTreeWidgetItem(self.tree_widget.invisibleRootItem(), ['Stale (0)'])
self._stale_node.setIcon(0, self._error_icon)
self.tree_widget.addTopLevelItem(self._stale_node)
self._error_node = QTreeWidgetItem(self.tree_widget.invisibleRootItem(), ['Errors (0)'])
self._error_node.setIcon(0, self._error_icon)
self.tree_widget.addTopLevelItem(self._error_node)
self._warning_node = QTreeWidgetItem(self.tree_widget.invisibleRootItem(), ['Warnings (0)'])
self._warning_node.setIcon(0, self._warning_icon)
self.tree_widget.addTopLevelItem(self._warning_node)
self._ok_node = QTreeWidgetItem(self.tree_widget.invisibleRootItem(), ['Ok (0)'])
self._ok_node.setIcon(0, self._ok_icon)
self.tree_widget.addTopLevelItem(self._ok_node)
self.tree_widget.itemSelectionChanged.connect(self._refresh_selection)
self.keyPressEvent = self._on_key_press
self._name_to_item = {}
self._new_errors_callback = None
self._subscriber = rospy.Subscriber(topic, DiagnosticArray, self._diagnostics_callback)
self._previous_ros_time = rospy.Time.now()
self._timer = QTimer()
self._timer.timeout.connect(self._on_timer)
self._timer.start(1000)
self._msg_timer = QTimer()
self._msg_timer.timeout.connect(self._update_messages)
self._msg_timer.start(100)
self._messages = []
self._used_items = 0
def append_tree(self, container, parent=None):
"""Append an item to the tree view."""
if not parent:
node = QTreeWidgetItem()
node.setText(0, container._label)
self._widget.tree.addTopLevelItem(node)
for child_label in container._children:
child = QTreeWidgetItem(node)
child.setText(0, child_label)
def motor_switcher(self):
'''
Loads the motors into the tree and adds the checkboxes
'''
self._widget.motorTree.setHeaderLabel("Stiff Motors")
self._motorCheckBody.setText(0, "Body")
self._motorCheckBody.setFlags(self._motorCheckBody.flags() | Qt.ItemIsTristate | Qt.ItemIsUserCheckable)
self._motorCheckBody.setExpanded(True)
self._motorCheckHead.setText(0, "Head")
self._motorCheckHead.setFlags(self._motorCheckHead.flags() | Qt.ItemIsTristate | Qt.ItemIsUserCheckable)
self._motorCheckHead.setExpanded(True)
self._motorCheckArms.setText(0, "Arms")
self._motorCheckArms.setFlags(self._motorCheckArms.flags() | Qt.ItemIsTristate | Qt.ItemIsUserCheckable)
self._motorCheckArms.setExpanded(True)
self._motorCheckLegs.setText(0, "Legs")
self._motorCheckLegs.setFlags(self._motorCheckLegs.flags() | Qt.ItemIsTristate | Qt.ItemIsUserCheckable)
self._motorCheckLegs.setExpanded(True)
self._motorCheckLArm.setText(0, "Left Arm")
self._motorCheckLArm.setFlags(self._motorCheckLArm.flags() | Qt.ItemIsTristate | Qt.ItemIsUserCheckable)
self._motorCheckLArm.setExpanded(True)
self._motorCheckRArm.setText(0, "Right Arm")
self._motorCheckRArm.setFlags(self._motorCheckRArm.flags() | Qt.ItemIsTristate | Qt.ItemIsUserCheckable)
self._motorCheckRArm.setExpanded(True)
self._motorCheckLLeg.setText(0, "Left Leg")
self._motorCheckLLeg.setFlags(self._motorCheckLLeg.flags() | Qt.ItemIsTristate | Qt.ItemIsUserCheckable)
self._motorCheckLLeg.setExpanded(True)
self._motorCheckRLeg.setText(0, "Right Leg")
self._motorCheckRLeg.setFlags(self._motorCheckRLeg.flags() | Qt.ItemIsTristate | Qt.ItemIsUserCheckable)
self._motorCheckRLeg.setExpanded(True)
for k, v in self._currentGoals.items():
parent = None
if 'LHip' in k or 'LKnee' in k or 'LAnkle' in k:
parent = self._motorCheckLLeg
elif 'RHip' in k or 'RKnee' in k or 'RAnkle' in k:
parent = self._motorCheckRLeg
elif 'LShoulder' in k or 'LElbow' in k:
parent = self._motorCheckLArm
elif 'RShoulder' in k or 'RElbow' in k:
parent = self._motorCheckRArm
elif 'Head' in k:
parent = self._motorCheckHead
child = QTreeWidgetItem(parent)
child.setText(0, k)
child.setFlags(child.flags() | Qt.ItemIsUserCheckable)
child.setCheckState(0, Qt.Checked)
self._treeItems[k] = child
self._widget.motorTree.itemClicked.connect(self.box_ticked)
def __init__(self, topic="diagnostics"):
super(RuntimeMonitorWidget, self).__init__()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_runtime_monitor'), 'resource',
'runtime_monitor_widget.ui')
loadUi(ui_file, self)
self.setObjectName('RuntimeMonitorWidget')
self._mutex = threading.Lock()
self._error_icon = QIcon.fromTheme('dialog-error')
self._warning_icon = QIcon.fromTheme('dialog-warning')
self._ok_icon = QIcon.fromTheme('dialog-information')
self._stale_node = QTreeWidgetItem(
self.tree_widget.invisibleRootItem(), ['Stale (0)'])
self._stale_node.setIcon(0, self._error_icon)
self.tree_widget.addTopLevelItem(self._stale_node)
self._error_node = QTreeWidgetItem(
self.tree_widget.invisibleRootItem(), ['Errors (0)'])
self._error_node.setIcon(0, self._error_icon)
self.tree_widget.addTopLevelItem(self._error_node)
self._warning_node = QTreeWidgetItem(
self.tree_widget.invisibleRootItem(), ['Warnings (0)'])
self._warning_node.setIcon(0, self._warning_icon)
self.tree_widget.addTopLevelItem(self._warning_node)
self._ok_node = QTreeWidgetItem(self.tree_widget.invisibleRootItem(),
['Ok (0)'])
self._ok_node.setIcon(0, self._ok_icon)
self.tree_widget.addTopLevelItem(self._ok_node)
self.tree_widget.itemSelectionChanged.connect(self._refresh_selection)
self.keyPressEvent = self._on_key_press
self._name_to_item = {}
self._new_errors_callback = None
self._subscriber = rospy.Subscriber(topic, DiagnosticArray,
self._diagnostics_callback)
self._previous_ros_time = rospy.Time.now()
self._timer = QTimer()
self._timer.timeout.connect(self._on_timer)
self._timer.start(1000)
self._msg_timer = QTimer()
self._msg_timer.timeout.connect(self._update_messages)
self._msg_timer.start(100)
self._messages = []
self._used_items = 0
def load_steps(self, skill):
#skill += "_Skill"
steps = self.kb_interface.get_task_steps(skill)
for s in steps:
step = QTreeWidgetItem(self.skill_tree)
step.setText(0, s)
tasks = self.kb_interface.extract_actions_from_step(s)
print tasks
for t in tasks:
if not t.startswith('Null'):
task = QTreeWidgetItem(step)
task.setText(0, t)
def create_tree(self):
# Set up the main tree widget
self.tree_widget = QTreeWidget()
self.tree_widget.setColumnCount(2)
self.tree_widget.setColumnWidth(0, 250)
self.tree_widget.setHeaderLabels(['Parts', 'Status'])
self.item = QTreeWidgetItem()
# Create the BPO section
self.BPO_header = QTreeWidgetItem(self.tree_widget)
self.BPO_header.setFlags(self.BPO_header.flags() | Qt.ItemIsTristate | Qt.ItemIsUserCheckable)
self.BPO_header.setText(0, 'BPO Checklist')
self.BPO_header.setExpanded(True)
self.create_item(self.BPO_header, self.BPO_checklist) # Adds the list of items into the section
# Create the BTO section
self.BTO_header = QTreeWidgetItem(self.tree_widget)
self.BTO_header.setFlags(self.BTO_header.flags() | Qt.ItemIsTristate | Qt.ItemIsUserCheckable)
self.BTO_header.setText(0, 'BTO Checklist')
self.BTO_header.setExpanded(True)
self.create_item(self.BTO_header, self.BTO_checklist) # Adds the list of items into the section
def _add_wm_node(self, elem):
#print "Adding {}".format(elem.id)
parent_rel = elem.getRelation(pred=self._wmi.get_sub_properties('skiros:spatiallyRelated'), obj='-1')
to_expand = True
if not parent_rel:
parent_rel = elem.getRelation(pred=self._wmi.get_sub_properties('skiros:skillProperty'), obj='-1')
to_expand = False
if not parent_rel:
to_expand = False
parent_rel = elem.getRelation(pred='skiros:hasSkill', obj='-1')
if not parent_rel:
log.warn("[add_wm_node]", "Skipping element without declared parent: {}".format(elem.id))
return True
parent_id = '{}_skills'.format(parent_rel['src'])
item = self.wm_tree_widget.findItems(parent_id, Qt.MatchRecursive | Qt.MatchFixedString, 1)
if not item: # In case it is still not existing i create the "support" skill node
item = self.wm_tree_widget.findItems(parent_rel['src'], Qt.MatchRecursive | Qt.MatchFixedString, 1)[0]
item = QTreeWidgetItem(item, ['Skills', parent_id])
else:
item = item[0]
else:
items = self.wm_tree_widget.findItems(parent_rel['src'], Qt.MatchRecursive | Qt.MatchFixedString, 1)
if not items:
log.warn("[add_wm_node]", "Parent {} of node {} is not in the known tree.".format(parent_rel['src'], elem.id))
return False
item = items[0]
name = utils.ontology_type2name(elem.id) if not elem.label else utils.ontology_type2name(elem.label)
item = QTreeWidgetItem(item, [name, elem.id])
item.setExpanded(to_expand)
return True
def on_progress_update(self, msg):
self._update_progress_table(msg)
self._save_log(msg, "skill")
# Update buttons
if msg.type.find("Root") >= 0:
if not self.skill_stop_button.isEnabled():
self.create_task_tree(msg.id, msg.processor)
self._toggle_task_active()
for manager in self._sli.agents.values():
manager.reset_tick_rate()
if abs(msg.progress_code) == 1:
self._toggle_task_active()
# Update task tree
with self._task_mutex:
items = self.task_tree_widget.findItems(str(msg.id), Qt.MatchRecursive | Qt.MatchFixedString, 1)
#int(msg.progress_period*1000)
if items:
items[0].setData(0, 0, "{}({}) {}".format(msg.label, State(msg.state).name, "! SLOW !" if msg.progress_period>0.04 else ""))
else:
parents = self.task_tree_widget.findItems(str(msg.parent_id), Qt.MatchRecursive | Qt.MatchFixedString, 1)
if not parents:
log.error("No parent found. Debug: {}".format(msg))
return
item = QTreeWidgetItem(parents[0], ["{}({})".format(msg.label, State(msg.state).name), str(msg.id)])
item.setIcon(0, self.get_icon(msg.processor))
item.setExpanded(True)
def on_call_service_button_clicked(self):
current_services = dict(self._node.get_service_names_and_types())
if self._service_info['service_name'] not in current_services:
qWarning(
'Service "{}" is no longer available. Refresh the list of services'
.format(self._service_info['service_name']))
return
self.response_tree_widget.clear()
request = self._service_info['service_class'].Request()
self.fill_message_slots(request, self._service_info['service_name'],
self._service_info['expressions'],
self._service_info['counter'])
cli = self._node.create_client(self._service_info['service_class'],
self._service_info['service_name'])
future = cli.call_async(request)
while rclpy.ok() and not future.done():
pass
if future.result() is not None:
response = future.result()
top_level_item = self._recursive_create_widget_items(
None,
'/',
self._service_info['service_class_name'] + '.Response',
response,
is_editable=False)
else:
qWarning(
'ServiceCaller.on_call_service_button_clicked(): request:\n%r'
% (request))
qWarning(
'ServiceCaller.on_call_service_button_clicked(): error calling service "%s".'
% (self._service_info['service_name']))
top_level_item = QTreeWidgetItem()
top_level_item.setText(self._column_index['service'], 'ERROR')
top_level_item.setText(self._column_index['type'],
'rospy.ServiceException')
top_level_item.setText(self._column_index['expression'], '')
self._node.destroy_client(cli)
self.response_tree_widget.addTopLevelItem(top_level_item)
# resize columns
self.response_tree_widget.expandAll()
for i in range(self.response_tree_widget.columnCount()):
self.response_tree_widget.resizeColumnToContents(i)
def _add_available_skill(self, s):
stype = self.skill_tree_widget.findItems(s.type, Qt.MatchRecursive | Qt.MatchFixedString, 1)
if not stype: #If it is the first of its type, add the parents hierarchy to the tree
hierarchy = self._wmi.query_ontology('SELECT ?x {{ {} rdfs:subClassOf* ?x }}'.format(s.type))
hierarchy = hierarchy[:hierarchy.index("skiros:Skill")]
hierarchy.reverse()
parent = self.skill_tree_widget.findItems("All", Qt.MatchRecursive | Qt.MatchFixedString, 1)[0]
for c in hierarchy:
child = self.skill_tree_widget.findItems(c, Qt.MatchRecursive | Qt.MatchFixedString, 1)
if child:
parent = child[0]
else:
parent = QTreeWidgetItem(parent, [c.replace("skiros:", ""), c])
parent.setExpanded(True)
else:
parent = stype[0]
skill = QTreeWidgetItem(parent, [s.name, s.name])
skill.setData(2, 0, s)
def _create_wm_tree(self, item, scene, elem):
name = utils.ontology_type2name(elem.id) if not elem.label else utils.ontology_type2name(elem.label)
item = QTreeWidgetItem(item, [name, elem.id])
spatialRel = sorted(elem.getRelations(subj='-1', pred=self._wmi.get_sub_properties('skiros:spatiallyRelated')), key=lambda r: r['dst'])
for rel in spatialRel:
if rel['dst'] in scene:
self._create_wm_tree(item, scene, scene[rel['dst']])
item.setExpanded(True)
skillRel = sorted(elem.getRelations(subj='-1', pred='skiros:hasSkill'), key=lambda r: r['dst'])
if skillRel:
skillItem = QTreeWidgetItem(item, ['Skills', '{}_skills'.format(elem.id)])
for rel in skillRel:
if rel['dst'] in scene:
self._create_wm_tree(skillItem, scene, scene[rel['dst']])
skillItem.setExpanded(True)
skillPropRel = sorted(elem.getRelations(subj='-1', pred=self._wmi.get_sub_properties('skiros:skillProperty')), key=lambda r: r['dst'])
for rel in skillPropRel:
if rel['dst'] in scene:
self._create_wm_tree(item, scene, scene[rel['dst']])
def refresh_timer_cb(self):
"""
Keeps ui updated
"""
# Update skill list
if self._sli.has_changes:
self.skill_tree_widget.setSortingEnabled(False)
self.skill_tree_widget.sortByColumn(0, Qt.AscendingOrder)
self.skill_tree_widget.clear()
self.skill_tree_widget.setColumnCount(3)
self.skill_tree_widget.hideColumn(2)
self.skill_tree_widget.hideColumn(1)
fu = QTreeWidgetItem(self.skill_tree_widget, ["Frequently used", "fu"])
fu.setExpanded(True)
root = QTreeWidgetItem(self.skill_tree_widget, ["All", "All"])
root.setExpanded(True)
for ak, e in self._sli._agents.iteritems():
for s in e._skill_list.values():
s.manager = ak
self._add_available_skill(s)
# simplifies hierarchy
self.simplify_tree_hierarchy(root)
self.skill_tree_widget.setSortingEnabled(True)
# select last skill
s = self.skill_tree_widget.findItems(self.last_executed_skill, Qt.MatchRecursive | Qt.MatchFixedString, 1)
self.skill_params_table.setRowCount(0)
if s:
self.skill_tree_widget.setCurrentItem(s[0])
# Update robot BT rate
if self._sli.agents:
robot_info = ""
for name, manager in self._sli.agents.iteritems():
robot_info += "{}: {:0.1f}hz ".format(name.replace("/", ""), manager.get_tick_rate())
self.robot_rate_info.setText(robot_info)
self.robot_output.setText(self.robot_text)
else:
self.robot_rate_info.setText("No robot connected.")
self.robot_output.setText("")
def refresh_actions(self):
actions = {}
for topic in rospy.get_published_topics():
if "/result" in topic[0]:
name = "/".join(topic[0].split("/")[:-1])
msgName = topic[1]
actionType = roslib.message.get_message_class(msgName +
"Action")
goalType = roslib.message.get_message_class(msgName + "Goal")
actions[name] = {"action": actionType, "goal": goalType}
new_actions = {}
for action_name in actions:
# if topic is new or has changed its type
if action_name not in self._actions:
# create new TopicInfo
item = QTreeWidgetItem(self._widget.actions_tree_widget,
[action_name])
new_actions[action_name] = {
'type': actions[action_name],
'item': item
}
else:
# if topic has been seen before, copy it to new dict and
# remove it from the old one
new_actions[action_name] = self._actions[action_name]
del self._actions[action_name]
for action_name in self._actions.keys():
index = self._widget.actions_tree_widget.indexOfTopLevelItem(
self._actions[action_name]['item'])
self._widget.actions_tree_widget.takeTopLevelItem(index)
del self._actions[action_name]
self._actions = new_actions
def on_call_service_button_clicked(self):
self.response_tree_widget.clear()
request = self._service_info['service_class']._request_class()
self.fill_message_slots(request, self._service_info['service_name'], self._service_info['expressions'], self._service_info['counter'])
try:
response = self._service_info['service_proxy'](request)
except rospy.ServiceException as e:
qWarning('ServiceCaller.on_call_service_button_clicked(): request:\n%r' % (request))
qWarning('ServiceCaller.on_call_service_button_clicked(): error calling service "%s":\n%s' % (self._service_info['service_name'], e))
top_level_item = QTreeWidgetItem()
top_level_item.setText(self._column_index['service'], 'ERROR')
top_level_item.setText(self._column_index['type'], 'rospy.ServiceException')
top_level_item.setText(self._column_index['expression'], str(e))
else:
#qDebug('ServiceCaller.on_call_service_button_clicked(): response: %r' % (response))
top_level_item = self._recursive_create_widget_items(None, '/', response._type, response, is_editable=False)
self.response_tree_widget.addTopLevelItem(top_level_item)
# resize columns
self.response_tree_widget.expandAll()
for i in range(self.response_tree_widget.columnCount()):
self.response_tree_widget.resizeColumnToContents(i)
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 _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 __init__(self, context):
super(RecordUI, self).__init__(context)
self._widget = QMainWindow()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('bitbots_recordui_rqt'), 'resource', 'RecordUI.ui')
loadUi(ui_file, self._widget, {})
self._recorder = Recorder()
self._sliders = {}
self._textFields = {}
self._motorSwitched = {}
self._selected_frame = None
self._currentGoals = {} # this is the data about the current unsaved frame
self._currentDuration = 1.0
self._currentPause = 0.0
self._currentName = "new frame"
self._workingValues = {} # this is the data about the frame that is displayed
self._workingDuration = 1.0
self._workingPause = 0.0
self._workingName = self._currentName
self._current = True
self._saveDir = None
self._robot_anim_path = None
#save current frame when switching to other frames for reference
#working frame
self._treeItems = {}
self._motorCheckBody = QTreeWidgetItem(self._widget.motorTree)
self._motorCheckLegs = QTreeWidgetItem(self._motorCheckBody)
self._motorCheckArms = QTreeWidgetItem(self._motorCheckBody)
self._motorCheckHead = QTreeWidgetItem(self._motorCheckBody)
self._motorCheckLArm = QTreeWidgetItem(self._motorCheckArms)
self._motorCheckRArm = QTreeWidgetItem(self._motorCheckArms)
self._motorCheckLLeg = QTreeWidgetItem(self._motorCheckLegs)
self._motorCheckRLeg = QTreeWidgetItem(self._motorCheckLegs)
#saves configuration of the trees checkboxes for each of the tree modes.
self._checkBoxesSave = {}
self._checkBoxesPower = {}
self._previousTreeMode = 0
self.setObjectName('Record Animation')
self._initial_joints = None
self._widget.frameList = DragDropList(self._widget, self)
self._widget.verticalLayout_2.insertWidget(0, self._widget.frameList)
self._widget.frameList.setDragDropMode(QAbstractItemView.InternalMove)
self.state_sub = rospy.Subscriber("/joint_states", JointState, self.state_update, queue_size=1, tcp_nodelay=True)
self._joint_pub = rospy.Publisher("/record_motor_goals", JointCommand, queue_size=1)
self.effort_pub = rospy.Publisher("/ros_control/set_torque_individual", JointTorque, queue_size=1)
self.ids = {"HeadPan": 0,
"HeadTilt": 1,
"LShoulderPitch": 2,
"LShoulderRoll": 3,
"LElbow": 4,
"RShoulderPitch": 5,
"RShoulderRoll": 6,
"RElbow": 7,
"LHipYaw": 8,
"LHipRoll": 9,
"LHipPitch": 10,
"LKnee": 11,
"LAnklePitch": 12,
"LAnkleRoll": 13,
"RHipYaw": 14,
"RHipRoll": 15,
"RHipPitch": 16,
"RKnee": 17,
"RAnklePitch": 18,
"RAnkleRoll": 19}
self._initial_joints = JointState()
self.update_time = rospy.Duration(0.1)
for k,v in self.ids.iteritems():
rospy.loginfo(k)
self._initial_joints.name.append(k)
self._initial_joints.position.append(0)
while not self._initial_joints:
if not rospy.is_shutdown():
time.rospy.sleep(0.5)
rospy.logwarn("wait")
else:
return
self.initialize()
context.add_widget(self._widget)
self._widget.statusBar.showMessage("Initialization complete.")
class RecordUI(Plugin):
''' class containing the UI part of the framework'''
def __init__(self, context):
super(RecordUI, self).__init__(context)
self._widget = QMainWindow()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('bitbots_recordui_rqt'), 'resource', 'RecordUI.ui')
loadUi(ui_file, self._widget, {})
self._recorder = Recorder()
self._sliders = {}
self._textFields = {}
self._motorSwitched = {}
self._selected_frame = None
self._currentGoals = {} # this is the data about the current unsaved frame
self._currentDuration = 1.0
self._currentPause = 0.0
self._currentName = "new frame"
self._workingValues = {} # this is the data about the frame that is displayed
self._workingDuration = 1.0
self._workingPause = 0.0
self._workingName = self._currentName
self._current = True
self._saveDir = None
self._robot_anim_path = None
#save current frame when switching to other frames for reference
#working frame
self._treeItems = {}
self._motorCheckBody = QTreeWidgetItem(self._widget.motorTree)
self._motorCheckLegs = QTreeWidgetItem(self._motorCheckBody)
self._motorCheckArms = QTreeWidgetItem(self._motorCheckBody)
self._motorCheckHead = QTreeWidgetItem(self._motorCheckBody)
self._motorCheckLArm = QTreeWidgetItem(self._motorCheckArms)
self._motorCheckRArm = QTreeWidgetItem(self._motorCheckArms)
self._motorCheckLLeg = QTreeWidgetItem(self._motorCheckLegs)
self._motorCheckRLeg = QTreeWidgetItem(self._motorCheckLegs)
#saves configuration of the trees checkboxes for each of the tree modes.
self._checkBoxesSave = {}
self._checkBoxesPower = {}
self._previousTreeMode = 0
self.setObjectName('Record Animation')
self._initial_joints = None
self._widget.frameList = DragDropList(self._widget, self)
self._widget.verticalLayout_2.insertWidget(0, self._widget.frameList)
self._widget.frameList.setDragDropMode(QAbstractItemView.InternalMove)
self.state_sub = rospy.Subscriber("/joint_states", JointState, self.state_update, queue_size=1, tcp_nodelay=True)
self._joint_pub = rospy.Publisher("/record_motor_goals", JointCommand, queue_size=1)
self.effort_pub = rospy.Publisher("/ros_control/set_torque_individual", JointTorque, queue_size=1)
self.ids = {"HeadPan": 0,
"HeadTilt": 1,
"LShoulderPitch": 2,
"LShoulderRoll": 3,
"LElbow": 4,
"RShoulderPitch": 5,
"RShoulderRoll": 6,
"RElbow": 7,
"LHipYaw": 8,
"LHipRoll": 9,
"LHipPitch": 10,
"LKnee": 11,
"LAnklePitch": 12,
"LAnkleRoll": 13,
"RHipYaw": 14,
"RHipRoll": 15,
"RHipPitch": 16,
"RKnee": 17,
"RAnklePitch": 18,
"RAnkleRoll": 19}
self._initial_joints = JointState()
self.update_time = rospy.Duration(0.1)
for k,v in self.ids.iteritems():
rospy.loginfo(k)
self._initial_joints.name.append(k)
self._initial_joints.position.append(0)
while not self._initial_joints:
if not rospy.is_shutdown():
time.rospy.sleep(0.5)
rospy.logwarn("wait")
else:
return
self.initialize()
context.add_widget(self._widget)
self._widget.statusBar.showMessage("Initialization complete.")
def initialize(self):
for i in range(0, len(self._initial_joints.name)):
self._currentGoals[self._initial_joints.name[i]] = self._initial_joints.position[i]
self._workingValues[self._initial_joints.name[i]] = self._initial_joints.position[i]
self._motorSwitched[self._initial_joints.name[i]] = True
host = os.environ['ROS_MASTER_URI'].split('/')[2].split(':')[0]
self._robot_anim_path = "bitbots@{}:~/wolfgang_ws/src/wolfgang_robot/wolfgang_animations/animations/motion/".format(host)
rospy.loginfo("Set animation path to: "+str(self._robot_anim_path+"record.json"))
initTorque = {}
for k, v in self._workingValues.items():
initTorque[k] = 2
self._checkBoxesPower['#CURRENT_FRAME'] = initTorque
self.motor_controller()
self.motor_switcher()
self.action_connect()
self.box_ticked()
self.frame_list()
self.update_frames()
self.set_sliders_and_text_fields(manual=True)
def state_update(self, joint_states):
'''
Callback method for /joint_states. Updates the sliders to the actual values of the motors when the robot moves.
'''
if not self._initial_joints:
self._initial_joints = joint_states
time.rospy.sleep(1)
else:
for i in range(0, len(joint_states.name)):
if (not self._motorSwitched[joint_states.name[i]]):
self._workingValues[joint_states.name[i]] = joint_states.position[i]
rospy.sleep(self.update_time)
self.set_sliders_and_text_fields(manual=False)
def motor_controller(self):
'''
Sets up the GUI in the middle of the Screen to control the motors.
Uses self._motorValues to determine which motors are present.
'''
i = 0
for k, v in sorted(self._currentGoals.items()):
group = QGroupBox()
slider = QSlider(Qt.Horizontal)
slider.setTickInterval(1)
slider.setMinimum(-181)
slider.setMaximum(181)
slider.valueChanged.connect(self.slider_update)
self._sliders[k] = slider
textfield = QLineEdit()
textfield.setText('0')
textfield.textEdited.connect(self.textfield_update)
self._textFields[k] = textfield
label = QLabel()
label.setText(k)
layout = QVBoxLayout()
layout.addWidget(label)
layout.addWidget(self._sliders[k])
layout.addWidget(self._textFields[k])
group.setLayout(layout)
self._widget.motorControlLayout.addWidget(group, i / 5, i % 5)
i = i+1
def action_connect(self):
'''
Connects the actions in the top bar to the corresponding functions, and sets their shortcuts
:return:
'''
self._widget.actionNew.triggered.connect(self.new)
self._widget.actionOpen.triggered.connect(self.open)
self._widget.actionSave.triggered.connect(self.save)
self._widget.actionSave_as.triggered.connect(self.save_as)
self._widget.actionInit.triggered.connect(self.goto_init)
self._widget.actionCurrent_Frame.triggered.connect(self.goto_frame)
self._widget.actionNext_Frame.triggered.connect(self.goto_next)
self._widget.actionAnimation.triggered.connect(self.play)
self._widget.actionAnimation_until_Frame.triggered.connect(self.play_until)
self._widget.actionDuplicate_Frame.triggered.connect(self.duplicate)
self._widget.actionDelete_Frame.triggered.connect(self.delete)
self._widget.actionLeft.triggered.connect(lambda: self.mirrorFrame("L"))
self._widget.actionRight.triggered.connect(lambda: self.mirrorFrame("R"))
self._widget.actionInvert.triggered.connect(self.invertFrame)
self._widget.actionUndo.triggered.connect(self.undo)
self._widget.actionRedo.triggered.connect(self.redo)
self._widget.actionHelp.triggered.connect(self.help)
self._widget.buttonRecord.clicked.connect(self.record)
self._widget.buttonRecord.shortcut = QShortcut(QKeySequence("Space"), self._widget)
self._widget.buttonRecord.shortcut.activated.connect(self.record)
self._widget.frameList.keyPressed.connect(self.delete)
self._widget.treeModeSelector.currentIndexChanged.connect(self.treeModeChanged)
def help(self):
'''
Prints out the help dialogue
'''
helpDialog = QMessageBox.about(self._widget, "About RecordUI", "This is RecordUI, a tool to record robot animations.\n \n Keyboard shortcuts: \n \n \
New: Ctrl + N \n \
Open: Ctrl + O \n \
Save: Ctrl + S \n \
Save as: Ctrl + Shift + S \n \n \
Play Init: Ctrl + I \n \
Play Frame: Ctrl + P \n \
Play Next: Alt + P \n \
Play Animation: Ctrl + Shift + P \n \
Play Until: Ctrl + Alt + P \n \n \
Record Frame: Space \n \
Duplicate Frame: '+' \n \
Delete Frame: '-' \n \
Undo: Ctrl + Z \n \
Redo: Ctrl + Y \n \
Help: Ctrl + H \n \n \
Mirror to Left: Ctrl + Left Arrow \n \
Mirror to Right: Ctrl + Right Arrow \n \
Invert: Ctrl + Down Arrow")
def new(self):
'''
Deletes all currently recorded frames
'''
if len(self._widget.frameList) > 1:
message = "This will discard your current Animation. Continue?"
sure = QMessageBox.question(self._widget, 'Sure?', message, QMessageBox.Yes | QMessageBox.No)
if sure == QMessageBox.Yes:
self._recorder.clear()
self.update_frames()
def save(self):
'''
Saves all currently recorded frames into a json file
'''
self.treeModeChanged(self._previousTreeMode)
self.set_metadata()
if not self._saveDir:
self._saveDir = QFileDialog.getExistingDirectory()
status = self._recorder.save_animation(self._saveDir, self._widget.lineAnimationName.text(), self._checkBoxesSave)
self._widget.statusBar.showMessage(status)
def save_as(self):
'''
Saves all currently recorded frames into a json file, which is saved at a user specified location
'''
self.treeModeChanged(self._previousTreeMode)
self._saveDir = QFileDialog.getExistingDirectory()
self._recorder.save_animation(self._saveDir, self._widget.lineAnimationName.text(), self._checkBoxesSave)
def set_metadata(self):
status = self._recorder.set_meta_data(self._widget.lineAnimationName.text(),
self._widget.lineVersion.text(),
self._widget.lineAuthor.text(),
self._widget.fieldDescription.toPlainText())
self._widget.statusBar.showMessage(status)
def open(self):
'''
Deletes all current frames and instead loads an animation from a json file
'''
if len(self._widget.frameList) > 1:
message = "This will discard your current Animation. Continue?"
sure = QMessageBox.question(self._widget, 'Sure?', message, QMessageBox.Yes | QMessageBox.No)
if sure == QMessageBox.No:
return
my_file = QFileDialog.getOpenFileName()[0]
if my_file:
status = self._recorder.load_animation(my_file)
if status == "":
status = "Load successful."
self._widget.statusBar.showMessage(status)
animstate = self._recorder.get_animation_state()
for i in animstate:
try:
self._checkBoxesPower[i['name']] = i['torque']
except KeyError:
self._checkBoxesPower[i['name']] = {}
for key in self.ids:
self._checkBoxesPower[i['name']][key] = 2
self.update_frames()
metadata = self._recorder.get_meta_data()
self._widget.lineAnimationName.setText(metadata[0])
self._widget.lineAuthor.setText(metadata[2])
self._widget.lineVersion.setText(str(metadata[1]))
self._widget.fieldDescription.setPlainText(metadata[3])
def play(self):
'''
Plays the animation
'''
status = self._recorder.play(self._robot_anim_path)
self._widget.statusBar.showMessage(status)
def play_until(self):
'''
Plays the animation up to a certain frame
'''
steps = self._recorder.get_animation_state()
j = 0
for i in range(0, len(steps)):
j += 1
rospy.loginfo(steps[i]["name"])
if steps[i]["name"] == self._selected_frame["name"]:
break
self._recorder.play(self._robot_anim_path, j)
def goto_frame(self):
'''
Plays one single frame
'''
self.set_all_joints_stiff()
pos_msg = JointCommand()
pos_msg.joint_names = []
pos_msg.velocities = [1.0] * 20
pos_msg.positions = []
pos_msg.accelerations = [-1.0] * 20
pos_msg.max_currents = [-1.0] * 20
for k,v in self._workingValues.items():
pos_msg.joint_names.append(k)
pos_msg.positions.append(v)
torque_msg = JointTorque()
torque_msg.joint_names = []
torque_msg.on = []
for k, v in self._checkBoxesPower[self._widget.frameList.currentItem().text()].items():
torque_msg.joint_names.append(k)
torque_msg.on.append(v)
self.effort_pub.publish(torque_msg)
self._joint_pub.publish(pos_msg)
def goto_next(self):
if self._widget.frameList.currentRow() < self._widget.frameList.count() - 2:
self._widget.frameList.setCurrentRow(self._widget.frameList.currentRow()+1)
self.goto_frame()
def goto_init(self):
'''
Plays init frame, sets all motor values to 0
'''
self.set_all_joints_stiff()
if self._widget.frameList.currentItem().text() == "#CURRENT_FRAME":
for k, v in self._workingValues.iteritems():
self._workingValues[k] = 0.0
for k, v in self._currentGoals.iteritems():
self._currentGoals[k] = 0.0
self.set_sliders_and_text_fields(manual=False)
pos_msg = JointCommand()
pos_msg.joint_names = self._initial_joints.name
pos_msg.velocities = [1.0] * len(self._initial_joints.name)
pos_msg.positions = [0.0] * len(self._initial_joints.name)
pos_msg.accelerations = [-1.0] * len(self._initial_joints.name)
pos_msg.max_currents = [-1.0] * len(self._initial_joints.name)
self._joint_pub.publish(pos_msg)
def set_all_joints_stiff(self):
'''
Enables torque for all motors
'''
if self._widget.frameList.currentItem().text() == '#CURRENT_FRAME':
for k, v in self._treeItems.items():
v.setCheckState(0, Qt.Checked)
torque_msg = JointTorque()
torque_msg.joint_names = []
torque_msg.on = []
for k, v in sorted(self._treeItems.items()):
torque_msg.joint_names.append(k)
torque_msg.on.append(True)
self.effort_pub.publish(torque_msg)
def duplicate(self):
'''
Creates a copy of the selected frame
'''
try:
frame = self._widget.frameList.selectedItems()[0].text()
except:
return
if frame:
if not frame == "#CURRENT_FRAME":
self._recorder.duplicate(frame)
self._widget.statusBar.showMessage("Duplicated frame "+ frame)
else:
self._widget.statusBar.showMessage("Cannot duplicate current frame. Record first.")
self.update_frames()
def delete(self):
'''
Deletes a frame
'''
try:
frame = self._widget.frameList.selectedItems()[0].text()
except:
return
if frame:
if not frame == "#CURRENT_FRAME":
self._recorder.delete(frame)
self._widget.statusBar.showMessage("Deleted frame "+ frame)
else:
self._widget.statusBar.showMessage("Cannot delete current frame.")
self.update_frames()
def record(self, keep=False):
'''
Records a frame
'''
if not self._widget.frameList.currentItem() == None:
if self._widget.frameList.currentItem().text() == "#CURRENT_FRAME":
x = True
n = 0
for state in self._recorder.get_animation_state():
if self._workingName == state["name"]:
while(x):
x = False
for state in self._recorder.get_animation_state():
if self._workingName+str(n) == state["name"]:
n+=1
x = True
self._workingName = self._workingName+str(n)
self.set_sliders_and_text_fields(manual=True)
self._checkBoxesPower[self._workingName] = {}
initTorque = {}
for k, v in self._workingValues.items():
initTorque[k] = 2
self._checkBoxesPower[self._workingName] = initTorque
self._recorder.record(self._workingValues,
initTorque,
self._widget.lineFrameName.text(),
self._widget.spinBoxDuration.value(),
self._widget.spinBoxPause.value())
self._currentGoals = deepcopy(self._workingValues)
else:
current_row = self._widget.frameList.currentRow()
self._recorder.record(self._workingValues,
self._checkBoxesPower[self._widget.frameList.currentItem().text()],
self._widget.lineFrameName.text(),
self._widget.spinBoxDuration.value(),
self._widget.spinBoxPause.value(),
current_row,
True)
self._widget.statusBar.showMessage("Recorded frame "+ self._workingName)
self.update_frames(keep)
def undo(self):
'''
Undos the previous action
'''
status = self._recorder.undo()
self._widget.statusBar.showMessage(status)
self.update_frames()
def redo(self):
'''
Redos an action
'''
status = self._recorder.redo()
self._widget.statusBar.showMessage(status)
self.update_frames()
def mirrorFrame(self, direction):
'''
Copies all motor values from one side of the robot to the other. Inverts values, if necessary
'''
opposite = "L"
if direction == "L":
opposite = "R"
for k, v in self._workingValues.items():
if k[0] == opposite:
for k1, v1 in self._workingValues.items():
if k1 == str(direction)+k[1:]:
self._workingValues[k1] = v * -1
boxmode = 0
if self._widget.treeModeSelector.currentIndex() == 0:
boxmode = self._checkBoxesPower[self._widget.frameList.currentItem().text()]
elif self._widget.treeModeSelector.currentIndex() == 1:
boxmode = self._checkBoxesSave[self._widget.frameList.currentItem().text()]
for k, v in boxmode.items():
if k[0] == opposite:
for k1, v1 in boxmode.items():
if k1 == str(direction)+k[1:]:
if boxmode[k] == 0:
boxmode[k1] = 0
elif boxmode[k] ==2:
boxmode[k1] = 2
self.updateTreeConfig(self._widget.treeModeSelector.currentIndex())
self.box_ticked()
self._widget.statusBar.showMessage("Mirrored frame to "+ direction)
def invertFrame(self):
'''
Copies all values from the left side to the right and all values from the right side to the left.
Inverts values, if necessary
'''
tempDict={}
for k, v in self._workingValues.items():
if k[0] == "L":
tempDict["R"+k[1:]] = -v
elif k[0] == "R":
tempDict["L"+k[1:]] = -v
for k, v in tempDict.items():
self._workingValues[k] = v
boxmode = 0
if self._widget.treeModeSelector.currentIndex() == 0:
boxmode = self._checkBoxesPower
elif self._widget.treeModeSelector.currentIndex() == 1:
boxmode = self._checkBoxesSave
for k, v in boxmode[self._widget.frameList.currentItem().text()].items():
if k[0] == "L":
if v == 2:
tempDict["R"+k[1:]] = 2
elif v == 0:
tempDict["R"+k[1:]] = 0
elif k[0] == "R":
if v == 2:
tempDict["L"+k[1:]] = 2
elif v == 0:
tempDict["L"+k[1:]] = 0
boxmode[self._widget.frameList.currentItem().text()] = deepcopy(tempDict)
self.updateTreeConfig(self._widget.treeModeSelector.currentIndex())
self.box_ticked()
self._widget.statusBar.showMessage("Inverted frame")
def frame_list(self):
'''
Connects triggers from the frame list to their callback methods.
'''
self._widget.frameList.itemSelectionChanged.connect(self.frame_select)
self._widget.lineFrameName.textEdited.connect(self.frame_meta_update)
self._widget.spinBoxPause.valueChanged.connect(self.frame_meta_update)
self._widget.spinBoxDuration.valueChanged.connect(self.frame_meta_update)
def frame_meta_update(self):
'''
Updates the respective values for the two spin boxes and the frame name, when they are changed
'''
self._workingDuration = self._widget.spinBoxDuration.value()
self._workingPause = self._widget.spinBoxPause.value()
self._workingName = self._widget.lineFrameName.text()
def frame_select(self):
'''
Loads all information on a specific frame into the working values, if the frame selection changes
'''
if not (self._widget.frameList.currentItem() == None):
self.copyOldTreeConfig()
selected_frame_name = self._widget.frameList.currentItem().text()
self._selected_frame = None
for v in self._recorder.get_animation_state():
if v["name"] == selected_frame_name:
self._selected_frame = v
break
#save current values to _currentValues if switching from current frame to different one
#when switching from current to different, save current values
if selected_frame_name == "#CURRENT_FRAME":
self._widget.treeModeSelector.setCurrentIndex(0)
self.updateTreeConfig(0)
self._widget.treeModeSelector.setEnabled(False)
self._workingValues = deepcopy(self._currentGoals)
self._workingName = deepcopy(self._currentName)
self._workingDuration = deepcopy(self._currentDuration)
self._workingPause = deepcopy(self._currentPause)
self._current = True
else:
if self._selected_frame == None:
return
self._widget.treeModeSelector.setEnabled(True)
if self._current:
self._currentGoals = deepcopy(self._workingValues)
self._currentName = deepcopy(self._workingName)
self._currentDuration = deepcopy(self._workingDuration)
self._currentPause = deepcopy(self._workingPause)
self._workingValues = self._selected_frame["goals"]
self._workingName = self._selected_frame["name"]
self._workingPause = self._selected_frame["pause"]
self._workingDuration = float(self._selected_frame["duration"])
self._widget.lineFrameName.setText(self._widget.frameList.currentItem().text())
self._current = False
self.updateTreeConfig(self._previousTreeMode)
if self._widget.treeModeSelector.currentIndex() == 0:
for k, v in self._treeItems.items():
self._motorSwitched[k] = (v.checkState(0) == 2)
for k, v in self._motorSwitched.items():
self._textFields[k].setEnabled(v)
self._sliders[k].setEnabled(v)
self.set_sliders_and_text_fields(manual=False)
self.box_ticked()
def motor_switcher(self):
'''
Loads the motors into the tree and adds the checkboxes
'''
self._widget.motorTree.setHeaderLabel("Stiff Motors")
self._motorCheckBody.setText(0, "Body")
self._motorCheckBody.setFlags(self._motorCheckBody.flags() | Qt.ItemIsTristate | Qt.ItemIsUserCheckable)
self._motorCheckBody.setExpanded(True)
self._motorCheckHead.setText(0, "Head")
self._motorCheckHead.setFlags(self._motorCheckHead.flags() | Qt.ItemIsTristate | Qt.ItemIsUserCheckable)
self._motorCheckHead.setExpanded(True)
self._motorCheckArms.setText(0, "Arms")
self._motorCheckArms.setFlags(self._motorCheckArms.flags() | Qt.ItemIsTristate | Qt.ItemIsUserCheckable)
self._motorCheckArms.setExpanded(True)
self._motorCheckLegs.setText(0, "Legs")
self._motorCheckLegs.setFlags(self._motorCheckLegs.flags() | Qt.ItemIsTristate | Qt.ItemIsUserCheckable)
self._motorCheckLegs.setExpanded(True)
self._motorCheckLArm.setText(0, "Left Arm")
self._motorCheckLArm.setFlags(self._motorCheckLArm.flags() | Qt.ItemIsTristate | Qt.ItemIsUserCheckable)
self._motorCheckLArm.setExpanded(True)
self._motorCheckRArm.setText(0, "Right Arm")
self._motorCheckRArm.setFlags(self._motorCheckRArm.flags() | Qt.ItemIsTristate | Qt.ItemIsUserCheckable)
self._motorCheckRArm.setExpanded(True)
self._motorCheckLLeg.setText(0, "Left Leg")
self._motorCheckLLeg.setFlags(self._motorCheckLLeg.flags() | Qt.ItemIsTristate | Qt.ItemIsUserCheckable)
self._motorCheckLLeg.setExpanded(True)
self._motorCheckRLeg.setText(0, "Right Leg")
self._motorCheckRLeg.setFlags(self._motorCheckRLeg.flags() | Qt.ItemIsTristate | Qt.ItemIsUserCheckable)
self._motorCheckRLeg.setExpanded(True)
for k, v in self._currentGoals.items():
parent = None
if 'LHip' in k or 'LKnee' in k or 'LAnkle' in k:
parent = self._motorCheckLLeg
elif 'RHip' in k or 'RKnee' in k or 'RAnkle' in k:
parent = self._motorCheckRLeg
elif 'LShoulder' in k or 'LElbow' in k:
parent = self._motorCheckLArm
elif 'RShoulder' in k or 'RElbow' in k:
parent = self._motorCheckRArm
elif 'Head' in k:
parent = self._motorCheckHead
child = QTreeWidgetItem(parent)
child.setText(0, k)
child.setFlags(child.flags() | Qt.ItemIsUserCheckable)
child.setCheckState(0, Qt.Checked)
self._treeItems[k] = child
self._widget.motorTree.itemClicked.connect(self.box_ticked)
def copyOldTreeConfig(self):
'''
Saves the current configuration of the motor tree checkboxes into the corresponding dictionary
'''
if not self._selected_frame == None:
tempDict={}
for k, v in self._treeItems.items():
tempDict[k]= self._treeItems[k].checkState(0)
if self._previousTreeMode == 1:
self._checkBoxesSave[self._selected_frame["name"]] = deepcopy(tempDict)
elif self._previousTreeMode == 0:
self._checkBoxesPower[self._selected_frame["name"]] = deepcopy(tempDict)
def updateTreeConfig(self, index):
'''
Loads the new configuration of the motor tree depending on the change
'''
tempDict2={}
if not self._widget.frameList.currentItem() == None:
if not self._widget.frameList.currentItem().text() == "#CURRENT_FRAME":
if index == 1:
if self._selected_frame["name"] in self._checkBoxesSave.keys():
tempDict2 = deepcopy(self._checkBoxesSave[self._selected_frame["name"]])
else:
for k in self._workingValues:
tempDict2[k] = 2
elif index == 0:
if self._selected_frame["name"] in self._checkBoxesPower.keys():
tempDict2 = deepcopy(self._checkBoxesPower[self._selected_frame["name"]])
else:
for k in self._workingValues:
tempDict2[k] = 2
else:
for k in self._workingValues:
tempDict2[k] = 2
self._previousTreeMode = index
for k, v in tempDict2.items():
if v == 0:
self._treeItems[k].setCheckState(0, Qt.Unchecked)
elif v == 1:
self._treeItems[k].setCheckState(0, Qt.PartiallyChecked)
elif v == 2:
self._treeItems[k].setCheckState(0, Qt.Checked)
def treeModeChanged(self, index):
self.copyOldTreeConfig()
self.updateTreeConfig(index)
def slider_update(self):
'''
Updates all sliders, checks whether a value is too large, then replaces it
'''
for k, v in self._sliders.items():
self._workingValues[k] = math.radians(v.value())
if self._workingValues[k] < -math.pi:
self._workingValues[k] = -math.pi
elif self._workingValues[k] > math.pi:
self._workingValues[k] = math.pi
self.set_sliders_and_text_fields(manual=True)
def textfield_update(self):
'''
Updates all textfields.
'''
for k, v in self._textFields.items():
try:
self._workingValues[k] = math.radians(float(v.text()))
except ValueError:
continue
self.set_sliders_and_text_fields(manual=True)
def set_sliders_and_text_fields(self, manual):
'''
Updates the text fields and sliders in self._sliders and self._textfields and also frame name and duration and pause
to the values in self._workingValues.
'''
for k, v in self._workingValues.items():
try:
if not self._treeItems[k].checkState(0) == 0:
self._textFields[k].setText(str(int(round(math.degrees(v)))))
self._sliders[k].setValue(int(round(math.degrees(v))))
except KeyError:
rospy.logwarn("Found a key-value pair for motor (%s), which doesn't seem to exist (yet). Ignoring it." % k)
self._widget.statusBar.showMessage("Found a key-value pair for motor (%s), which doesn't seem to exist (yet). Ignoring it.")
continue
except RuntimeError:
rospy.logwarn("Tried to access a PyQt element that no longer exists. This happens when you close the framework.")
self._widget.statusBar.showMessage("Tried to access a PyQt element that no longer exists. This happens when you close the framework.")
break
if manual:
self._widget.lineFrameName.setText(self._workingName)
self._widget.spinBoxDuration.setValue(self._workingDuration)
self._widget.spinBoxPause.setValue(self._workingPause)
def box_ticked(self):
'''
Controls whether a checkbox has been clicked, and reacts.
'''
for k, v in self._treeItems.items():
self._motorSwitched[k] = (v.checkState(0) == 2)
for k, v in self._motorSwitched.items():
self._textFields[k].setEnabled(v)
self._sliders[k].setEnabled(v)
self.set_sliders_and_text_fields(manual=False)
torque_msg = JointTorque()
torque_msg.joint_names = []
torque_msg.on = []
if self._widget.treeModeSelector.currentIndex() == 0:
for k, v in sorted(self._treeItems.items()):
torque_msg.joint_names.append(k)
torque_msg.on.append(v.checkState(0) == 2)
pos_msg = JointCommand()
pos_msg.joint_names = []
pos_msg.velocities = [1.0] * 20
pos_msg.positions = []
pos_msg.accelerations = [-1.0] * 20
pos_msg.max_currents = [-1.0] * 20
for k,v in self._workingValues.items():
pos_msg.joint_names.append(k)
pos_msg.positions.append(v)
self._joint_pub.publish(pos_msg)
self.effort_pub.publish(torque_msg)
if not self._widget.frameList.currentItem() == None:
if not self._widget.frameList.currentItem().text() == "#CURRENT_FRAME":
self.treeModeChanged(self._widget.treeModeSelector.currentIndex())
def update_frames(self, keep=False):
'''
updates the list of frames present in the current animation
:return:
'''
current_index = self._widget.frameList.currentIndex()
current_mode = self._widget.treeModeSelector.currentIndex()
current_state = self._recorder.get_animation_state()
while self._widget.frameList.takeItem(0):
continue
for i in range(0, len(current_state)):
item = QListWidgetItem()
if "name" in current_state[i]:
item.setText(current_state[i]["name"])
else: #older animations without names for the frames
item.setText(str(i))
self._widget.frameList.addItem(item)
current = QListWidgetItem()
current.setText("#CURRENT_FRAME")
self._widget.frameList.addItem(current)
if keep:
self._widget.frameList.setCurrentIndex(current_index)
self._widget.treeModeSelector.setCurrentIndex(current_mode)
else:
self._widget.frameList.setCurrentItem(current)
self._current = True
def change_frame_order(self, new_order):
''' Calls the recorder to update frame order and updates the gui'''
self._recorder.change_frame_order(new_order)
self.update_frames()
def shutdown_plugin(self):
'''Clean up by sending the HCM a signal that we are no longer recording'''
self._joint_pub.publish(JointCommand())
rospy.sleep(1)
def add_to_tree(self, path, parent):
"""Add a path to the tree view."""
if parent is None:
container = QTreeWidgetItem()
container.setText(0, self._containers[path]._label)
self._widget.tree.addTopLevelItem(container)
else:
container = QTreeWidgetItem(parent)
container.setText(0, self._containers[path]._label)
# Add children to_tree
for label in self._containers[path]._children:
child_path = '/'.join([path, label])
if child_path in self._containers.keys():
self.add_to_tree(child_path, container)
else:
child = QTreeWidgetItem(container)
child.setText(0, label)
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 refresh_tree(self):
self.select_tree.itemChanged.disconnect()
self.select_tree.clear()
for joint_name in self.joint_names:
item = QTreeWidgetItem(self.select_tree)
item.setText(0, joint_name)
item.setCheckState(0, Qt.Unchecked)
item.setFlags(Qt.ItemIsUserCheckable | Qt.ItemIsEnabled)
for traj_name in ['position', 'velocity', 'acceleration', 'effort']:
sub_item = QTreeWidgetItem(item)
sub_item.setText(0, traj_name)
sub_item.setCheckState(0, Qt.Unchecked)
sub_item.setFlags(Qt.ItemIsUserCheckable | Qt.ItemIsEnabled)
self.select_tree.itemChanged.connect(self.update_checkbox)
class RuntimeMonitorWidget(QWidget):
def __init__(self, topic="diagnostics"):
super(RuntimeMonitorWidget, self).__init__()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_runtime_monitor'), 'resource', 'runtime_monitor_widget.ui')
loadUi(ui_file, self)
self.setObjectName('RuntimeMonitorWidget')
self._mutex = threading.Lock()
self._error_icon = QIcon.fromTheme('dialog-error')
self._warning_icon = QIcon.fromTheme('dialog-warning')
self._ok_icon = QIcon.fromTheme('dialog-information')
self._stale_node = QTreeWidgetItem(self.tree_widget.invisibleRootItem(), ['Stale (0)'])
self._stale_node.setIcon(0, self._error_icon)
self.tree_widget.addTopLevelItem(self._stale_node)
self._error_node = QTreeWidgetItem(self.tree_widget.invisibleRootItem(), ['Errors (0)'])
self._error_node.setIcon(0, self._error_icon)
self.tree_widget.addTopLevelItem(self._error_node)
self._warning_node = QTreeWidgetItem(self.tree_widget.invisibleRootItem(), ['Warnings (0)'])
self._warning_node.setIcon(0, self._warning_icon)
self.tree_widget.addTopLevelItem(self._warning_node)
self._ok_node = QTreeWidgetItem(self.tree_widget.invisibleRootItem(), ['Ok (0)'])
self._ok_node.setIcon(0, self._ok_icon)
self.tree_widget.addTopLevelItem(self._ok_node)
self.tree_widget.itemSelectionChanged.connect(self._refresh_selection)
self.keyPressEvent = self._on_key_press
self._name_to_item = {}
self._new_errors_callback = None
self._subscriber = rospy.Subscriber(topic, DiagnosticArray, self._diagnostics_callback)
self._previous_ros_time = rospy.Time.now()
self._timer = QTimer()
self._timer.timeout.connect(self._on_timer)
self._timer.start(1000)
self._msg_timer = QTimer()
self._msg_timer.timeout.connect(self._update_messages)
self._msg_timer.start(100)
self._messages = []
self._used_items = 0
def __del__(self):
self.shutdown()
def shutdown(self):
"""
Unregisters subscriber and stops timers
"""
self._msg_timer.stop()
self._timer.stop()
if rospy.is_shutdown():
return
if self._subscriber:
self._subscriber.unregister()
self._subscriber = None
def change_diagnostic_topic(self, topic):
"""
Changes diagnostics topic name. Must be of type diagnostic_msgs/DiagnosticArray
"""
if not topic:
self.reset_monitor()
return
if self._subscriber:
self._subscriber.unregister()
self._subscriber = rospy.Subscriber(str(topic), DiagnosticArray, self._diagnostics_callback)
self.reset_monitor()
def reset_monitor(self):
"""
Removes all values from monitor display, resets buffers
"""
self._name_to_item = {} # Reset all stale topics
self._messages = []
self._clear_tree()
def _clear_tree(self):
for index in range(self._stale_node.childCount()):
self._stale_node.removeChild(self._stale_node.child(index))
for index in range(self._error_node.childCount()):
self._error_node.removeChild(self._error_node.child(index))
for index in range(self._warning_node.childCount()):
self._warning_node.removeChild(self._warning_node.child(index))
for index in range(self._ok_node.childCount()):
self._ok_node.removeChild(self._ok_node.child(index))
self._update_root_labels()
# Diagnostics callbacks (subscriber thread)
def _diagnostics_callback(self, message):
with self._mutex:
self._messages.append(message)
# Update display of messages from main thread
def _update_messages(self):
with self._mutex:
messages = self._messages[:]
self._messages = []
had_errors = False
for message in messages:
for status in message.status:
was_selected = False
if (self._name_to_item.has_key(status.name)):
item = self._name_to_item[status.name]
if item.tree_node.isSelected():
was_selected = True
if (item.status.level == DiagnosticStatus.ERROR and status.level != DiagnosticStatus.ERROR):
had_errors = True
self._update_item(item, status, was_selected)
else:
self._create_item(status, was_selected, True)
if (status.level == DiagnosticStatus.ERROR):
had_errors = True
if (had_errors and self._new_errors_callback != None):
self._new_errors_callback()
self._update_root_labels()
self.update()
self._refresh_selection()
def _update_item(self, item, status, was_selected):
change_parent = False
if (item.status.level != status.level):
change_parent = True
if (change_parent):
if (item.status.level == DiagnosticStatus.OK):
self._ok_node.removeChild(item.tree_node)
elif (item.status.level == DiagnosticStatus.WARN):
self._warning_node.removeChild(item.tree_node)
elif (item.status.level == -1) or (item.status.level == DiagnosticStatus.STALE):
self._stale_node.removeChild(item.tree_node)
else: # ERROR
self._error_node.removeChild(item.tree_node)
if (status.level == DiagnosticStatus.OK):
parent_node = self._ok_node
elif (status.level == DiagnosticStatus.WARN):
parent_node = self._warning_node
elif (status.level == -1) or (status.level == DiagnosticStatus.STALE):
parent_node = self._stale_node
else: # ERROR
parent_node = self._error_node
item.tree_node.setText(0, status.name + ": " + status.message)
item.tree_node.setData(0, Qt.UserRole, item)
parent_node.addChild(item.tree_node)
# expand errors automatically
if (status.level > 1 or status.level == -1):
parent_node.setExpanded(True)
parent_node.sortChildren(0, Qt.AscendingOrder)
if (was_selected):
self.tree_widget.setCurrentItem(item.tree_node)
else:
item.tree_node.setText(0, status.name + ": " + status.message)
item.status = status
if (was_selected):
self._fillout_info(item.tree_node)
item.mark = True
def _create_item(self, status, select, expand_if_error):
if (status.level == DiagnosticStatus.OK):
parent_node = self._ok_node
elif (status.level == DiagnosticStatus.WARN):
parent_node = self._warning_node
elif (status.level == -1) or (status.level == DiagnosticStatus.STALE):
parent_node = self._stale_node
else: # ERROR
parent_node = self._error_node
item = TreeItem(status, QTreeWidgetItem(parent_node, [status.name + ": " + status.message]))
item.tree_node.setData(0, Qt.UserRole, item)
parent_node.addChild(item.tree_node)
self._name_to_item[status.name] = item
parent_node.sortChildren(0, Qt.AscendingOrder)
if (select):
item.tree_node.setSelected(True)
if (expand_if_error and (status.level > 1 or status.level == -1)):
parent_node.setExpanded(True)
item.mark = True
return item
def _fillout_info(self, node):
item = node.data(0, Qt.UserRole)
if not item:
return
scroll_value = self.html_browser.verticalScrollBar().value()
status = item.status
s = cStringIO.StringIO()
s.write("<html><body>")
s.write("<b>Component</b>: %s<br>\n" % (status.name))
s.write("<b>Message</b>: %s<br>\n" % (status.message))
s.write("<b>Hardware ID</b>: %s<br><br>\n\n" % (status.hardware_id))
s.write('<table border="1" cellpadding="2" cellspacing="0">')
for value in status.values:
value.value = value.value.replace("\n", "<br>")
s.write("<tr><td><b>%s</b></td> <td>%s</td></tr>\n" % (value.key, value.value))
s.write("</table></body></html>")
self.html_browser.setHtml(s.getvalue())
if self.html_browser.verticalScrollBar().maximum() < scroll_value:
scroll_value = self.html_browser.verticalScrollBar().maximum()
self.html_browser.verticalScrollBar().setValue(scroll_value)
def _refresh_selection(self):
current_item = self.tree_widget.selectedItems()
if current_item:
self._fillout_info(current_item[0])
def _on_key_press(self, event):
key = event.key()
if key == Qt.Key_Delete:
nodes = self.tree_widget.selectedItems()
if (nodes != [] and nodes[0] not in (self._ok_node, self._warning_node, self._stale_node, self._error_node)):
item = nodes[0].data(0, Qt.UserRole)
if (item.status.level == 0):
self._ok_node.removeChild(item.tree_node)
elif (item.status.level == 1):
self._warning_node.removeChild(item.tree_node)
elif (item.status.level == -1) or (item.status.level == DiagnosticStatus.STALE):
self._stale_node.removeChild(item.tree_node)
else:
self._error_node.removeChild(item.tree_node)
del self._name_to_item[item.status.name]
self._update_root_labels()
self.update()
event.accept()
else:
event.ignore()
def _on_timer(self):
if self._previous_ros_time + rospy.Duration(5) > rospy.Time.now():
return
self._previous_ros_time = rospy.Time.now()
for name, item in self._name_to_item.iteritems():
node = item.tree_node
if (item != None):
if (not item.mark):
was_selected = False
selected = self.tree_widget.selectedItems()
if selected != [] and selected[0] == node:
was_selected = True
new_status = copy.deepcopy(item.status)
new_status.level = -1 # mark stale
self._update_item(item, new_status, was_selected)
item.mark = False
self._update_root_labels()
self.update()
def set_new_errors_callback(self, callback):
self._new_errors_callback = callback
def _update_root_labels(self):
self._stale_node.setText(0, "Stale (%s)" % (self._stale_node.childCount()))
self._error_node.setText(0, "Errors (%s)" % (self._error_node.childCount()))
self._warning_node.setText(0, "Warnings (%s)" % (self._warning_node.childCount()))
self._ok_node.setText(0, "Ok (%s)" % (self._ok_node.childCount()))
class RuntimeMonitorWidget(QWidget):
def __init__(self, topic="/diagnostics"):
super(RuntimeMonitorWidget, self).__init__()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_runtime_monitor'), 'resource', 'runtime_monitor_widget.ui')
loadUi(ui_file, self)
self.setObjectName('RuntimeMonitorWidget')
self._mutex = threading.Lock()
self._error_icon = QIcon.fromTheme('dialog-error')
self._warning_icon = QIcon.fromTheme('dialog-warning')
self._ok_icon = QIcon.fromTheme('dialog-information')
self._stale_node = QTreeWidgetItem(self.tree_widget.invisibleRootItem(), ['Stale (0)'])
self._stale_node.setIcon(0, self._error_icon)
self.tree_widget.addTopLevelItem(self._stale_node)
self._error_node = QTreeWidgetItem(self.tree_widget.invisibleRootItem(), ['Errors (0)'])
self._error_node.setIcon(0, self._error_icon)
self.tree_widget.addTopLevelItem(self._error_node)
self._warning_node = QTreeWidgetItem(self.tree_widget.invisibleRootItem(), ['Warnings (0)'])
self._warning_node.setIcon(0, self._warning_icon)
self.tree_widget.addTopLevelItem(self._warning_node)
self._ok_node = QTreeWidgetItem(self.tree_widget.invisibleRootItem(), ['Ok (0)'])
self._ok_node.setIcon(0, self._ok_icon)
self.tree_widget.addTopLevelItem(self._ok_node)
self.tree_widget.itemSelectionChanged.connect(self._refresh_selection)
self.keyPressEvent = self._on_key_press
self._name_to_item = {}
self._new_errors_callback = None
self._subscriber = rospy.Subscriber(topic, DiagnosticArray, self._diagnostics_callback)
self._timer = QTimer()
self._timer.timeout.connect(self._on_timer)
self._timer.start(5000)
self._msg_timer = QTimer()
self._msg_timer.timeout.connect(self._update_messages)
self._msg_timer.start(100)
self._messages = []
self._used_items = 0
def __del__(self):
self.shutdown()
def shutdown(self):
"""
Unregisters subscriber and stops timers
"""
self._msg_timer.stop()
self._timer.stop()
if rospy.is_shutdown():
return
if self._subscriber:
self._subscriber.unregister()
self._subscriber = None
def change_diagnostic_topic(self, topic):
"""
Changes diagnostics topic name. Must be of type diagnostic_msgs/DiagnosticArray
"""
if not topic:
self.reset_monitor()
return
if self._subscriber:
self._subscriber.unregister()
self._subscriber = rospy.Subscriber(str(topic), DiagnosticArray, self._diagnostics_callback)
self.reset_monitor()
def reset_monitor(self):
"""
Removes all values from monitor display, resets buffers
"""
self._name_to_item = {} # Reset all stale topics
self._messages = []
self._clear_tree()
def _clear_tree(self):
for index in range(self._stale_node.childCount()):
self._stale_node.removeChild(self._stale_node.child(index))
for index in range(self._error_node.childCount()):
self._error_node.removeChild(self._error_node.child(index))
for index in range(self._warning_node.childCount()):
self._warning_node.removeChild(self._warning_node.child(index))
for index in range(self._ok_node.childCount()):
self._ok_node.removeChild(self._ok_node.child(index))
self._update_root_labels()
# Diagnostics callbacks (subscriber thread)
def _diagnostics_callback(self, message):
with self._mutex:
self._messages.append(message)
# Update display of messages from main thread
def _update_messages(self):
with self._mutex:
messages = self._messages[:]
self._messages = []
had_errors = False
for message in messages:
for status in message.status:
was_selected = False
if (self._name_to_item.has_key(status.name)):
item = self._name_to_item[status.name]
if self.tree_widget.isItemSelected(item.tree_node):
was_selected = True
if (item.status.level == DiagnosticStatus.ERROR and status.level != DiagnosticStatus.ERROR):
had_errors = True
self._update_item(item, status, was_selected)
else:
self._create_item(status, was_selected, True)
if (status.level == DiagnosticStatus.ERROR):
had_errors = True
if (had_errors and self._new_errors_callback != None):
self._new_errors_callback()
self._update_root_labels()
self.update()
self._refresh_selection()
def _update_item(self, item, status, was_selected):
change_parent = False
if (item.status.level != status.level):
change_parent = True
if (change_parent):
if (item.status.level == DiagnosticStatus.OK):
self._ok_node.removeChild(item.tree_node)
elif (item.status.level == DiagnosticStatus.WARN):
self._warning_node.removeChild(item.tree_node)
elif (item.status.level == -1):
self._stale_node.removeChild(item.tree_node)
else: # ERROR
self._error_node.removeChild(item.tree_node)
if (status.level == DiagnosticStatus.OK):
parent_node = self._ok_node
elif (status.level == DiagnosticStatus.WARN):
parent_node = self._warning_node
elif (status.level == -1):
parent_node = self._stale_node
else: # ERROR
parent_node = self._error_node
item.tree_node.setText(0, status.name + ": " + status.message)
item.tree_node.setData(0, Qt.UserRole, item)
parent_node.addChild(item.tree_node)
# expand errors automatically
if (status.level > 1 or status.level == -1):
parent_node.setExpanded(True)
parent_node.sortChildren(0, Qt.AscendingOrder)
if (was_selected):
self.tree_widget.setCurrentItem(item.tree_node)
else:
item.tree_node.setText(0, status.name + ": " + status.message)
item.status = status
if (was_selected):
self._fillout_info(item.tree_node)
item.mark = True
def _create_item(self, status, select, expand_if_error):
if (status.level == DiagnosticStatus.OK):
parent_node = self._ok_node
elif (status.level == DiagnosticStatus.WARN):
parent_node = self._warning_node
elif (status.level == -1):
parent_node = self._stale_node
else: # ERROR
parent_node = self._error_node
item = TreeItem(status, QTreeWidgetItem(parent_node, [status.name + ": " + status.message]))
item.tree_node.setData(0, Qt.UserRole, item)
parent_node.addChild(item.tree_node)
self._name_to_item[status.name] = item
parent_node.sortChildren(0, Qt.AscendingOrder)
if (select):
item.tree_node.setSelected(True)
if (expand_if_error and (status.level > 1 or status.level == -1)):
parent_node.setExpanded(True)
item.mark = True
return item
def _fillout_info(self, node):
item = node.data(0, Qt.UserRole)
if not item:
return
scroll_value = self.html_browser.verticalScrollBar().value()
status = item.status
s = cStringIO.StringIO()
s.write("<html><body>")
s.write("<b>Component</b>: %s<br>\n" % (status.name))
s.write("<b>Message</b>: %s<br>\n" % (status.message))
s.write("<b>Hardware ID</b>: %s<br><br>\n\n" % (status.hardware_id))
s.write('<table border="1" cellpadding="2" cellspacing="0">')
for value in status.values:
value.value = value.value.replace("\n", "<br>")
s.write("<tr><td><b>%s</b></td> <td>%s</td></tr>\n" % (value.key, value.value))
s.write("</table></body></html>")
self.html_browser.setHtml(s.getvalue())
if self.html_browser.verticalScrollBar().maximum() < scroll_value:
scroll_value = self.html_browser.verticalScrollBar().maximum()
self.html_browser.verticalScrollBar().setValue(scroll_value)
def _refresh_selection(self):
current_item = self.tree_widget.selectedItems()
if current_item:
self._fillout_info(current_item[0])
def _on_key_press(self, event):
key = event.key()
if key == Qt.Key_Delete:
nodes = self.tree_widget.selectedItems()
if (nodes != [] and nodes[0] not in (self._ok_node, self._warning_node, self._stale_node, self._error_node)):
item = nodes[0].data(0, Qt.UserRole)
if (item.status.level == 0):
self._ok_node.removeChild(item.tree_node)
elif (item.status.level == 1):
self._warning_node.removeChild(item.tree_node)
elif (item.status.level == -1):
self._stale_node.removeChild(item.tree_node)
else:
self._error_node.removeChild(item.tree_node)
del self._name_to_item[item.status.name]
self._update_root_labels()
self.update()
event.accept()
else:
event.ignore()
def _on_timer(self):
for name, item in self._name_to_item.iteritems():
node = item.tree_node
if (item != None):
if (not item.mark):
was_selected = False
selected = self.tree_widget.selectedItems()
if selected != [] and selected[0] == node:
was_selected = True
new_status = copy.deepcopy(item.status)
new_status.level = -1 # mark stale
self._update_item(item, new_status, was_selected)
item.mark = False
self._update_root_labels()
self.update()
def set_new_errors_callback(self, callback):
self._new_errors_callback = callback
def get_num_errors(self):
return self._error_node.childCount() + self._stale_node.childCount()
def get_num_warnings(self):
return self._warning_node.childCount()
def get_num_ok(self):
return self._ok_node.childCount()
def _update_root_labels(self):
self._stale_node.setText(0, "Stale (%s)" % (self._stale_node.childCount()))
self._error_node.setText(0, "Errors (%s)" % (self._error_node.childCount()))
self._warning_node.setText(0, "Warnings (%s)" % (self._warning_node.childCount()))
self._ok_node.setText(0, "Ok (%s)" % (self._ok_node.childCount()))
class ChecklistWindow(QWidget):
def __init__(self, aircraft_id):
super(ChecklistWindow, self).__init__()
# Set properties of the window
self.setWindowTitle("BTO and BPO Checklist")
self.resize(500, 700)
self.move(200,100)
self.checklist_state = 0
# Relative path for the default BPO and BTO checklist
BPO_checklist_file = os.path.join(rospkg.RosPack().get_path('yonah_rqt'), 'src/yonah_rqt', 'BPO_checklist.csv')
BTO_checklist_file = os.path.join(rospkg.RosPack().get_path('yonah_rqt'), 'src/yonah_rqt', 'BTO_checklist.csv')
# Check whether checklist is present, if not print a error message to terminal
try:
# If checklist is present, parse it and pass it to its respective variable
self.BPO_checklist = self.excel_parser(BPO_checklist_file)
self.BTO_checklist = self.excel_parser(BTO_checklist_file)
except:
rospy.logerr("Checklist files are missing or named wrongly. Please follow the original directory and naming")
exit()
# Create the layout
self.main_layout = QVBoxLayout()
self.buttons_layout = QHBoxLayout()
self.tree_widget_layout = QHBoxLayout()
# Create the widgets
self.create_widget()
self.has_message_opened = 0
# Add the widgets into the layouts
self.main_layout.addLayout(self.tree_widget_layout)
self.main_layout.addLayout(self.buttons_layout)
self.setLayout(self.main_layout)
# Create the main layout of widget
def create_widget(self):
# Create tree structure
self.create_tree()
# Declare buttons and connect each of them to a function
self.load_button = QPushButton('Load')
self.ok_button = QPushButton('OK')
self.cancel_button = QPushButton('Cancel')
self.load_button.pressed.connect(self.load_clicked)
self.ok_button.pressed.connect(self.ok_clicked)
self.cancel_button.pressed.connect(self.cancel_clicked)
# Add buttons into the layout
self.buttons_layout.addWidget(self.load_button)
self.buttons_layout.addWidget(self.cancel_button)
self.buttons_layout.addWidget(self.ok_button)
self.tree_widget_layout.addWidget(self.tree_widget)
# Create the tree layout of widget inside of the main layout
def create_tree(self):
# Set up the main tree widget
self.tree_widget = QTreeWidget()
self.tree_widget.setColumnCount(2)
self.tree_widget.setColumnWidth(0, 250)
self.tree_widget.setHeaderLabels(['Parts', 'Status'])
self.item = QTreeWidgetItem()
# Create the BPO section
self.BPO_header = QTreeWidgetItem(self.tree_widget)
self.BPO_header.setFlags(self.BPO_header.flags() | Qt.ItemIsTristate | Qt.ItemIsUserCheckable)
self.BPO_header.setText(0, 'BPO Checklist')
self.BPO_header.setExpanded(True)
self.create_item(self.BPO_header, self.BPO_checklist) # Adds the list of items into the section
# Create the BTO section
self.BTO_header = QTreeWidgetItem(self.tree_widget)
self.BTO_header.setFlags(self.BTO_header.flags() | Qt.ItemIsTristate | Qt.ItemIsUserCheckable)
self.BTO_header.setText(0, 'BTO Checklist')
self.BTO_header.setExpanded(True)
self.create_item(self.BTO_header, self.BTO_checklist) # Adds the list of items into the section
# Populate the tree layout with items
def create_item(self, parent, list):
section_header = [] # List of the section headers
for i in range (len(list)):
if (list[i][1] == '' and list[i][0] != ''):
section_header.append(list[i][0])
k = 0
# Iterate through the different sections
for j in range (len(section_header)):
# Child refers to the sections (mechanical, avionics, etc)
child = QTreeWidgetItem(parent)
child.setFlags(child.flags() | Qt.ItemIsTristate | Qt.ItemIsUserCheckable)
child.setText(0, section_header[j])
while k < len(list):
if list[k][0] in section_header:
# When the while loop encounters the first title, continue the loop
if (list[k][0] == section_header[0]):
k += 1
continue
# When the while loop encounters the next titles, break the loop so that the value of j increases
k += 1
break
# when the list contains empty cells, skip the cell and continue
elif list[k][0] == '':
k += 1
continue
# Add the list items to the treewidgetitem
# Grandchild refers to the items under each section (wing nuts, tail nuts, etc)
grandchild = QTreeWidgetItem(child)
grandchild.setText(0, list[k][0])
grandchild.setText(1, list[k][1])
grandchild.setCheckState(0, Qt.Unchecked) # Set all checkbox to its unchecked state
k += 1
# Read the excel sheet and parse it as an array
def excel_parser(self, file_name):
with open(file_name, 'r') as file:
checklist = []
reader = csv.reader(file)
for row in reader:
checklist.append(row)
return checklist
# Determines what happens when load button is clicked
def load_clicked(self):
# Use QFileDialog to open the system's file browser
filenames = QFileDialog.getOpenFileNames(
self, self.tr('Load from Files'), '.', self.tr('csv files {.csv} (*.csv)'))
# Iterate through the file names selected
for filename in filenames[0]:
# If the file names has the word BPO or BTO in it, remove current widget, add the loaded one
if (filename.find('BPO') != -1):
self.BPO_checklist = self.excel_parser(filename)
self.remove_widget()
self.create_widget()
elif (filename.find('BTO') != -1):
self.BTO_checklist = self.excel_parser(filename)
self.remove_widget()
self.create_widget()
else:
rospy.logerr('rqt: Checklist name must contain BPO or BTO')
self.close()
# Close all the main_layout
def remove_widget(self):
self.main_layout.removeWidget(self.tree_widget)
self.tree_widget.deleteLater()
self.buttons_layout.removeWidget(self.ok_button)
self.buttons_layout.removeWidget(self.cancel_button)
self.buttons_layout.removeWidget(self.load_button)
self.ok_button.deleteLater()
self.cancel_button.deleteLater()
self.load_button.deleteLater()
# Declare what will happen when ok button is clicked
def ok_clicked(self):
if self.BPO_header.checkState(0) != 2 or self.BTO_header.checkState(0) != 2: # User clicks ok without checking all
self.dialog_window("Some items in the checklist are still unchecked", "Do you still want to continue?", True)
else:
self.checklist_state = 1
self.close()
# Declare what will happen when cancel button is clicked
def cancel_clicked(self):
if self.BPO_header.checkState(0) != 0 or self.BTO_header.checkState(0) != 0: # User clicks cancel with some boxes checked
self.dialog_window('Some of your items are checked. Cancelling will uncheck all your items', 'Do you still want to continue?', False)
else:
self.BTO_header.setCheckState(0, Qt.Unchecked)
self.BPO_header.setCheckState(0, Qt.Unchecked)
self.close()
# Create a pop up window when user pre-emptively cancelled or clicked ok without completing the checklist
def dialog_window(self, message, detail, check):
self.message = QMessageBox()
self.has_message_opened = 1
self.message.setIcon(QMessageBox.Warning)
self.message.setText(message)
self.message.setInformativeText(detail)
self.message.setWindowTitle("Items are unchecked")
self.message.setStandardButtons(QMessageBox.Yes | QMessageBox.No)
self.message.show()
if check == True: # Check == True means it is from ok_clicked
self.message.buttonClicked.connect(self.message_action)
else:
self.message.buttonClicked.connect(self.message_action_uncheck)
# Determines what happens after dialog_window pops up from ok_button
def message_action(self, i):
if i.text() == '&Yes':
self.checklist_state = 1
self.close()
else:
self.message.close()
# Determines what happens after dialog_window pops up from cancel_button
def message_action_uncheck(self, i):
self.response = i.text()
if self.response == '&Yes':
self.checklist_state = 1
self.BTO_header.setCheckState(0, Qt.Unchecked)
self.BPO_header.setCheckState(0, Qt.Unchecked)
self.close()
else:
self.message.close()
# Shutdown function
def shutdown(self):
self.close()
if self.has_message_opened == 1:
self.message.close()
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 create_item(self, parent, list):
section_header = [] # List of the section headers
for i in range (len(list)):
if (list[i][1] == '' and list[i][0] != ''):
section_header.append(list[i][0])
k = 0
# Iterate through the different sections
for j in range (len(section_header)):
# Child refers to the sections (mechanical, avionics, etc)
child = QTreeWidgetItem(parent)
child.setFlags(child.flags() | Qt.ItemIsTristate | Qt.ItemIsUserCheckable)
child.setText(0, section_header[j])
while k < len(list):
if list[k][0] in section_header:
# When the while loop encounters the first title, continue the loop
if (list[k][0] == section_header[0]):
k += 1
continue
# When the while loop encounters the next titles, break the loop so that the value of j increases
k += 1
break
# when the list contains empty cells, skip the cell and continue
elif list[k][0] == '':
k += 1
continue
# Add the list items to the treewidgetitem
# Grandchild refers to the items under each section (wing nuts, tail nuts, etc)
grandchild = QTreeWidgetItem(child)
grandchild.setText(0, list[k][0])
grandchild.setText(1, list[k][1])
grandchild.setCheckState(0, Qt.Unchecked) # Set all checkbox to its unchecked state
k += 1
