def add_widget_with_frame(parent, widget, text = ""): box_layout = QHBoxLayout() box_layout.addWidget(widget) group_box = QGroupBox() group_box.setStyleSheet("QGroupBox { border: 1px solid gray; border-radius: 4px; margin-top: 0.5em; } QGroupBox::title { subcontrol-origin: margin; left: 10px; padding: 0 3px 0 3px; }") group_box.setTitle(text) group_box.setLayout(box_layout) parent.addWidget(group_box)
def add_widget_with_frame(parent, widget, text=""): box_layout = QHBoxLayout() box_layout.addWidget(widget) group_box = QGroupBox() group_box.setStyleSheet( "QGroupBox { border: 1px solid gray; border-radius: 4px; margin-top: 0.5em; } QGroupBox::title { subcontrol-origin: margin; left: 10px; padding: 0 3px 0 3px; }" ) group_box.setTitle(text) group_box.setLayout(box_layout) parent.addWidget(group_box)
class PbDGUI(Plugin): exp_state_sig = Signal(ExperimentState) def __init__(self, context): super(PbDGUI, self).__init__(context) self.setObjectName('PbDGUI') self._widget = QWidget() self.speech_cmd_publisher = rospy.Publisher('recognized_command', Command) self.gui_cmd_publisher = rospy.Publisher('gui_command', GuiCommand) rospy.Subscriber('experiment_state', ExperimentState, self.exp_state_cb) rospy.Subscriber('robotsound', SoundRequest, self.robotSoundReceived) QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10)) self.exp_state_sig.connect(self.update_state) self.commands = dict() self.commands[Command.TEST_MICROPHONE] = 'Test microphone' self.commands[Command.NEW_DEMONSTRATION] = 'New demonstration' self.commands[Command.START_RECORDING] = 'Start recording' self.commands[Command.STOP_RECORDING] = 'Stop recording' self.commands[Command.REPLAY_DEMONSTRATION] = 'Replay demonstration' self.commands[Command.DETECT_SURFACE] = 'Detect surface' self.commands[Command.TAKE_TOOL] = 'Take tool' self.commands[Command.RELEASE_TOOL] = 'Release tool' self.commands[Command.SAVE_ARM_POSE] = 'Save arm pose' self.currentAction = -1 self.currentStep = -1 allWidgetsBox = QtGui.QVBoxLayout() actionBox = QGroupBox('Demonstrations', self._widget) self.actionGrid = QtGui.QGridLayout() self.actionGrid.setHorizontalSpacing(0) for i in range(6): self.actionGrid.addItem(QtGui.QSpacerItem(90, 90), 0, i, QtCore.Qt.AlignCenter) self.actionGrid.setColumnStretch(i, 0) self.actionIcons = dict() actionBoxLayout = QtGui.QHBoxLayout() actionBoxLayout.addLayout(self.actionGrid) actionBox.setLayout(actionBoxLayout) actionButtonGrid = QtGui.QHBoxLayout() actionButtonGrid.addWidget( self.create_button(Command.NEW_DEMONSTRATION)) self.stepsBox = QGroupBox('No demonstrations', self._widget) self.stepsGrid = QtGui.QGridLayout() self.l_model = QtGui.QStandardItemModel(self) self.l_view = self._create_table_view(self.l_model, self.l_row_clicked_cb) self.r_model = QtGui.QStandardItemModel(self) self.r_view = self._create_table_view(self.r_model, self.r_row_clicked_cb) self.stepsGrid.addItem(QtGui.QSpacerItem(280, 10), 0, 0, 2, 3) self.stepsGrid.addItem(QtGui.QSpacerItem(10, 10), 0, 1, 2, 3) self.stepsGrid.addItem(QtGui.QSpacerItem(280, 10), 0, 2, 2, 3) self.stepsGrid.addWidget(QtGui.QLabel('Left Arm'), 0, 0) self.stepsGrid.addWidget(QtGui.QLabel('Right Arm'), 0, 2) self.stepsGrid.addWidget(self.l_view, 1, 0) self.stepsGrid.addWidget(self.r_view, 1, 2) stepsBoxLayout = QtGui.QHBoxLayout() stepsBoxLayout.addLayout(self.stepsGrid) self.stepsBox.setLayout(stepsBoxLayout) stepsButtonGrid = QtGui.QHBoxLayout() stepsButtonGrid.addWidget( self.create_button(Command.REPLAY_DEMONSTRATION)) motionButtonGrid = QtGui.QHBoxLayout() motionButtonGrid.addWidget(self.create_button(Command.START_RECORDING)) motionButtonGrid.addWidget(self.create_button(Command.STOP_RECORDING)) misc_grid = QtGui.QHBoxLayout() misc_grid.addWidget(self.create_button(Command.TEST_MICROPHONE)) misc_grid.addWidget(self.create_button(Command.DETECT_SURFACE)) misc_grid.addStretch(1) misc_grid2 = QtGui.QHBoxLayout() misc_grid2.addWidget(self.create_button(Command.TAKE_TOOL)) misc_grid2.addWidget(self.create_button(Command.RELEASE_TOOL)) misc_grid2.addStretch(1) misc_grid3 = QtGui.QHBoxLayout() misc_grid3.addWidget(self.create_button(Command.SAVE_ARM_POSE)) misc_grid3.addStretch(1) speechGroupBox = QGroupBox('Robot Speech', self._widget) speechGroupBox.setObjectName('RobotSpeechGroup') speechBox = QtGui.QHBoxLayout() self.speechLabel = QtGui.QLabel('Robot has not spoken yet') palette = QtGui.QPalette() palette.setColor(QtGui.QPalette.Foreground, QtCore.Qt.blue) self.speechLabel.setPalette(palette) speechBox.addWidget(self.speechLabel) speechGroupBox.setLayout(speechBox) allWidgetsBox.addWidget(actionBox) allWidgetsBox.addLayout(actionButtonGrid) allWidgetsBox.addWidget(self.stepsBox) allWidgetsBox.addLayout(motionButtonGrid) allWidgetsBox.addLayout(stepsButtonGrid) allWidgetsBox.addItem(QtGui.QSpacerItem(100, 20)) allWidgetsBox.addLayout(misc_grid) allWidgetsBox.addItem(QtGui.QSpacerItem(100, 20)) allWidgetsBox.addLayout(misc_grid2) allWidgetsBox.addItem(QtGui.QSpacerItem(100, 40)) allWidgetsBox.addLayout(misc_grid3) allWidgetsBox.addItem(QtGui.QSpacerItem(100, 20)) allWidgetsBox.addWidget(speechGroupBox) allWidgetsBox.addStretch(1) # Fix layout and add main widget to the user interface QtGui.QApplication.setStyle(QtGui.QStyleFactory.create('plastique')) vAllBox = QtGui.QVBoxLayout() vAllBox.addLayout(allWidgetsBox) vAllBox.addStretch(1) hAllBox = QtGui.QHBoxLayout() hAllBox.addLayout(vAllBox) hAllBox.addStretch(1) self._widget.setObjectName('PbDGUI') self._widget.setLayout(hAllBox) context.add_widget(self._widget) rospy.loginfo('Will wait for the experiment state service...') rospy.wait_for_service('get_experiment_state') exp_state_srv = rospy.ServiceProxy('get_experiment_state', GetExperimentState) rospy.loginfo('Got response from the experiment state service...') response = exp_state_srv() self.update_state(response.state) def _create_table_view(self, model, row_click_cb): proxy = QtGui.QSortFilterProxyModel(self) proxy.setSourceModel(model) view = QtGui.QTableView(self._widget) verticalHeader = view.verticalHeader() verticalHeader.sectionClicked.connect(row_click_cb) view.setModel(proxy) view.setMaximumWidth(250) view.setSortingEnabled(False) view.setCornerButtonEnabled(False) return view def get_uid(self, arm_index, index): '''Returns a unique id of the marker''' return (2 * (index + 1) + arm_index) def get_arm_and_index(self, uid): '''Returns a unique id of the marker''' arm_index = uid % 2 index = (uid - arm_index) / 2 return (arm_index, (index - 1)) def r_row_clicked_cb(self, logicalIndex): self.step_pressed(self.get_uid(0, logicalIndex)) def l_row_clicked_cb(self, logicalIndex): self.step_pressed(self.get_uid(1, logicalIndex)) def create_button(self, command): btn = QtGui.QPushButton(self.commands[command], self._widget) btn.clicked.connect(self.command_cb) return btn def update_state(self, state): qWarning('Received new state') n_actions = len(self.actionIcons.keys()) if n_actions < state.n_actions: for i in range(n_actions, state.n_actions): self.new_action() if (self.currentAction != (state.i_current_action - 1)): self.delete_all_steps() self.action_pressed(state.i_current_action - 1, False) n_steps = self.n_steps() if (n_steps < state.n_steps): for i in range(n_steps, state.n_steps): self.save_pose(frameType=ord(state.frame_types[i])) elif (n_steps > state.n_steps): n_to_remove = n_steps - state.n_steps self.r_model.invisibleRootItem().removeRows( state.n_steps, n_to_remove) self.l_model.invisibleRootItem().removeRows( state.n_steps, n_to_remove) ## TODO: DEAL with the following arrays!!! state.r_gripper_states state.l_gripper_states state.r_ref_frames state.l_ref_frames state.objects if (self.currentStep != state.i_current_step): if (self.n_steps() > 0): self.currentStep = state.i_current_step arm_index, index = self.get_arm_and_index(self.currentStep) if (arm_index == 0): self.r_view.selectRow(index) else: self.l_view.selectRow(index) def get_frame_type(self, fr_type): if (fr_type > 1): rospy.logwarn( "Invalid frame type @ save_pose -> get_frame_type: " + str(fr_type)) return ["Go to pose", "Maneuver"][fr_type] def save_pose(self, actionIndex=None, frameType=0): nColumns = 9 if actionIndex is None: actionIndex = self.currentAction stepIndex = self.n_steps(actionIndex) r_step = [ QtGui.QStandardItem('Step' + str(stepIndex + 1)), QtGui.QStandardItem(self.get_frame_type(frameType)), QtGui.QStandardItem('Absolute') ] l_step = [ QtGui.QStandardItem('Step' + str(stepIndex + 1)), QtGui.QStandardItem(self.get_frame_type(frameType)), QtGui.QStandardItem('Absolute') ] self.r_model.invisibleRootItem().appendRow(r_step) self.l_model.invisibleRootItem().appendRow(l_step) self.update_table_view() self.currentStep = stepIndex def update_table_view(self): self.l_view.setColumnWidth(0, 50) self.l_view.setColumnWidth(1, 100) self.l_view.setColumnWidth(2, 70) self.r_view.setColumnWidth(0, 50) self.r_view.setColumnWidth(1, 100) self.r_view.setColumnWidth(2, 70) def n_steps(self, actionIndex=None): return self.l_model.invisibleRootItem().rowCount() def delete_all_steps(self, actionIndex=None): if actionIndex is None: actionIndex = self.currentAction n_steps = self.n_steps() if (n_steps > 0): self.l_model.invisibleRootItem().removeRows(0, n_steps) self.r_model.invisibleRootItem().removeRows(0, n_steps) def n_actions(self): return len(self.actionIcons.keys()) def new_action(self): nColumns = 6 actionIndex = self.n_actions() for key in self.actionIcons.keys(): self.actionIcons[key].selected = False self.actionIcons[key].updateView() actIcon = ActionIcon(self._widget, actionIndex, self.action_pressed) self.actionGrid.addLayout(actIcon, int(actionIndex / nColumns), actionIndex % nColumns) self.actionIcons[actionIndex] = actIcon def step_pressed(self, step_index): gui_cmd = GuiCommand(GuiCommand.SELECT_ACTION_STEP, step_index) self.gui_cmd_publisher.publish(gui_cmd) def action_pressed(self, actionIndex, isPublish=True): for i in range(len(self.actionIcons.keys())): key = self.actionIcons.keys()[i] if key == actionIndex: self.actionIcons[key].selected = True else: self.actionIcons[key].selected = False self.actionIcons[key].updateView() self.currentAction = actionIndex self.stepsBox.setTitle('Steps for Action ' + str(self.currentAction + 1)) if isPublish: gui_cmd = GuiCommand(GuiCommand.SWITCH_TO_ACTION, (actionIndex + 1)) self.gui_cmd_publisher.publish(gui_cmd) def command_cb(self): clickedButtonName = self._widget.sender().text() for key in self.commands.keys(): if (self.commands[key] == clickedButtonName): qWarning('Sending speech command: ' + key) command = Command() command.command = key self.speech_cmd_publisher.publish(command) def robotSoundReceived(self, soundReq): if (soundReq.command == SoundRequest.SAY): qWarning('Robot said: ' + soundReq.arg) self.speechLabel.setText('Robot sound: ' + soundReq.arg) def exp_state_cb(self, state): qWarning('Received new experiment state.') self.exp_state_sig.emit(state) def shutdown_plugin(self): # TODO unregister all publishers here self.speech_cmd_publisher.unregister() self.gui_cmd_publisher.unregister() pass def save_settings(self, plugin_settings, instance_settings): # TODO save intrinsic configuration, usually using: # instance_settings.set_value(k, v) pass def restore_settings(self, plugin_settings, instance_settings): # TODO restore intrinsic configuration, usually using: # v = instance_settings.value(k) pass
class PbDGUI(Plugin): exp_state_sig = Signal(ExperimentState) def __init__(self, context): super(PbDGUI, self).__init__(context) self.setObjectName('PbDGUI') self._widget = QWidget() self.speech_cmd_publisher = rospy.Publisher('recognized_command', Command) self.gui_cmd_publisher = rospy.Publisher('gui_command', GuiCommand) rospy.Subscriber('experiment_state', ExperimentState, self.exp_state_cb) rospy.Subscriber('robotsound', SoundRequest, self.robotSoundReceived) QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10)) self.exp_state_sig.connect(self.update_state) self.commands = dict() self.commands[Command.CREATE_NEW_ACTION] = 'New action' self.commands[Command.TEST_MICROPHONE] = 'Test microphone' self.commands[Command.NEXT_ACTION] = 'Next action' self.commands[Command.PREV_ACTION] = 'Previous action' self.commands[Command.SAVE_POSE] = 'Save pose' self.commands[Command.RELAX_RIGHT_ARM] = 'Relax right arm' self.commands[Command.RELAX_LEFT_ARM] = 'Relax left arm' self.commands[Command.FREEZE_RIGHT_ARM] = 'Freeze right arm' self.commands[Command.FREEZE_LEFT_ARM] = 'Freeze left arm' self.commands[Command.OPEN_RIGHT_HAND] = 'Open right hand' self.commands[Command.OPEN_LEFT_HAND] = 'Open left hand' self.commands[Command.CLOSE_RIGHT_HAND] = 'Close right hand' self.commands[Command.CLOSE_LEFT_HAND] = 'Close left hand' self.commands[Command.CLOSE_LEFT_HAND] = 'Close left hand' self.commands[Command.EXECUTE_ACTION] = 'Execute action' self.commands[Command.STOP_EXECUTION] = 'Stop execution' self.commands[Command.DELETE_ALL_STEPS] = 'Delete all' self.commands[Command.DELETE_LAST_STEP] = 'Delete last' self.commands[Command.RECORD_OBJECT_POSE] = 'Record object poses' self.currentAction = -1 self.currentStep = -1 allWidgetsBox = QtGui.QVBoxLayout() actionBox = QGroupBox('Actions', self._widget) self.actionGrid = QtGui.QGridLayout() self.actionGrid.setHorizontalSpacing(0) for i in range(6): self.actionGrid.addItem(QtGui.QSpacerItem(90, 90), 0, i, QtCore.Qt.AlignCenter) self.actionGrid.setColumnStretch(i, 0) self.actionIcons = dict() actionBoxLayout = QtGui.QHBoxLayout() actionBoxLayout.addLayout(self.actionGrid) actionBox.setLayout(actionBoxLayout) actionButtonGrid = QtGui.QHBoxLayout() actionButtonGrid.addWidget( self.create_button(Command.CREATE_NEW_ACTION)) self.stepsBox = QGroupBox('No actions created yet', self._widget) self.stepsGrid = QtGui.QGridLayout() self.l_model = QtGui.QStandardItemModel(self) self.l_view = self._create_table_view(self.l_model, self.l_row_clicked_cb) self.r_model = QtGui.QStandardItemModel(self) self.r_view = self._create_table_view(self.r_model, self.r_row_clicked_cb) self.stepsGrid.addItem(QtGui.QSpacerItem(280, 10), 0, 0, 2, 3) self.stepsGrid.addItem(QtGui.QSpacerItem(10, 10), 0, 1, 2, 3) self.stepsGrid.addItem(QtGui.QSpacerItem(280, 10), 0, 2, 2, 3) self.stepsGrid.addWidget(QtGui.QLabel('Left Arm'), 0, 0) self.stepsGrid.addWidget(QtGui.QLabel('Right Arm'), 0, 2) self.stepsGrid.addWidget(self.l_view, 1, 0) self.stepsGrid.addWidget(self.r_view, 1, 2) stepsBoxLayout = QtGui.QHBoxLayout() stepsBoxLayout.addLayout(self.stepsGrid) self.stepsBox.setLayout(stepsBoxLayout) stepsButtonGrid = QtGui.QHBoxLayout() stepsButtonGrid.addWidget(self.create_button(Command.SAVE_POSE)) stepsButtonGrid.addWidget(self.create_button(Command.EXECUTE_ACTION)) stepsButtonGrid.addWidget(self.create_button(Command.STOP_EXECUTION)) stepsButtonGrid.addWidget(self.create_button(Command.DELETE_ALL_STEPS)) stepsButtonGrid.addWidget(self.create_button(Command.DELETE_LAST_STEP)) misc_grid = QtGui.QHBoxLayout() misc_grid.addWidget(self.create_button(Command.TEST_MICROPHONE)) misc_grid.addWidget(self.create_button(Command.RECORD_OBJECT_POSE)) misc_grid.addStretch(1) misc_grid2 = QtGui.QHBoxLayout() misc_grid2.addWidget(self.create_button(Command.RELAX_RIGHT_ARM)) misc_grid2.addWidget(self.create_button(Command.RELAX_LEFT_ARM)) misc_grid2.addWidget(self.create_button(Command.FREEZE_RIGHT_ARM)) misc_grid2.addWidget(self.create_button(Command.FREEZE_LEFT_ARM)) misc_grid2.addStretch(1) misc_grid3 = QtGui.QHBoxLayout() misc_grid3.addWidget(self.create_button(Command.OPEN_RIGHT_HAND)) misc_grid3.addWidget(self.create_button(Command.OPEN_LEFT_HAND)) misc_grid3.addWidget(self.create_button(Command.CLOSE_RIGHT_HAND)) misc_grid3.addWidget(self.create_button(Command.CLOSE_LEFT_HAND)) misc_grid3.addStretch(1) misc_grid4 = QtGui.QHBoxLayout() misc_grid4.addWidget(self.create_button(Command.PREV_ACTION)) misc_grid4.addWidget(self.create_button(Command.NEXT_ACTION)) misc_grid4.addStretch(1) speechGroupBox = QGroupBox('Robot Speech', self._widget) speechGroupBox.setObjectName('RobotSpeechGroup') speechBox = QtGui.QHBoxLayout() self.speechLabel = QtGui.QLabel('Robot has not spoken yet') palette = QtGui.QPalette() palette.setColor(QtGui.QPalette.Foreground, QtCore.Qt.blue) self.speechLabel.setPalette(palette) speechBox.addWidget(self.speechLabel) speechGroupBox.setLayout(speechBox) allWidgetsBox.addWidget(actionBox) allWidgetsBox.addLayout(actionButtonGrid) allWidgetsBox.addWidget(self.stepsBox) allWidgetsBox.addLayout(stepsButtonGrid) allWidgetsBox.addItem(QtGui.QSpacerItem(100, 20)) allWidgetsBox.addLayout(misc_grid) allWidgetsBox.addItem(QtGui.QSpacerItem(100, 20)) allWidgetsBox.addLayout(misc_grid2) allWidgetsBox.addLayout(misc_grid3) allWidgetsBox.addItem(QtGui.QSpacerItem(100, 20)) allWidgetsBox.addLayout(misc_grid4) allWidgetsBox.addItem(QtGui.QSpacerItem(100, 20)) allWidgetsBox.addWidget(speechGroupBox) allWidgetsBox.addStretch(1) # Fix layout and add main widget to the user interface QtGui.QApplication.setStyle(QtGui.QStyleFactory.create('plastique')) vAllBox = QtGui.QVBoxLayout() vAllBox.addLayout(allWidgetsBox) vAllBox.addStretch(1) hAllBox = QtGui.QHBoxLayout() hAllBox.addLayout(vAllBox) hAllBox.addStretch(1) self._widget.setObjectName('PbDGUI') self._widget.setLayout(hAllBox) context.add_widget(self._widget) rospy.loginfo('Will wait for the experiment state service...') rospy.wait_for_service('get_experiment_state') exp_state_srv = rospy.ServiceProxy('get_experiment_state', GetExperimentState) rospy.loginfo('Got response from the experiment state service...') response = exp_state_srv() self.update_state(response.state) @staticmethod def loginfo(message): '''Because all other ROS logging has some kind of information about what's being emitted, and qWarning doesn't, we're going to wrap the function to provide a little bit of additional info for newbies. Args: message (str): The message to log ''' qWarning('[INFO] [pbd_gui.py] ' + message) def _create_table_view(self, model, row_click_cb): proxy = QtGui.QSortFilterProxyModel(self) proxy.setSourceModel(model) view = QtGui.QTableView(self._widget) verticalHeader = view.verticalHeader() verticalHeader.sectionClicked.connect(row_click_cb) view.setModel(proxy) view.setMaximumWidth(250) view.setSortingEnabled(False) view.setCornerButtonEnabled(False) return view def get_uid(self, arm_index, index): '''Returns a unique id of the marker''' return (2 * (index + 1) + arm_index) def get_arm_and_index(self, uid): '''Returns a unique id of the marker''' arm_index = uid % 2 index = (uid - arm_index) / 2 return (arm_index, (index - 1)) def r_row_clicked_cb(self, logicalIndex): self.step_pressed(self.get_uid(0, logicalIndex)) def l_row_clicked_cb(self, logicalIndex): self.step_pressed(self.get_uid(1, logicalIndex)) def create_button(self, command): btn = QtGui.QPushButton(self.commands[command], self._widget) btn.clicked.connect(self.command_cb) return btn def update_state(self, state): PbDGUI.loginfo('Received new state') n_actions = len(self.actionIcons.keys()) if n_actions < state.n_actions: for i in range(n_actions, state.n_actions): self.new_action() if (self.currentAction != (state.i_current_action - 1)): self.delete_all_steps() self.action_pressed(state.i_current_action - 1, False) n_steps = self.n_steps() if (n_steps < state.n_steps): for i in range(n_steps, state.n_steps): self.save_pose() elif (n_steps > state.n_steps): n_to_remove = n_steps - state.n_steps self.r_model.invisibleRootItem().removeRows( state.n_steps, n_to_remove) self.l_model.invisibleRootItem().removeRows( state.n_steps, n_to_remove) ## TODO: DEAL with the following arrays!!! state.r_gripper_states state.l_gripper_states state.r_ref_frames state.l_ref_frames state.objects if (self.currentStep != state.i_current_step): if (self.n_steps() > 0): self.currentStep = state.i_current_step arm_index, index = self.get_arm_and_index(self.currentStep) if (arm_index == 0): self.r_view.selectRow(index) else: self.l_view.selectRow(index) def save_pose(self, actionIndex=None): nColumns = 9 if actionIndex is None: actionIndex = self.currentAction stepIndex = self.n_steps(actionIndex) r_step = [ QtGui.QStandardItem('Step' + str(stepIndex + 1)), QtGui.QStandardItem('Go to pose'), QtGui.QStandardItem('Absolute') ] l_step = [ QtGui.QStandardItem('Step' + str(stepIndex + 1)), QtGui.QStandardItem('Go to pose'), QtGui.QStandardItem('Absolute') ] self.r_model.invisibleRootItem().appendRow(r_step) self.l_model.invisibleRootItem().appendRow(l_step) self.update_table_view() self.currentStep = stepIndex def update_table_view(self): self.l_view.setColumnWidth(0, 50) self.l_view.setColumnWidth(1, 100) self.l_view.setColumnWidth(2, 70) self.r_view.setColumnWidth(0, 50) self.r_view.setColumnWidth(1, 100) self.r_view.setColumnWidth(2, 70) def n_steps(self, actionIndex=None): return self.l_model.invisibleRootItem().rowCount() def delete_all_steps(self, actionIndex=None): if actionIndex is None: actionIndex = self.currentAction n_steps = self.n_steps() if (n_steps > 0): self.l_model.invisibleRootItem().removeRows(0, n_steps) self.r_model.invisibleRootItem().removeRows(0, n_steps) def n_actions(self): return len(self.actionIcons.keys()) def new_action(self): nColumns = 6 actionIndex = self.n_actions() for key in self.actionIcons.keys(): self.actionIcons[key].selected = False self.actionIcons[key].updateView() actIcon = ActionIcon(self._widget, actionIndex, self.action_pressed) self.actionGrid.addLayout(actIcon, int(actionIndex / nColumns), actionIndex % nColumns) self.actionIcons[actionIndex] = actIcon def step_pressed(self, step_index): gui_cmd = GuiCommand(GuiCommand.SELECT_ACTION_STEP, step_index) self.gui_cmd_publisher.publish(gui_cmd) def action_pressed(self, actionIndex, isPublish=True): for i in range(len(self.actionIcons.keys())): key = self.actionIcons.keys()[i] if key == actionIndex: self.actionIcons[key].selected = True else: self.actionIcons[key].selected = False self.actionIcons[key].updateView() self.currentAction = actionIndex self.stepsBox.setTitle('Steps for Action ' + str(self.currentAction + 1)) if isPublish: gui_cmd = GuiCommand(GuiCommand.SWITCH_TO_ACTION, (actionIndex + 1)) self.gui_cmd_publisher.publish(gui_cmd) def command_cb(self): clickedButtonName = self._widget.sender().text() for key in self.commands.keys(): if (self.commands[key] == clickedButtonName): PbDGUI.loginfo('Sending speech command: ' + key) command = Command() command.command = key self.speech_cmd_publisher.publish(command) def robotSoundReceived(self, soundReq): if (soundReq.command == SoundRequest.SAY): PbDGUI.loginfo('Robot said: ' + soundReq.arg) self.speechLabel.setText('Robot sound: ' + soundReq.arg) def exp_state_cb(self, state): self.exp_state_sig.emit(state) def shutdown_plugin(self): # TODO unregister all publishers here self.speech_cmd_publisher.unregister() self.gui_cmd_publisher.unregister() pass def save_settings(self, plugin_settings, instance_settings): # TODO save intrinsic configuration, usually using: # instance_settings.set_value(k, v) pass def restore_settings(self, plugin_settings, instance_settings): # TODO restore intrinsic configuration, usually using: # v = instance_settings.value(k) pass
class PbDGUI(Plugin): exp_state_sig = Signal(ExperimentState) def __init__(self, context): super(PbDGUI, self).__init__(context) self.setObjectName('PbDGUI') self._widget = QWidget() self.speech_cmd_publisher = rospy.Publisher('recognized_command', Command) self.gui_cmd_publisher = rospy.Publisher('gui_command', GuiCommand) rospy.Subscriber('experiment_state', ExperimentState, self.exp_state_cb) rospy.Subscriber('robotsound', SoundRequest, self.robotSoundReceived) QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10)) self.exp_state_sig.connect(self.update_state) self.commands = dict() self.commands[Command.TEST_MICROPHONE] = 'Test microphone' self.commands[Command.START_RECORDING] = 'Start recording' self.commands[Command.STOP_RECORDING] = 'Stop recording' self.commands[Command.REPLAY_DEMONSTRATION] = 'Replay demonstration' self.commands[Command.TAKE_TOOL] = 'Take tool' self.commands[Command.RELEASE_TOOL] = 'Release tool' self.currentAction = -1 self.currentStep = -1 allWidgetsBox = QtGui.QVBoxLayout() actionBox = QGroupBox('Demonstrations', self._widget) self.actionGrid = QtGui.QGridLayout() self.actionGrid.setHorizontalSpacing(0) for i in range(6): self.actionGrid.addItem(QtGui.QSpacerItem(90, 90), 0, i, QtCore.Qt.AlignCenter) self.actionGrid.setColumnStretch(i, 0) self.actionIcons = dict() actionBoxLayout = QtGui.QHBoxLayout() actionBoxLayout.addLayout(self.actionGrid) actionBox.setLayout(actionBoxLayout) actionButtonGrid = QtGui.QHBoxLayout() self.stepsBox = QGroupBox('No demonstrations', self._widget) self.stepsGrid = QtGui.QGridLayout() self.l_model = QtGui.QStandardItemModel(self) self.l_view = self._create_table_view(self.l_model, self.l_row_clicked_cb) self.r_model = QtGui.QStandardItemModel(self) self.r_view = self._create_table_view(self.r_model, self.r_row_clicked_cb) self.stepsGrid.addItem(QtGui.QSpacerItem(280, 10), 0, 0, 2, 3) self.stepsGrid.addItem(QtGui.QSpacerItem(10, 10), 0, 1, 2, 3) self.stepsGrid.addItem(QtGui.QSpacerItem(280, 10), 0, 2, 2, 3) self.stepsGrid.addWidget(QtGui.QLabel('Left Arm'), 0, 0) self.stepsGrid.addWidget(QtGui.QLabel('Right Arm'), 0, 2) self.stepsGrid.addWidget(self.l_view, 1, 0) self.stepsGrid.addWidget(self.r_view, 1, 2) stepsBoxLayout = QtGui.QHBoxLayout() stepsBoxLayout.addLayout(self.stepsGrid) self.stepsBox.setLayout(stepsBoxLayout) stepsButtonGrid = QtGui.QHBoxLayout() stepsButtonGrid.addWidget(self.create_button(Command.REPLAY_DEMONSTRATION)) motionButtonGrid = QtGui.QHBoxLayout() motionButtonGrid.addWidget(self.create_button(Command.START_RECORDING)) motionButtonGrid.addWidget(self.create_button(Command.STOP_RECORDING)) misc_grid = QtGui.QHBoxLayout() misc_grid.addWidget(self.create_button(Command.TEST_MICROPHONE)) misc_grid.addStretch(1) misc_grid2 = QtGui.QHBoxLayout() misc_grid2.addWidget(self.create_button(Command.TAKE_TOOL)) misc_grid2.addWidget(self.create_button(Command.RELEASE_TOOL)) misc_grid2.addStretch(1) speechGroupBox = QGroupBox('Robot Speech', self._widget) speechGroupBox.setObjectName('RobotSpeechGroup') speechBox = QtGui.QHBoxLayout() self.speechLabel = QtGui.QLabel('Robot has not spoken yet') palette = QtGui.QPalette() palette.setColor(QtGui.QPalette.Foreground,QtCore.Qt.blue) self.speechLabel.setPalette(palette) speechBox.addWidget(self.speechLabel) speechGroupBox.setLayout(speechBox) allWidgetsBox.addWidget(actionBox) allWidgetsBox.addLayout(actionButtonGrid) allWidgetsBox.addWidget(self.stepsBox) allWidgetsBox.addLayout(motionButtonGrid) allWidgetsBox.addLayout(stepsButtonGrid) allWidgetsBox.addItem(QtGui.QSpacerItem(100, 20)) allWidgetsBox.addLayout(misc_grid) allWidgetsBox.addItem(QtGui.QSpacerItem(100, 20)) allWidgetsBox.addLayout(misc_grid2) allWidgetsBox.addItem(QtGui.QSpacerItem(100, 40)) allWidgetsBox.addWidget(speechGroupBox) allWidgetsBox.addStretch(1) # Fix layout and add main widget to the user interface QtGui.QApplication.setStyle(QtGui.QStyleFactory.create('plastique')) vAllBox = QtGui.QVBoxLayout() vAllBox.addLayout(allWidgetsBox) vAllBox.addStretch(1) hAllBox = QtGui.QHBoxLayout() hAllBox.addLayout(vAllBox) hAllBox.addStretch(1) self._widget.setObjectName('PbDGUI') self._widget.setLayout(hAllBox) context.add_widget(self._widget) rospy.loginfo('Will wait for the experiment state service...') rospy.wait_for_service('get_experiment_state') exp_state_srv = rospy.ServiceProxy('get_experiment_state', GetExperimentState) rospy.loginfo('Got response from the experiment state service...') response = exp_state_srv() self.update_state(response.state) def _create_table_view(self, model, row_click_cb): proxy = QtGui.QSortFilterProxyModel(self) proxy.setSourceModel(model) view = QtGui.QTableView(self._widget) verticalHeader = view.verticalHeader() verticalHeader.sectionClicked.connect(row_click_cb) view.setModel(proxy) view.setMaximumWidth(250) view.setSortingEnabled(False) view.setCornerButtonEnabled(False) return view def get_uid(self, arm_index, index): '''Returns a unique id of the marker''' return (2 * (index + 1) + arm_index) def get_arm_and_index(self, uid): '''Returns a unique id of the marker''' arm_index = uid % 2 index = (uid - arm_index) / 2 return (arm_index, (index - 1)) def r_row_clicked_cb(self, logicalIndex): self.step_pressed(self.get_uid(0, logicalIndex)) def l_row_clicked_cb(self, logicalIndex): self.step_pressed(self.get_uid(1, logicalIndex)) def create_button(self, command): btn = QtGui.QPushButton(self.commands[command], self._widget) btn.clicked.connect(self.command_cb) return btn def update_state(self, state): qWarning('Received new state') n_actions = len(self.actionIcons.keys()) if n_actions < state.n_actions: for i in range(n_actions, state.n_actions): self.new_action() if (self.currentAction != (state.i_current_action - 1)): self.delete_all_steps() self.action_pressed(state.i_current_action - 1, False) n_steps = self.n_steps() if (n_steps < state.n_steps): for i in range(n_steps, state.n_steps): self.save_pose(frameType=ord(state.frame_types[i])) elif (n_steps > state.n_steps): n_to_remove = n_steps - state.n_steps self.r_model.invisibleRootItem().removeRows(state.n_steps, n_to_remove) self.l_model.invisibleRootItem().removeRows(state.n_steps, n_to_remove) ## TODO: DEAL with the following arrays!!! state.r_gripper_states state.l_gripper_states state.r_ref_frames state.l_ref_frames state.objects if (self.currentStep != state.i_current_step): if (self.n_steps() > 0): self.currentStep = state.i_current_step arm_index, index = self.get_arm_and_index(self.currentStep) if (arm_index == 0): self.r_view.selectRow(index) else: self.l_view.selectRow(index) def get_frame_type(self, fr_type): if (fr_type > 1): rospy.logwarn("Invalid frame type @ save_pose -> get_frame_type: " + str(fr_type)) return ["Go to pose", "Maneuver"][fr_type] def save_pose(self, actionIndex=None, frameType=0): nColumns = 9 if actionIndex is None: actionIndex = self.currentAction stepIndex = self.n_steps(actionIndex) r_step = [QtGui.QStandardItem('Step' + str(stepIndex + 1)), QtGui.QStandardItem(self.get_frame_type(frameType)), QtGui.QStandardItem('Absolute')] l_step = [QtGui.QStandardItem('Step' + str(stepIndex + 1)), QtGui.QStandardItem(self.get_frame_type(frameType)), QtGui.QStandardItem('Absolute')] self.r_model.invisibleRootItem().appendRow(r_step) self.l_model.invisibleRootItem().appendRow(l_step) self.update_table_view() self.currentStep = stepIndex def update_table_view(self): self.l_view.setColumnWidth(0, 50) self.l_view.setColumnWidth(1, 100) self.l_view.setColumnWidth(2, 70) self.r_view.setColumnWidth(0, 50) self.r_view.setColumnWidth(1, 100) self.r_view.setColumnWidth(2, 70) def n_steps(self, actionIndex=None): return self.l_model.invisibleRootItem().rowCount() def delete_all_steps(self, actionIndex=None): if actionIndex is None: actionIndex = self.currentAction n_steps = self.n_steps() if (n_steps > 0): self.l_model.invisibleRootItem().removeRows(0, n_steps) self.r_model.invisibleRootItem().removeRows(0, n_steps) def n_actions(self): return len(self.actionIcons.keys()) def new_action(self): nColumns = 6 actionIndex = self.n_actions() for key in self.actionIcons.keys(): self.actionIcons[key].selected = False self.actionIcons[key].updateView() actIcon = ActionIcon(self._widget, actionIndex, self.action_pressed) self.actionGrid.addLayout(actIcon, int(actionIndex/nColumns), actionIndex%nColumns) self.actionIcons[actionIndex] = actIcon def step_pressed(self, step_index): gui_cmd = GuiCommand(GuiCommand.SELECT_ACTION_STEP, step_index) self.gui_cmd_publisher.publish(gui_cmd) def action_pressed(self, actionIndex, isPublish=True): for i in range(len(self.actionIcons.keys())): key = self.actionIcons.keys()[i] if key == actionIndex: self.actionIcons[key].selected = True else: self.actionIcons[key].selected = False self.actionIcons[key].updateView() self.currentAction = actionIndex self.stepsBox.setTitle('Steps for Action ' + str(self.currentAction+1)) if isPublish: gui_cmd = GuiCommand(GuiCommand.SWITCH_TO_ACTION, (actionIndex+1)) self.gui_cmd_publisher.publish(gui_cmd) def command_cb(self): clickedButtonName = self._widget.sender().text() for key in self.commands.keys(): if (self.commands[key] == clickedButtonName): qWarning('Sending speech command: '+ key) command = Command() command.command = key self.speech_cmd_publisher.publish(command) def robotSoundReceived(self, soundReq): if (soundReq.command == SoundRequest.SAY): qWarning('Robot said: ' + soundReq.arg) self.speechLabel.setText('Robot sound: ' + soundReq.arg) def exp_state_cb(self, state): qWarning('Received new experiment state.') self.exp_state_sig.emit(state) def shutdown_plugin(self): # TODO unregister all publishers here self.speech_cmd_publisher.unregister() self.gui_cmd_publisher.unregister() pass def save_settings(self, plugin_settings, instance_settings): # TODO save intrinsic configuration, usually using: # instance_settings.set_value(k, v) pass def restore_settings(self, plugin_settings, instance_settings): # TODO restore intrinsic configuration, usually using: # v = instance_settings.value(k) pass