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 _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 _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("")
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)
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()
