class ExclusiveOptionGroup(QGroupBox):
"""
Creates a button group of exclusive radio options.
Options must be a dict with following keys: 'enabled','selected','title','description','tooltip'
"""
def __init__(self, options, title='Exclusive Options', selected_index=None, parent=None):
super(ExclusiveOptionGroup, self).__init__()
self.setTitle(title)
self.setLayout(QVBoxLayout())
self._button_group = QButtonGroup()
self._button_group.setExclusive(True)
self._options = options
if parent == None:
parent = self
for (button_id, option) in enumerate(self._options):
radio_button = QRadioButton(option.get('title', 'option %d' % button_id))
radio_button.setEnabled(option.get('enabled', True))
radio_button.setChecked(option.get('selected', False) or button_id == selected_index)
radio_button.setToolTip(option.get('tooltip', ''))
self._button_group.addButton(radio_button, button_id)
parent.layout().addWidget(radio_button)
if 'description' in option:
parent.layout().addWidget(QLabel(option['description']))
def get_settings(self):
"""Returns dictionary with selected_index (int) and selected_option (dict) keys."""
selected_index = self._button_group.checkedId()
if selected_index >= 0:
return {'selected_index': selected_index, 'selected_option': self._options[selected_index]}
return {'selected_index': None, 'selected_option': None}
class ExclusiveOptionGroup(QGroupBox):
"""
Creates a button group of exclusive radio options.
Options must be a dict with following keys: 'enabled','selected','title','description','tooltip'
"""
def __init__(self,
options,
title='Exclusive Options',
selected_index=None,
parent=None):
super(ExclusiveOptionGroup, self).__init__()
self.setTitle(title)
self.setLayout(QVBoxLayout())
self._button_group = QButtonGroup()
self._button_group.setExclusive(True)
self._options = options
if parent == None:
parent = self
for (button_id, option) in enumerate(self._options):
radio_button = QRadioButton(
option.get('title', 'option %d' % button_id))
radio_button.setEnabled(option.get('enabled', True))
radio_button.setChecked(
option.get('selected', False) or button_id == selected_index)
radio_button.setToolTip(option.get('tooltip', ''))
self._button_group.addButton(radio_button, button_id)
parent.layout().addWidget(radio_button)
if 'description' in option:
parent.layout().addWidget(QLabel(option['description']))
def get_settings(self):
"""Returns dictionary with selected_index (int) and selected_option (dict) keys."""
selected_index = self._button_group.checkedId()
if selected_index >= 0:
return {
'selected_index': selected_index,
'selected_option': self._options[selected_index]
}
return {'selected_index': None, 'selected_option': None}
class ExclusiveOptionGroup(QGroupBox):
def __init__(self,
options,
title='Exclusive Options',
selected_index=None):
super(ExclusiveOptionGroup, self).__init__()
self.setTitle(title)
self.setLayout(QVBoxLayout())
self._button_group = QButtonGroup()
self._button_group.setExclusive(True)
self._options = options
button_id = 0
for option in self._options:
button_id += 1
radio_button = QRadioButton(
option.get('title', 'option %d' % button_id))
radio_button.setEnabled(option.get('enabled', True))
radio_button.setChecked(
option.get('selected', False)
or (button_id - 1) == selected_index)
radio_button.setToolTip(option.get('tooltip', ''))
widget = QWidget()
widget.setLayout(QVBoxLayout())
widget.layout().addWidget(radio_button)
if 'description' in option:
widget.layout().addWidget(QLabel(option['description']))
self._button_group.addButton(radio_button, button_id)
self.layout().addWidget(widget)
def get_settings(self):
selected_index = self._button_group.checkedId() - 1
if selected_index >= 0:
return {
'selected_index': selected_index,
'selected_option': self._options[selected_index]
}
return {'selected_index': None, 'selected_option': None}
class PositionModeWidget(QObject):
updateStateSignal = Signal(object)
updatePelvisSignal = Signal(object)
updateGhostSignal = Signal(object)
def __init__(self, context):
#super(PositionModeDialog, self).__init__(context)
#self.setObjectName('PositionModeDialog')
super(PositionModeWidget, self).__init__()
self.name = 'PositionModeWidget'
self.updateStateSignal.connect(self.on_updateState)
self.updatePelvisSignal.connect(self.on_updatePelvis)
self.updateGhostSignal.connect(self.on_updateGhost)
## Process standalone plugin command-line arguments
#from argparse import ArgumentParser
#parser = ArgumentParser()
## Add argument(s) to the parser.
#parser.add_argument("-f", "--file",
# dest="file",
# help="Input file name")
#args, unknowns = parser.parse_known_args(context.argv())
#print 'arguments: ', args
#print 'unknowns: ', unknowns
#
#if ((args.file == "") or (args.file == None)):
# print "No file specified - abort!"
# sys.exit(-1)
# Define how long the trajectories will take to execute
self.time_factor = rospy.get_param('~time_factor', 2.1)
print "Using ",self.time_factor, " as the time factor for all trajectories"
# Joint Name : Child Link :OSRF
self.joint_names=[]
self.joint_names.append('back_bkz') # : ltorso : 0
self.joint_names.append('back_bky') # : mtorso : 1
self.joint_names.append('back_bkx') # : utorso : 2
self.joint_names.append('neck_ry') # : head : 3
self.joint_names.append('l_leg_hpz') # : l_uglut : 4
self.joint_names.append('l_leg_hpx') # : l_lglut : 5
self.joint_names.append('l_leg_hpy') # : l_uleg : 6
self.joint_names.append('l_leg_kny') # : l_lleg : 7
self.joint_names.append('l_leg_aky') # : l_talus : 8
self.joint_names.append('l_leg_akx') # : l_foot : 9
self.joint_names.append('r_leg_hpz') # : r_uglut : 10
self.joint_names.append('r_leg_hpx') # : r_lglut : 11
self.joint_names.append('r_leg_hpy') # : r_uleg : 12
self.joint_names.append('r_leg_kny') # : r_lleg : 13
self.joint_names.append('r_leg_aky') # : r_talus : 14
self.joint_names.append('r_leg_akx') # : r_foot : 15
self.joint_names.append('l_arm_shz') # : l_clav : 16
self.joint_names.append('l_arm_shx') # : l_scap : 17
self.joint_names.append('l_arm_ely') # : l_uarm : 18
self.joint_names.append('l_arm_elx') # : l_larm : 19
self.joint_names.append('l_arm_wry') # : l_farm : 20
self.joint_names.append('l_arm_wrx') # : l_hand : 21
self.joint_names.append('l_arm_wry2') # : l_farm : 22
self.joint_names.append('r_arm_shz') # : r_clav : 23
self.joint_names.append('r_arm_shx') # : r_scap : 24
self.joint_names.append('r_arm_ely') # : r_uarm : 25
self.joint_names.append('r_arm_elx') # : r_larm : 26
self.joint_names.append('r_arm_wry') # : r_farm : 27
self.joint_names.append('r_arm_wrx') # : r_hand : 28
self.joint_names.append('r_arm_wry2') # : r_farm : 29
self.joint_states = JointState()
self.pelvis_names=[]
self.joint_names.append('forward') # : 30
self.joint_names.append('lateral') # : 31
self.joint_names.append('height') # : 32
self.joint_names.append('roll') # : 33
self.joint_names.append('pitch') # : 34
self.joint_names.append('yaw') # : 35
self.pelvis_states = JointState()
self.pelvis_states.position = [0.0, 0.0, 0.85, 0.0, 0.0, 0.0] # default pelvis pose
#self._widget = QWidget()
self._widget = context
vbox = QVBoxLayout()
# Define checkboxes
radios = QWidget();
hbox_radio = QHBoxLayout()
self.radioGroup = QButtonGroup()
self.radioGroup.setExclusive(True)
self.radio_ghost_target = QRadioButton()
self.radio_ghost_target.setText("Ghost")
self.radio_robot_target = QRadioButton()
self.radio_robot_target.setText("Robot")
self.radioGroup.addButton(self.radio_ghost_target,0)
self.radioGroup.addButton(self.radio_robot_target,1)
self.radio_ghost_target.setChecked(True)
self.commandGroup = QButtonGroup()
self.commandGroup.setExclusive(True)
self.radio_position_command = QRadioButton()
self.radio_position_command.setText("Position")
self.commandGroup.addButton(self.radio_position_command,0)
self.radio_trajectory_command = QRadioButton()
self.radio_trajectory_command.setText("Trajectory")
self.commandGroup.addButton(self.radio_trajectory_command,1)
self.radio_trajectory_command.setChecked(True)
#print "position button is checked: ",self.radio_position_command.isChecked()
#print "trajectory button is checked: ",self.radio_trajectory_command.isChecked()
print "Default group button checked is ",self.radioGroup.checkedId()
print "Default command button checked is ",self.commandGroup.checkedId()
hbox_radio.addStretch()
hbox_radio.addWidget(self.radio_ghost_target)
#hbox_radio.addWidget(QLabel("Ghost"))
hbox_radio.addStretch()
vbox_robot = QVBoxLayout()
widget_robot = QWidget()
widget_robot_sel = QWidget()
hbox_sel = QHBoxLayout()
hbox_radio.addStretch()
hbox_sel.addWidget(self.radio_robot_target)
#hbox_sel.addWidget(QLabel("Robot"))
hbox_radio.addStretch()
widget_robot_sel.setLayout(hbox_sel)
vbox_robot.addWidget(widget_robot_sel);
widget_cmd = QWidget()
hbox_cmd = QHBoxLayout()
hbox_radio.addStretch()
hbox_cmd.addWidget(self.radio_position_command)
#hbox_cmd.addWidget(QLabel("Position"))
hbox_radio.addStretch()
hbox_cmd.addWidget(self.radio_trajectory_command)
#hbox_cmd.addWidget(QLabel("Trajectory"))
hbox_radio.addStretch()
widget_cmd.setLayout(hbox_cmd)
vbox_robot.addWidget(widget_cmd);
widget_robot.setLayout(vbox_robot)
hbox_radio.addWidget(widget_robot)
radios.setLayout(hbox_radio)
vbox.addWidget(radios)
positions_widget = QWidget()
hbox = QHBoxLayout()
#self.robot = URDF.from_parameter_server()
self.chains=[]
# Subscribe to Joint States update /atlas/atlas_state
#print "Load parameters"
#print rospy.get_param_names()
#fileName = rospy.get_param('positions_file', '/opt/vigir/catkin_ws/src/vigir_ocs_eui/vigir_rqt/vigir_rqt_position_mode/launch/r_arm_positions.txt')
#print "FileName:",fileName
# Left to right layout
numFiles = rospy.get_param("~num_files");
for i in range(1,numFiles+1):
path = "~position_files/file_" + str(i)
foobar = str(rospy.get_param(path))
self.chains.append(ChainData(self, foobar, hbox))
#add stretch at end so all GUI elements are at left of dialog
hbox.addStretch(1)
positions_widget.setLayout(hbox)
vbox.addWidget(positions_widget)
vbox.addStretch(1)
self._widget.setLayout(vbox)
#context.add_widget(self._widget)
self.stateSubscriber = rospy.Subscriber('/atlas/joint_states', JointState, self.stateCallbackFnc)
self.ghostSubscriber = rospy.Subscriber('/flor/ghost/get_joint_states', JointState, self.ghostCallbackFnc)
self.pelvisSubscriber = rospy.Subscriber('/robot_controllers/pelvis_traj_controller/current_states',JointState, self.pelvisCallbackFnc)
def shutdown_plugin(self):
#Just make sure to stop timers and publishers, unsubscribe from Topics etc in the shutdown_plugin method.
#TODO, is the above comment why the plugin gets stuck when we try to shutdown?
print "Shutdown"
rospy.sleep(0.1)
self.stateSubscriber.unregister()
self.ghostSubscriber.unregister()
self.pelvisSubscriber.unregister()
def stateCallbackFnc(self, jointState_msg):
self.updateStateSignal.emit(jointState_msg)
def on_updateState(self, jointState_msg):
self.joint_states = jointState_msg
def pelvisCallbackFnc(self, jointState_msg):
self.updatePelvisSignal.emit(jointState_msg)
def on_updatePelvis(self, jointState_msg):
self.pelvis_states = jointState_msg;
def ghostCallbackFnc(self, jointState_msg):
self.updateGhostSignal.emit(jointState_msg)
def on_updateGhost(self, ghost_joint_state_msg):
self.ghost_joint_states = ghost_joint_state_msg
