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