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)
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):
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)
def _create_found_frame(self):
self.found_files_frame = ff_frame = QGroupBox("recursive search")
ff_frame.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding)
self.found_files_vbox_layout = QVBoxLayout(ff_frame)
self.found_files_vbox_layout.setSpacing(0)
self.found_files_vbox_layout.setContentsMargins(0, 0, 0, 0)
self.found_files_list = QListWidget(ff_frame)
self.found_files_list.setSizePolicy(QSizePolicy.Expanding,
QSizePolicy.Expanding)
self.found_files_list.setFrameStyle(QFrame.StyledPanel)
self.found_files_list.itemActivated.connect(self.on_itemActivated)
self.found_files_list.setStyleSheet("QListWidget {"
"background-color:transparent;"
"}"
"QListWidget::item {"
"background-color:transparent;"
"}"
"QListWidget::item:selected {"
"background-color: darkgray;"
"}")
self.found_files_vbox_layout.addWidget(self.found_files_list)
ff_frame.setCheckable(True)
ff_frame.setChecked(False)
ff_frame.setFlat(True)
self.found_files_list.setVisible(False)
return self.found_files_frame
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 beginGroup(self, obj):
parent, layout = self.__get_immediate_parent()
panel = QGroupBox(obj.name, parent)
if obj.layout == "grid":
l = QGridLayout()
elif obj.layout == "vertical":
l = QVBoxLayout()
else:
l = QHBoxLayout()
self.__increase_nesting_level(panel, l)
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 __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)
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, updater, config):
super(BoxGroup, self).__init__(updater, config)
self.box = QGroupBox(self.name)
self.box.setLayout(self.grid)
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'
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
def __init__(self, updater, config):
super(BoxGroup, self).__init__(updater, config)
self.box = QGroupBox(self.name)
self.box.setLayout(self.grid)
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(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 __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(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 __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)
