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 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
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 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 __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 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)
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, 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,
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_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 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):
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):
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 _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 _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 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 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 _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 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 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 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 _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 __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.")
