class BoxGroup(GroupWidget): def __init__(self, updater, config): super(BoxGroup, self).__init__(updater, config) self.box = QGroupBox(self.name) self.box.setLayout(self.grid) def display(self, grid, row): grid.addWidget(self.box, row, 0, 1, -1)
class BoxGroup(GroupWidget): def __init__(self, updater, config, nodename): super(BoxGroup, self).__init__(updater, config, nodename) self.box = QGroupBox(self.param_name) self.box.setLayout(self.grid) def display(self, grid): grid.addRow(self.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)
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 FootstepParamControlWidget(QObject): def __init__(self, context): #super(FootstepParamControlDialog, self).__init__(context) #self.setObjectName('FootstepParamControlDialog') super(FootstepParamControlWidget, self).__init__() self.name = 'FootstepParamControlWidget' #self._widget = QWidget() self._widget = context vbox = QVBoxLayout() ### STEP_COST_ESTIMATOR ######## self.step_cost_vbox = QVBoxLayout() self.step_cost_groupbox = QGroupBox( "Step Cost Estimator" ) self.step_cost_groupbox.setCheckable( True ) self.step_cost_groupbox.setChecked(False) self.step_cost_combobox = QComboBox() self.step_cost_combobox.addItem( "Euclidean" ) self.step_cost_combobox.addItem( "GPR" ) self.step_cost_combobox.addItem( "Map" ) self.step_cost_combobox.addItem( "Boundary" ) self.step_cost_combobox.addItem( "Dynamics" ) self.step_cost_vbox.addWidget( self.step_cost_combobox ) self.step_cost_groupbox.setLayout( self.step_cost_vbox ) vbox.addWidget( self.step_cost_groupbox ) # #HARD ### FOOTSTEP_SET ######## # # parameters for discret footstep planning mode # vigir_atlas_control_msgs/VigirBehaviorStepData[] footstep_set # set of footsteps (displacement vectors (in meter / rad)) # float32[] footstep_cost # cost for each footstep given in footstep set # # #HARD ### LOAD_GPR_STEP_COST ######## # # map step cost file # std_msgs/String map_step_cost_file # # #HARD ### LOAD_MAP_STEP_COST ######## # # destination of gpr file # std_msgs/String gpr_step_cost_file ### COLLISION_CHECK_TYPE ######## self.collision_check_groupbox = QGroupBox( "Collision Check Type" ) self.collision_check_groupbox.setCheckable( True ) self.collision_check_groupbox.setChecked( False) self.collision_check_vbox = QVBoxLayout() self.collision_check_feet_checkbox = QCheckBox( "Feet" ) self.collision_check_vbox.addWidget( self.collision_check_feet_checkbox ) self.collision_check_ub_checkbox = QCheckBox( "Upper Body" ) self.collision_check_vbox.addWidget( self.collision_check_ub_checkbox ) self.collision_check_fc_checkbox = QCheckBox( "Foot Contact Support" ) self.collision_check_vbox.addWidget( self.collision_check_fc_checkbox ) self.collision_check_groupbox.setLayout( self.collision_check_vbox ) vbox.addWidget( self.collision_check_groupbox ) ### FOOT_SIZE ######## self.foot_size_vbox = QVBoxLayout() self.foot_size_groupbox = QGroupBox( "Foot Size" ) self.foot_size_groupbox.setCheckable( True ) self.foot_size_groupbox.setChecked( False ) self.foot_size_hbox = QHBoxLayout() self.foot_size_label = QLabel("Foot Size") self.foot_size_hbox.addWidget( self.foot_size_label ) self.foot_size_x = QDoubleSpinBox() self.foot_size_x.setSingleStep(.01) self.foot_size_hbox.addWidget(self.foot_size_x) self.foot_size_y = QDoubleSpinBox() self.foot_size_y.setSingleStep(.01) self.foot_size_hbox.addWidget(self.foot_size_y) self.foot_size_z = QDoubleSpinBox() self.foot_size_z.setSingleStep(.01) self.foot_size_hbox.addWidget(self.foot_size_z) self.foot_size_vbox.addLayout( self.foot_size_hbox ) self.foot_shift_hbox = QHBoxLayout() self.foot_shift_label = QLabel("Foot Origin Shift") self.foot_shift_hbox.addWidget( self.foot_shift_label ) self.foot_shift_x = QDoubleSpinBox() self.foot_shift_x.setRange(-1.0, 1.0) self.foot_shift_x.setSingleStep(.01) self.foot_shift_hbox.addWidget(self.foot_shift_x) self.foot_shift_y = QDoubleSpinBox() self.foot_shift_y.setRange(-1.0, 1.0) self.foot_shift_y.setSingleStep(.01) self.foot_shift_hbox.addWidget(self.foot_shift_y) self.foot_shift_z = QDoubleSpinBox() self.foot_shift_z.setRange(-1.0, 1.0) self.foot_shift_z.setSingleStep(.01) self.foot_shift_hbox.addWidget(self.foot_shift_z) self.foot_size_vbox.addLayout( self.foot_shift_hbox ) self.foot_size_groupbox.setLayout( self.foot_size_vbox ) vbox.addWidget( self.foot_size_groupbox ) ### UPPER_BODY_SIZE ######## self.upper_vbox = QVBoxLayout() self.upper_groupbox = QGroupBox( "Upper Body Size" ) self.upper_groupbox.setCheckable( True ) self.upper_groupbox.setChecked( False ) self.upper_size_hbox = QHBoxLayout() self.upper_size_label = QLabel("Upper Body Size") self.upper_size_hbox.addWidget( self.upper_size_label ) self.upper_size_x = QDoubleSpinBox() self.upper_size_x.setSingleStep(.01) self.upper_size_hbox.addWidget(self.upper_size_x) self.upper_size_y = QDoubleSpinBox() self.upper_size_y.setSingleStep(.01) self.upper_size_hbox.addWidget(self.upper_size_y) self.upper_size_z = QDoubleSpinBox() self.upper_size_z.setSingleStep(.01) self.upper_size_hbox.addWidget(self.upper_size_z) self.upper_vbox.addLayout( self.upper_size_hbox ) self.upper_origin_hbox = QHBoxLayout() self.upper_origin_label = QLabel("Upper Body Origin") self.upper_origin_hbox.addWidget( self.upper_origin_label ) self.upper_origin_x = QDoubleSpinBox() self.upper_origin_x.setRange(-1.0, 1.0) self.upper_origin_x.setSingleStep(.01) self.upper_origin_hbox.addWidget(self.upper_origin_x) self.upper_origin_y = QDoubleSpinBox() self.upper_origin_y.setRange(-1.0, 1.0) self.upper_origin_y.setSingleStep(.01) self.upper_origin_hbox.addWidget(self.upper_origin_y) self.upper_origin_z = QDoubleSpinBox() self.upper_origin_z.setRange(-1.0, 1.0) self.upper_origin_z.setSingleStep(.01) self.upper_origin_hbox.addWidget(self.upper_origin_z) self.upper_vbox.addLayout( self.upper_origin_hbox ) self.upper_groupbox.setLayout( self.upper_vbox ) vbox.addWidget( self.upper_groupbox ) ### TERRAIN_MODEL ######## self.terrain_model_groupbox = QGroupBox( "Terrain Model" ) self.terrain_model_groupbox.setCheckable( True ) self.terrain_model_groupbox.setChecked( False ) self.terrain_model_vbox = QVBoxLayout() self.terrain_model_checkbox = QCheckBox( "Use Terrain Model" ) self.terrain_model_vbox.addWidget( self.terrain_model_checkbox ) self.terrain_min_ssx_hbox = QHBoxLayout() self.terrain_min_ssx_label = QLabel("Min Sampling Steps x") self.terrain_min_ssx_hbox.addWidget( self.terrain_min_ssx_label ) self.terrain_min_ssx_val = QDoubleSpinBox() self.terrain_min_ssx_val.setSingleStep(1) self.terrain_min_ssx_val.setRange(0,100) self.terrain_min_ssx_hbox.addWidget( self.terrain_min_ssx_val ) self.terrain_model_vbox.addLayout( self.terrain_min_ssx_hbox ) self.terrain_min_ssy_hbox = QHBoxLayout() self.terrain_min_ssy_label = QLabel("Min Sampling Steps y") self.terrain_min_ssy_hbox.addWidget( self.terrain_min_ssy_label ) self.terrain_min_ssy_val = QDoubleSpinBox() self.terrain_min_ssy_val.setSingleStep(1) self.terrain_min_ssy_val.setRange(0,100) self.terrain_min_ssy_hbox.addWidget( self.terrain_min_ssy_val ) self.terrain_model_vbox.addLayout( self.terrain_min_ssy_hbox ) self.terrain_max_ssx_hbox = QHBoxLayout() self.terrain_max_ssx_label = QLabel("Max Sampling Steps x") self.terrain_max_ssx_hbox.addWidget( self.terrain_max_ssx_label ) self.terrain_max_ssx_val = QDoubleSpinBox() self.terrain_max_ssx_val.setSingleStep(1) self.terrain_max_ssx_val.setRange(0,100) self.terrain_max_ssx_hbox.addWidget( self.terrain_max_ssx_val ) self.terrain_model_vbox.addLayout( self.terrain_max_ssx_hbox ) self.terrain_max_ssy_hbox = QHBoxLayout() self.terrain_max_ssy_label = QLabel("Max Sampling Steps y") self.terrain_max_ssy_hbox.addWidget( self.terrain_max_ssy_label ) self.terrain_max_ssy_val = QDoubleSpinBox() self.terrain_max_ssy_val.setSingleStep(1) self.terrain_max_ssy_val.setRange(0,100) self.terrain_max_ssy_hbox.addWidget( self.terrain_max_ssy_val ) self.terrain_model_vbox.addLayout( self.terrain_max_ssy_hbox ) self.terrain_max_iz_hbox = QHBoxLayout() self.terrain_max_iz_label = QLabel("Max Intrusion z") self.terrain_max_iz_hbox.addWidget( self.terrain_max_iz_label ) self.terrain_max_iz_val = QDoubleSpinBox() self.terrain_max_iz_val.setDecimals(4) self.terrain_max_iz_val.setSingleStep(.0001) self.terrain_max_iz_val.setRange(0,.25) self.terrain_max_iz_hbox.addWidget( self.terrain_max_iz_val ) self.terrain_model_vbox.addLayout( self.terrain_max_iz_hbox ) self.terrain_max_gc_hbox = QHBoxLayout() self.terrain_max_gc_label = QLabel("Max Ground Clearance") self.terrain_max_gc_hbox.addWidget( self.terrain_max_gc_label ) self.terrain_max_gc_val = QDoubleSpinBox() self.terrain_max_gc_val.setDecimals(4) self.terrain_max_gc_val.setSingleStep(.0001) self.terrain_max_gc_val.setRange(0,.25) self.terrain_max_gc_hbox.addWidget( self.terrain_max_gc_val ) self.terrain_model_vbox.addLayout( self.terrain_max_gc_hbox ) self.terrain_ms_hbox = QHBoxLayout() self.terrain_ms_label = QLabel("Minimal Support") self.terrain_ms_hbox.addWidget( self.terrain_ms_label ) self.terrain_ms_val = QDoubleSpinBox() self.terrain_ms_val.setDecimals(2) self.terrain_ms_val.setSingleStep(.01) self.terrain_ms_val.setRange(0,1) self.terrain_ms_hbox.addWidget( self.terrain_ms_val ) self.terrain_model_vbox.addLayout( self.terrain_ms_hbox ) self.terrain_model_groupbox.setLayout( self.terrain_model_vbox ) vbox.addWidget( self.terrain_model_groupbox ) ### STANDARD_STEP_PARAMS ######## self.std_step_vbox = QVBoxLayout() self.std_step_groupbox = QGroupBox( "Standard Step Parameters" ) self.std_step_groupbox.setCheckable( True ) self.std_step_groupbox.setChecked( False ) self.foot_sep_hbox = QHBoxLayout() self.foot_sep_label = QLabel("Foot Separation") self.foot_sep_hbox.addWidget( self.foot_sep_label ) self.foot_sep_val = QDoubleSpinBox() self.foot_sep_val.setSingleStep(.01) self.foot_sep_hbox.addWidget(self.foot_sep_val) self.std_step_vbox.addLayout( self.foot_sep_hbox ) self.std_step_step_duration_hbox = QHBoxLayout() self.std_step_step_duration_label = QLabel("Step Duration") self.std_step_step_duration_hbox.addWidget( self.std_step_step_duration_label ) self.std_step_step_duration_val = QDoubleSpinBox() self.std_step_step_duration_val.setSingleStep(.01) self.std_step_step_duration_hbox.addWidget( self.std_step_step_duration_val ) self.std_step_vbox.addLayout( self.std_step_step_duration_hbox ) self.std_step_sway_duration_hbox = QHBoxLayout() self.std_step_sway_duration_label = QLabel("Sway Duration") self.std_step_sway_duration_hbox.addWidget( self.std_step_sway_duration_label ) self.std_step_sway_duration_val = QDoubleSpinBox() self.std_step_sway_duration_val.setSingleStep(.01) self.std_step_sway_duration_hbox.addWidget( self.std_step_sway_duration_val ) self.std_step_vbox.addLayout( self.std_step_sway_duration_hbox ) self.std_step_swing_height_hbox = QHBoxLayout() self.std_step_swing_height_label = QLabel("Swing Height") self.std_step_swing_height_hbox.addWidget( self.std_step_swing_height_label ) self.std_step_swing_height_val = QDoubleSpinBox() self.std_step_swing_height_val.setSingleStep(.01) self.std_step_swing_height_hbox.addWidget( self.std_step_swing_height_val ) self.std_step_vbox.addLayout( self.std_step_swing_height_hbox ) self.std_step_lift_height_hbox = QHBoxLayout() self.std_step_lift_height_label = QLabel("Lift Height") self.std_step_lift_height_hbox.addWidget( self.std_step_lift_height_label ) self.std_step_lift_height_val = QDoubleSpinBox() self.std_step_lift_height_val.setSingleStep(.01) self.std_step_lift_height_hbox.addWidget( self.std_step_lift_height_val ) self.std_step_vbox.addLayout( self.std_step_lift_height_hbox ) self.std_step_groupbox.setLayout( self.std_step_vbox ) vbox.addWidget( self.std_step_groupbox ) button_hbox = QHBoxLayout() button_get = QPushButton("Get Current Values") button_hbox.addWidget( button_get ) button_submit = QPushButton("Send Values") button_hbox.addWidget( button_submit) vbox.addLayout( button_hbox ) vbox.addStretch(1) self._widget.setLayout(vbox) #context.add_widget(self._widget) # publishers and subscribers self.param_pub = rospy.Publisher('/flor/footstep_planner/set_params', FootstepPlannerParams, queue_size=10) button_submit.pressed.connect(self.sendParams) self.param_sub = self.stateSubscriber = rospy.Subscriber('/ros_footstep_planner/params', FootstepPlannerParams, self.getParamCallbackFcn) button_get.pressed.connect(self.getParams) def shutdown_plugin(self): print "Shutdown ..." self.param_pub.unregister() print "Done!" def getParams( self ): msg = FootstepPlannerParams() msg.change_mask = 0 self.param_pub.publish( msg ) def sendParams( self ): msg = FootstepPlannerParams() msg.change_mask = 0 if self.step_cost_groupbox.isChecked(): msg.change_mask = msg.change_mask | 0x001 if self.collision_check_groupbox.isChecked(): msg.change_mask = msg.change_mask | 0x010 if self.foot_size_groupbox.isChecked(): msg.change_mask = msg.change_mask | 0x020 if self.upper_groupbox.isChecked(): msg.change_mask = msg.change_mask | 0x040 if self.std_step_groupbox.isChecked(): msg.change_mask = msg.change_mask | 0x080 if self.terrain_model_groupbox.isChecked(): msg.change_mask = msg.change_mask | 0x100 ### STEP_COST_ESTIMATOR ######## msg.step_cost_type = self.step_cost_combobox.currentIndex() # #HARD ### FOOTSTEP_SET ######## # # parameters for discret footstep planning mode # vigir_atlas_control_msgs/VigirBehaviorStepData[] footstep_set # set of footsteps (displacement vectors (in meter / rad)) # float32[] footstep_cost # cost for each footstep given in footstep set # # #HARD ### LOAD_GPR_STEP_COST ######## # # map step cost file # std_msgs/String map_step_cost_file # # #HARD ### LOAD_MAP_STEP_COST ######## # # destination of gpr file # std_msgs/String gpr_step_cost_file ### COLLISION_CHECK_TYPE ######## msg.collision_check_type = 0 if self.collision_check_feet_checkbox.isChecked(): msg.collision_check_type = msg.collision_check_type | 0x01 if self.collision_check_ub_checkbox.isChecked(): msg.collision_check_type = msg.collision_check_type | 0x02 if self.collision_check_fc_checkbox.isChecked(): msg.collision_check_type = msg.collision_check_type | 0x04 ### FOOT_SIZE ######## msg.foot_size.x = self.foot_size_x.value() msg.foot_size.y = self.foot_size_y.value() msg.foot_size.z = self.foot_size_z.value() msg.foot_origin_shift.x = self.foot_shift_x.value() msg.foot_origin_shift.y = self.foot_shift_y.value() msg.foot_origin_shift.z = self.foot_shift_z.value() ### UPPER_BODY_SIZE ######## msg.upper_body_size.x = self.upper_size_x.value() msg.upper_body_size.y = self.upper_size_y.value() msg.upper_body_size.z = self.upper_size_z.value() msg.upper_body_origin_shift.x = self.upper_origin_x.value() msg.upper_body_origin_shift.y = self.upper_origin_y.value() msg.upper_body_origin_shift.z = self.upper_origin_z.value() ### TERRAIN_MODEL ######## msg.use_terrain_model = self.terrain_model_checkbox.isChecked() msg.min_sampling_steps_x = self.terrain_min_ssx_val.value() msg.min_sampling_steps_y = self.terrain_min_ssy_val.value() msg.max_sampling_steps_x = self.terrain_max_ssx_val.value() msg.max_sampling_steps_y = self.terrain_max_ssy_val.value() msg.max_intrusion_z = self.terrain_max_iz_val.value() msg.max_ground_clearance = self.terrain_max_gc_val.value() msg.minimal_support = self.terrain_ms_val.value() ### STANDARD_STEP_PARAMS ######## msg.foot_seperation = self.foot_sep_val.value() msg.step_duration = self.std_step_step_duration_val.value() msg.sway_duration = self.std_step_sway_duration_val.value() msg.swing_height = self.std_step_swing_height_val.value() msg.lift_height = self.std_step_lift_height_val.value() print msg self.param_pub.publish( msg ) def getParamCallbackFcn(self, msg): ### STEP_COST_ESTIMATOR ######## if msg.change_mask & 0x001: self.step_cost_combobox.setCurrentIndex( msg.step_cost_type ) # #HARD ### FOOTSTEP_SET ######## # # parameters for discret footstep planning mode # vigir_atlas_control_msgs/VigirBehaviorStepData[] footstep_set # set of footsteps (displacement vectors (in meter / rad)) # float32[] footstep_cost # cost for each footstep given in footstep set # # #HARD ### LOAD_GPR_STEP_COST ######## # # map step cost file # std_msgs/String map_step_cost_file # # #HARD ### LOAD_MAP_STEP_COST ######## # # destination of gpr file # std_msgs/String gpr_step_cost_file ### COLLISION_CHECK_TYPE ######## if msg.change_mask & 0x010: self.collision_check_feet_checkbox.setChecked( msg.collision_check_type & 0x01 ) self.collision_check_ub_checkbox.setChecked( msg.collision_check_type & 0x02 ) self.collision_check_fc_checkbox.setChecked( msg.collision_check_type & 0x04 ) ### FOOT_SIZE ######## if msg.change_mask & 0x020: self.foot_size_x.setValue( msg.foot_size.x ) self.foot_size_y.setValue( msg.foot_size.y ) self.foot_size_z.setValue( msg.foot_size.z ) self.foot_shift_x.setValue( msg.foot_origin_shift.x ) self.foot_shift_y.setValue( msg.foot_origin_shift.y ) self.foot_shift_z.setValue( msg.foot_origin_shift.z ) ### UPPER_BODY_SIZE ######## if msg.change_mask & 0x040: self.upper_size_x.setValue( msg.upper_body_size.x ) self.upper_size_y.setValue( msg.upper_body_size.y ) self.upper_size_z.setValue( msg.upper_body_size.z ) self.upper_origin_x.setValue( msg.upper_body_origin_shift.x ) self.upper_origin_y.setValue( msg.upper_body_origin_shift.y ) self.upper_origin_z.setValue( msg.upper_body_origin_shift.z ) ### STANDARD_STEP_PARAMS ######## if msg.change_mask & 0x080: self.foot_sep_val.setValue( msg.foot_seperation ) self.std_step_step_duration_val.setValue( msg.step_duration ) self.std_step_sway_duration_val.setValue( msg.sway_duration ) self.std_step_swing_height_val.setValue( msg.swing_height ) self.std_step_lift_height_val.setValue( msg.lift_height ) ### TERRAIN_MODEL ######## if msg.change_mask & 0x100: self.terrain_model_checkbox.setChecked( msg.use_terrain_model ) self.terrain_min_ssx_val.setValue( msg.min_sampling_steps_x ) self.terrain_min_ssy_val.setValue( msg.min_sampling_steps_y ) self.terrain_max_ssx_val.setValue( msg.max_sampling_steps_x ) self.terrain_max_ssy_val.setValue( msg.max_sampling_steps_y ) self.terrain_max_iz_val.setValue( msg.max_intrusion_z ) self.terrain_max_gc_val.setValue( msg.max_ground_clearance ) self.terrain_ms_val.setValue( msg.minimal_support )
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)
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
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)
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 FootstepTerrainControlDialog(Plugin): def __init__(self, context): super(FootstepTerrainControlDialog, self).__init__(context) self.setObjectName('FootstepTerrainControlDialog') self.request_seq = 0 self._widget = QWidget() vbox = QVBoxLayout() self.cube_vbox = QVBoxLayout() self.cube_groupbox = QGroupBox( "Terrain Box" ) self.cube_min_hbox = QHBoxLayout() self.cube_min_label = QLabel("Bottom Left") self.cube_min_hbox.addWidget( self.cube_min_label ) self.cube_min_x = QDoubleSpinBox() self.cube_min_x.setSingleStep(.01) self.cube_min_x.setRange(-10.0, 10.0) self.cube_min_x.setValue(-3.0) self.cube_min_hbox.addWidget(self.cube_min_x) self.cube_min_y = QDoubleSpinBox() self.cube_min_y.setSingleStep(.01) self.cube_min_y.setRange(-10.0, 10.0) self.cube_min_y.setValue(-3.0) self.cube_min_hbox.addWidget(self.cube_min_y) self.cube_min_z = QDoubleSpinBox() self.cube_min_z.setSingleStep(.01) self.cube_min_z.setRange(-10.0, 10.0) self.cube_min_z.setValue(-1.0) self.cube_min_hbox.addWidget(self.cube_min_z) self.cube_vbox.addLayout( self.cube_min_hbox ) self.cube_max_hbox = QHBoxLayout() self.cube_max_label = QLabel("Top Right") self.cube_max_hbox.addWidget( self.cube_max_label ) self.cube_max_x = QDoubleSpinBox() self.cube_max_x.setSingleStep(.01) self.cube_max_x.setRange(-10.0, 10.0) self.cube_max_x.setValue(3.0) self.cube_max_hbox.addWidget(self.cube_max_x) self.cube_max_y = QDoubleSpinBox() self.cube_max_y.setSingleStep(.01) self.cube_max_y.setRange(-10.0, 10.0) self.cube_max_y.setValue(3.0) self.cube_max_hbox.addWidget(self.cube_max_y) self.cube_max_z = QDoubleSpinBox() self.cube_max_z.setSingleStep(.01) self.cube_max_z.setRange(-10.0, 10.0) self.cube_max_z.setValue(1.0) self.cube_max_hbox.addWidget(self.cube_max_z) self.cube_vbox.addLayout( self.cube_max_hbox ) self.cube_groupbox.setLayout( self.cube_vbox ) vbox.addWidget( self.cube_groupbox ) self.type_hbox = QHBoxLayout() # self.type_label = QLabel( "Type" ) # self.type_hbox.addWidget( self.type_label ) # # self.type_combobox = QComboBox() # # self.type_combobox.addItem( "Type 1" ) # self.type_combobox.addItem( "Type 2" ) # self.type_combobox.addItem( "Type 3" ) # # self.type_hbox.addWidget( self.type_combobox ) # # vbox.addLayout( self.type_hbox ) self.scan_number_hbox = QHBoxLayout() self.scan_number_label = QLabel("Number of Scans Used") self.scan_number_hbox.addWidget( self.scan_number_label ) self.scan_number_val = QDoubleSpinBox() self.scan_number_val.setDecimals(0) self.scan_number_val.setRange(0,10000) self.scan_number_val.setValue(2000) self.scan_number_hbox.addWidget(self.scan_number_val) vbox.addLayout( self.scan_number_hbox ) # self.use_terrain_checkbox = QCheckBox( "Use Terrain" ) # vbox.addWidget( self.use_terrain_checkbox ) button_hbox = QHBoxLayout() # button_get = QPushButton("Get Current Values") # button_hbox.addWidget( button_get ) button_submit = QPushButton("Send Values") button_hbox.addWidget( button_submit) vbox.addLayout( button_hbox ) vbox.addStretch(1) self._widget.setLayout(vbox) context.add_widget(self._widget) # publishers and subscribers self.terrain_pub = rospy.Publisher("/flor/terrain_classifier/generate_terrain_model", TerrainModelRequest, queue_size=10 ) button_submit.pressed.connect(self.sendParams) # self.terrain_sub = self.stateSubscriber = rospy.Subscriber('/ros_footstep_planner/terrains', FootstepPlannerParams, self.getParamCallbackFcn) # button_submit.pressed.connect(self.getParams) def shutdown_plugin(self): print "Shutdown ..." print "Done!" # def getParams( self ): # msg = FootstepPlannerParams() # # # self.terrain_pub.publish( msg ) def sendParams( self ): msg = TerrainModelRequest() msg.use_default_region_request = False rr = EnvironmentRegionRequest() rr.header.seq = self.request_seq self.request_seq = self.request_seq+1 rr.header.stamp = rospy.get_rostime() rr.header.frame_id = "/world" rr.bounding_box_min.x = self.cube_min_x.value() rr.bounding_box_min.y = self.cube_min_y.value() rr.bounding_box_min.z = self.cube_min_z.value() rr.bounding_box_max.x = self.cube_max_x.value() rr.bounding_box_max.y = self.cube_max_y.value() rr.bounding_box_max.z = self.cube_max_z.value() rr.resolution = 0 rr.request_augment = 0 msg.region_req = rr msg.aggregation_size = self.scan_number_val.value() print msg self.terrain_pub.publish( msg ) def getParamCallbackFcn(self, msg): print "Not Defined Yet"
def __init__(self, context): super(SensorParamControlDialog, self).__init__(context) self.setObjectName('SensorParamControlDialog') self._widget = QWidget() vbox = QVBoxLayout() ### Multisense ### ms_groupbox = QGroupBox( "Multisense" ) ms_vbox = QVBoxLayout() ms_gain_hbox = QHBoxLayout() self.ms_gain_label = QLabel("Image Gain [1, 1, 8]") ms_gain_hbox.addWidget( self.ms_gain_label ) self.ms_gain = QDoubleSpinBox() self.ms_gain.setSingleStep(.01) self.ms_gain.setRange(1,8) ms_gain_hbox.addWidget( self.ms_gain ) ms_vbox.addLayout( ms_gain_hbox ) ms_exp_hbox = QHBoxLayout() self.ms_exp_auto = QCheckBox("Image Exposure [.03, 0.5]") ms_exp_hbox.addWidget( self.ms_exp_auto ) self.ms_exp = QDoubleSpinBox() self.ms_exp.setSingleStep( .001 ) self.ms_exp.setRange( .025,.5 ) ms_exp_hbox.addWidget( self.ms_exp ) ms_vbox.addLayout( ms_exp_hbox ) ms_spindle_hbox = QHBoxLayout() ms_spindle_label = QLabel("Spindle Speed [0, 5.2]") ms_spindle_hbox.addWidget( ms_spindle_label ) self.ms_spindle = QDoubleSpinBox() self.ms_spindle.setSingleStep(.01) self.ms_spindle.setRange( 0,15.2 ) ms_spindle_hbox.addWidget( self.ms_spindle ) ms_vbox.addLayout( ms_spindle_hbox ) ms_light_hbox = QHBoxLayout() ms_light_label = QLabel("Light Brightness") ms_light_hbox.addWidget(ms_light_label) self.ms_light = QSlider(Qt.Horizontal) self.ms_light.setRange(0,100) ms_light_hbox.addWidget( self.ms_light ) ms_vbox.addLayout( ms_light_hbox ) ms_button_hbox = QHBoxLayout() ms_button_hbox.addStretch(1) ms_button_get = QPushButton("Get Settings") ms_button_get.pressed.connect(self.ms_get_callback) #ms_button_hbox.addWidget( ms_button_get ) ms_button_set = QPushButton("Set Settings") ms_button_set.pressed.connect(self.ms_set_callback) ms_button_hbox.addWidget( ms_button_set ) ms_vbox.addLayout( ms_button_hbox ) ms_groupbox.setLayout( ms_vbox ) vbox.addWidget( ms_groupbox ) ### Left SA ### sa_left_groupbox = QGroupBox( "Left SA Camera" ) sa_left_vbox = QVBoxLayout() sa_left_gain_hbox = QHBoxLayout() sa_left_gain_label = QLabel("Image Gain [0, 0, 25]") sa_left_gain_hbox.addWidget( sa_left_gain_label ) self.sa_left_gain = QDoubleSpinBox() self.sa_left_gain.setSingleStep(.01) self.sa_left_gain.setRange( 0, 25 ) sa_left_gain_hbox.addWidget( self.sa_left_gain ) sa_left_vbox.addLayout( sa_left_gain_hbox ) sa_left_exp_hbox = QHBoxLayout() sa_left_exp_label = QLabel("Image Shutter [0, 5.5, 25]") sa_left_exp_hbox.addWidget( sa_left_exp_label ) self.sa_left_exp = QDoubleSpinBox() self.sa_left_exp.setSingleStep(.01) self.sa_left_exp.setRange( 0, 25 ) sa_left_exp_hbox.addWidget( self.sa_left_exp ) sa_left_vbox.addLayout( sa_left_exp_hbox ) sa_left_button_hbox = QHBoxLayout() sa_left_button_hbox.addStretch(1) sa_left_button_get = QPushButton("Get Settings") sa_left_button_get.pressed.connect(self.sa_left_get_callback) #sa_left_button_hbox.addWidget( sa_left_button_get ) sa_left_button_set = QPushButton("Set Settings") sa_left_button_set.pressed.connect(self.sa_left_set_callback) sa_left_button_hbox.addWidget( sa_left_button_set ) sa_left_vbox.addLayout( sa_left_button_hbox ) sa_left_groupbox.setLayout( sa_left_vbox ) vbox.addWidget(sa_left_groupbox) ### Right SA ### sa_right_groupbox = QGroupBox( "Right SA Camera" ) sa_right_vbox = QVBoxLayout() sa_right_gain_hbox = QHBoxLayout() sa_right_gain_label = QLabel("Image Gain [0, 0, 25]") sa_right_gain_hbox.addWidget( sa_right_gain_label ) self.sa_right_gain = QDoubleSpinBox() self.sa_right_gain.setSingleStep(.01) self.sa_right_gain.setRange( 0, 25 ) sa_right_gain_hbox.addWidget( self.sa_right_gain ) sa_right_vbox.addLayout( sa_right_gain_hbox ) sa_right_exp_hbox = QHBoxLayout() sa_right_exp_label = QLabel("Image Shutter [0, 5.5, 25]") sa_right_exp_hbox.addWidget( sa_right_exp_label ) self.sa_right_exp = QDoubleSpinBox() self.sa_right_exp.setSingleStep(.01) self.sa_right_exp.setRange( 0, 25 ) sa_right_exp_hbox.addWidget( self.sa_right_exp ) sa_right_vbox.addLayout( sa_right_exp_hbox ) sa_right_button_hbox = QHBoxLayout() sa_right_button_hbox.addStretch(1) sa_right_button_get = QPushButton("Get Settings") sa_right_button_get.pressed.connect(self.sa_right_get_callback) #sa_right_button_hbox.addWidget( sa_right_button_get ) sa_right_button_set = QPushButton("Set Settings") sa_right_button_set.pressed.connect(self.sa_right_set_callback) sa_right_button_hbox.addWidget( sa_right_button_set ) sa_right_vbox.addLayout( sa_right_button_hbox ) sa_right_groupbox.setLayout( sa_right_vbox ) vbox.addWidget(sa_right_groupbox) vbox.addStretch(1) self._widget.setLayout(vbox) context.add_widget(self._widget) # publishers and subscribers self.ms_cam_pub = rospy.Publisher( '/multisense_sl/set_cam_parameters', Float64MultiArray, queue_size=10) self.ms_light_pub = rospy.Publisher( '/multisense_sl/set_light_brightness', Float64, queue_size=10) self.ms_spindle_pub = rospy.Publisher( '/multisense_sl/set_spindle_speed', Float64, queue_size=10) self.sa_left_cam_pub = rospy.Publisher( '/sa/left/set_cam_parameters', Float64MultiArray, queue_size=10) self.sa_right_cam_pub = rospy.Publisher( '/sa/right/set_cam_parameters', Float64MultiArray, queue_size=10)
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)
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
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' #adding record motion self.commands[Command.START_RECORDING_MOTION] = 'Record motion' self.commands[Command.START_RECORDING_RELATIVE_MOTION] = 'Record relative motion' self.commands[Command.STOP_RECORDING_MOTION] = 'Stop recording motion' 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.REPEAT_LAST_STEP] = 'Repeat last step' 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)) stepsButtonGrid.addWidget(self.create_button(Command.REPEAT_LAST_STEP)) motionButtonGrid = QtGui.QHBoxLayout() motionButtonGrid.addWidget(self.create_button(Command.START_RECORDING_MOTION)) motionButtonGrid.addWidget(self.create_button(Command.START_RECORDING_RELATIVE_MOTION)) motionButtonGrid.addWidget(self.create_button(Command.STOP_RECORDING_MOTION)) 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.addLayout(motionButtonGrid) 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)