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 __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 _add_radio_button(self, group, group_name, name):
     radio_button = QRadioButton(name)
     radio_button.toggled.connect(
         lambda checked: self.on_radio_button_toggled(
             group_name, name, checked))
     group.layout().addWidget(radio_button)
     return radio_button
Esempio n. 4
0
 def __init__(self,
              nodename,
              masteruri,
              loggername,
              level='INFO',
              parent=None):
     '''
     Creates a new item.
     '''
     QFrame.__init__(self, parent)
     self.setObjectName("LoggerItem")
     self.nodename = nodename
     self.masteruri = masteruri
     self.loggername = loggername
     self.current_level = None
     layout = QHBoxLayout(self)
     layout.setContentsMargins(1, 1, 1, 1)
     self.debug = QRadioButton()
     self.debug.setStyleSheet(
         "QRadioButton{ background-color: #39B54A;}")  # QColor(57, 181, 74)
     self.debug.toggled.connect(self.toggled_debug)
     layout.addWidget(self.debug)
     self.info = QRadioButton()
     self.info.setStyleSheet("QRadioButton{ background-color: #FFFAFA;}")
     self.info.toggled.connect(self.toggled_info)
     layout.addWidget(self.info)
     self.warn = QRadioButton()
     self.warn.setStyleSheet(
         "QRadioButton{ background-color: #FFC706;}")  # QColor(255, 199, 6)
     self.warn.toggled.connect(self.toggled_warn)
     layout.addWidget(self.warn)
     self.error = QRadioButton()
     self.error.setStyleSheet(
         "QRadioButton{ background-color: #DE382B;}")  # QColor(222, 56, 43)
     self.error.toggled.connect(self.toggled_error)
     layout.addWidget(self.error)
     self.fatal = QRadioButton()
     self.fatal.setStyleSheet("QRadioButton{ background-color: #FF0000;}")
     self.fatal.toggled.connect(self.toggled_fatal)
     layout.addWidget(self.fatal)
     self.label = QLabel(loggername)
     layout.addWidget(self.label)
     layout.addStretch()
     self._callback = None  # used to set all logger
     self.success_signal.connect(self.on_succes_update)
     self.error_signal.connect(self.on_error_update)
     self.set_level(level)
Esempio n. 5
0
    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']))
Esempio n. 6
0
class LoggerItem(QFrame):
    '''
    Represents one ROS logger and offers methods to change the logger level.
    '''

    success_signal = Signal(str)
    error_signal = Signal(str)

    def __init__(self,
                 nodename,
                 masteruri,
                 loggername,
                 level='INFO',
                 parent=None):
        '''
        Creates a new item.
        '''
        QFrame.__init__(self, parent)
        self.setObjectName("LoggerItem")
        self.nodename = nodename
        self.masteruri = masteruri
        self.loggername = loggername
        self.current_level = None
        layout = QHBoxLayout(self)
        layout.setContentsMargins(1, 1, 1, 1)
        self.debug = QRadioButton()
        self.debug.setStyleSheet(
            "QRadioButton{ background-color: #39B54A;}")  # QColor(57, 181, 74)
        self.debug.toggled.connect(self.toggled_debug)
        layout.addWidget(self.debug)
        self.info = QRadioButton()
        self.info.setStyleSheet("QRadioButton{ background-color: #FFFAFA;}")
        self.info.toggled.connect(self.toggled_info)
        layout.addWidget(self.info)
        self.warn = QRadioButton()
        self.warn.setStyleSheet(
            "QRadioButton{ background-color: #FFC706;}")  # QColor(255, 199, 6)
        self.warn.toggled.connect(self.toggled_warn)
        layout.addWidget(self.warn)
        self.error = QRadioButton()
        self.error.setStyleSheet(
            "QRadioButton{ background-color: #DE382B;}")  # QColor(222, 56, 43)
        self.error.toggled.connect(self.toggled_error)
        layout.addWidget(self.error)
        self.fatal = QRadioButton()
        self.fatal.setStyleSheet("QRadioButton{ background-color: #FF0000;}")
        self.fatal.toggled.connect(self.toggled_fatal)
        layout.addWidget(self.fatal)
        self.label = QLabel(loggername)
        layout.addWidget(self.label)
        layout.addStretch()
        self._callback = None  # used to set all logger
        self.success_signal.connect(self.on_succes_update)
        self.error_signal.connect(self.on_error_update)
        self.set_level(level)

    def set_callback(self, callback):
        self._callback = callback

    def toggled_debug(self, state):
        if state:
            self.set_level('DEBUG')

    def toggled_info(self, state):
        if state:
            self.set_level('INFO')

    def toggled_warn(self, state):
        if state:
            self.set_level('WARN')

    def toggled_error(self, state):
        if state:
            self.set_level('ERROR')

    def toggled_fatal(self, state):
        if state:
            self.set_level('FATAL')

    def on_succes_update(self, level):
        if level.upper() == 'DEBUG':
            self.debug.setChecked(True)
        elif level.upper() == 'INFO':
            self.info.setChecked(True)
        elif level.upper() == 'WARN':
            self.warn.setChecked(True)
        elif level.upper() == 'ERROR':
            self.error.setChecked(True)
        elif level.upper() == 'FATAL':
            self.fatal.setChecked(True)
        elif level:
            rospy.logwarn("loglevel not found '%s'" % (level))
            return
        self.current_level = level

    def on_error_update(self, level):
        self.on_succes_update(level)

    def set_level(self, level):
        if self.current_level is not None:
            if self._callback is not None:
                self._callback(level)
            else:
                # call set loglevel service
                thread = threading.Thread(target=self._set_level,
                                          kwargs={
                                              'level': level,
                                              'current_level':
                                              self.current_level
                                          })
                thread.setDaemon(True)
                thread.start()
                pass
        else:
            self.on_succes_update(level)

    def _set_level(self, level, current_level):
        try:
            backup_level = current_level
            service_name = '%s/set_logger_level' % self.nodename
            # get service URI from ROS-Master
            master = xmlrpcclient.ServerProxy(self.masteruri)
            code, _, serviceuri = master.lookupService(rospy.get_name(),
                                                       service_name)
            if code == 1:
                self.call_service_set_level(serviceuri, service_name,
                                            self.loggername, level)
                self.success_signal.emit(level)
        except rospy.ServiceException as e:
            rospy.logwarn("Set logger %s for %s to %s failed: %s" %
                          (self.loggername, self.nodename, level, e))
            if backup_level is not None:
                self.error_signal.emit(backup_level)

    @classmethod
    def call_service_set_level(cls, serviceuri, servicename, loggername,
                               level):
        _req, _resp = nm.starter().callService(
            serviceuri,
            servicename,
            SetLoggerLevel,
            service_args=[loggername, level])
    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 buildRadioButtons(self, 
                          labelTextArray,
                          orientation,
                          alignment,
                          activeButtons=None, 
                          behavior=CheckboxGroupBehavior.RADIO_BUTTONS):
        '''
        @param labelTextArray: Names of buttons
        @type labelTextArray: [string]
        @param orientation: Whether to arrange the buttons vertically or horizontally.
        @type orientation: Orientation
        @param alignment: whether buttons should be aligned Left/Center/Right for horizontal,
                          Top/Center/Bottom for vertical:/c
        @type  alignment: Alignment
        @param activeButtons: Name of the buttons that is to be checked initially. Or None.
        @type activeButtons: [string]
        @param behavior: Indicates whether the button group is to behave like Radio Buttons, or like Checkboxes.
        @type behavior: CheckboxGroupBehavior
        @return 
            1. The button group that contains the related buttons. Caller: ensure that 
               this object does not go out of scope. 
            2. The button layout, which callers will need to add to
               their own layouts.
            3. and a dictionary mapping button names to button objects that
               were created within this method. This dict is needed by the
               controller.
        @rtype (QButtonGroup, QLayout, dict<string,QRadioButton>).
        '''
        
        # Button control management group. It has no visible
        # representation. It allows the radio button bahavior, for instance:
        buttonGroup = QButtonGroup();
        
        if behavior == CheckboxGroupBehavior.CHECKBOXES:
            buttonGroup.setExclusive(False);
        else:
            buttonGroup.setExclusive(True);
        
        if orientation == Orientation.VERTICAL:
            buttonCompLayout = QVBoxLayout();
            # Arrange for the alignment (Part 1):
            if (alignment == Alignment.CENTER) or\
               (alignment == Alignment.BOTTOM):
                buttonCompLayout.addStretch(1);
        else:
            buttonCompLayout = QHBoxLayout();
            # Arrange for the alignment (Part 1):
            if (alignment == Alignment.CENTER) or\
               (alignment == Alignment.RIGHT):
                buttonCompLayout.addStretch(1);
            
        # Build the buttons:
        buttonDict  = {};
        
        for label in labelTextArray:
            button = QRadioButton(label);
            buttonDict[label] = button;
            # Add button to the button management group:
            buttonGroup.addButton(button);
            # Add the button to the visual group:
            buttonCompLayout.addWidget(button);
            if label in activeButtons:
                button.setChecked(True);

        if orientation == Orientation.VERTICAL:
            buttonCompLayout = QVBoxLayout();
            # Arrange for the alignment (Part 2):
            if (alignment == Alignment.CENTER) or\
               (alignment == Alignment.TOP):
                buttonCompLayout.addStretch(1);
        else:
            # Arrange for the alignment (Part 2):
            if (alignment == Alignment.CENTER) or\
               (alignment == Alignment.LEFT):
                buttonCompLayout.addStretch(1);
                        
        return (buttonGroup, buttonCompLayout, buttonDict);
    def __init__(self, context):
        super(NoLimitJointControlDialog, self).__init__(context)
        self.setObjectName('NoLimitJointControlDialog')
        self.updateStateSignal.connect(self.on_updateState)
        self.updateGhostSignal.connect(self.on_updateGhost)

        self.joint_states        = JointState()
        self.ghost_joint_states  = JointState()
        self._widget = QWidget()
        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.radioGroup.addButton(self.radio_ghost_target,0)
        self.radio_ghost_target.setChecked(True)
        self.radio_robot_target = QRadioButton()
        self.radio_robot_target.setText("Robot")
        self.radioGroup.addButton(self.radio_robot_target,1)
        hbox_radio.addStretch()
        hbox_radio.addWidget(self.radio_ghost_target)
        #hbox_radio.addWidget(QLabel("Ghost"))
        hbox_radio.addStretch()
        hbox_radio.addWidget(self.radio_robot_target)
        #hbox_radio.addWidget(QLabel("Robot"))
        hbox_radio.addStretch()
        radios.setLayout(hbox_radio)

        vbox.addWidget(radios)

        widget = QWidget()
        hbox = QHBoxLayout()

        # Left to right layout
        self.joint_control = NoLimitJointControl(self, roslib.packages.get_pkg_dir('vigir_rqt_no_limit_joint_control') + '/launch/joints.txt',hbox)

        widget.setLayout(hbox)

        vbox.addWidget(widget)

        print "Add buttons to apply all ..."
        all_widget = QWidget()
        all_box = QHBoxLayout()

        self.snap_to_ghost_button = QPushButton("SnapAllGhost")
        self.snap_to_ghost_button.pressed.connect(self.on_snapGhostPressed)
        all_box.addWidget(self.snap_to_ghost_button)
        self.snap_to_current_button = QPushButton("SnapAllCurrent")
        self.snap_to_current_button.pressed.connect(self.on_snapCurrentPressed)
        all_box.addWidget(self.snap_to_current_button)
        self.apply_to_robot_button = QPushButton("ApplyAllRobot")
        self.apply_to_robot_button.pressed.connect(self.on_applyRobotPressed)
        all_box.addWidget(self.apply_to_robot_button)
        self.apply_to_robot_button = QPushButton("Apply WBC Robot")
        self.apply_to_robot_button.pressed.connect(self.on_applyWBCRobotPressed)
        all_box.addWidget(self.apply_to_robot_button)

        all_widget.setLayout(all_box)

        vbox.addWidget(all_widget)

#        all_hbox = QHBoxLayout()
#        all_hbox.addStretch()
#        all_hbox.addWidget(all_widget)
#        all_hbox.addStretch()
#        bottom_widget=QWidget()
#        bottom_widget.setLayout(all_jbox)
#        vbox.addWidget(bottom_widget)

        vbox.addStretch()

        self._widget.setLayout(vbox)
        context.add_widget(self._widget)

        self.first_time = True

        self.stateSubscriber  = rospy.Subscriber('/atlas/joint_states', JointState, self.stateCallbackFnc)
        self.ghostSubscriber  = rospy.Subscriber('/flor/ghost/get_joint_states', JointState, self.ghostCallbackFnc)
        self.wbc_robot_pub    = rospy.Publisher('/flor/wbc_controller/joint_states',JointState, queue_size=10)
class NoLimitJointControlDialog(Plugin):
    updateStateSignal = Signal(object)
    updateGhostSignal = Signal(object)

    def __init__(self, context):
        super(NoLimitJointControlDialog, self).__init__(context)
        self.setObjectName('NoLimitJointControlDialog')
        self.updateStateSignal.connect(self.on_updateState)
        self.updateGhostSignal.connect(self.on_updateGhost)

        self.joint_states        = JointState()
        self.ghost_joint_states  = JointState()
        self._widget = QWidget()
        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.radioGroup.addButton(self.radio_ghost_target,0)
        self.radio_ghost_target.setChecked(True)
        self.radio_robot_target = QRadioButton()
        self.radio_robot_target.setText("Robot")
        self.radioGroup.addButton(self.radio_robot_target,1)
        hbox_radio.addStretch()
        hbox_radio.addWidget(self.radio_ghost_target)
        #hbox_radio.addWidget(QLabel("Ghost"))
        hbox_radio.addStretch()
        hbox_radio.addWidget(self.radio_robot_target)
        #hbox_radio.addWidget(QLabel("Robot"))
        hbox_radio.addStretch()
        radios.setLayout(hbox_radio)

        vbox.addWidget(radios)

        widget = QWidget()
        hbox = QHBoxLayout()

        # Left to right layout
        self.joint_control = NoLimitJointControl(self, roslib.packages.get_pkg_dir('vigir_rqt_no_limit_joint_control') + '/launch/joints.txt',hbox)

        widget.setLayout(hbox)

        vbox.addWidget(widget)

        print "Add buttons to apply all ..."
        all_widget = QWidget()
        all_box = QHBoxLayout()

        self.snap_to_ghost_button = QPushButton("SnapAllGhost")
        self.snap_to_ghost_button.pressed.connect(self.on_snapGhostPressed)
        all_box.addWidget(self.snap_to_ghost_button)
        self.snap_to_current_button = QPushButton("SnapAllCurrent")
        self.snap_to_current_button.pressed.connect(self.on_snapCurrentPressed)
        all_box.addWidget(self.snap_to_current_button)
        self.apply_to_robot_button = QPushButton("ApplyAllRobot")
        self.apply_to_robot_button.pressed.connect(self.on_applyRobotPressed)
        all_box.addWidget(self.apply_to_robot_button)
        self.apply_to_robot_button = QPushButton("Apply WBC Robot")
        self.apply_to_robot_button.pressed.connect(self.on_applyWBCRobotPressed)
        all_box.addWidget(self.apply_to_robot_button)

        all_widget.setLayout(all_box)

        vbox.addWidget(all_widget)

#        all_hbox = QHBoxLayout()
#        all_hbox.addStretch()
#        all_hbox.addWidget(all_widget)
#        all_hbox.addStretch()
#        bottom_widget=QWidget()
#        bottom_widget.setLayout(all_jbox)
#        vbox.addWidget(bottom_widget)

        vbox.addStretch()

        self._widget.setLayout(vbox)
        context.add_widget(self._widget)

        self.first_time = True

        self.stateSubscriber  = rospy.Subscriber('/atlas/joint_states', JointState, self.stateCallbackFnc)
        self.ghostSubscriber  = rospy.Subscriber('/flor/ghost/get_joint_states', JointState, self.ghostCallbackFnc)
        self.wbc_robot_pub    = rospy.Publisher('/flor/wbc_controller/joint_states',JointState, queue_size=10)





    def shutdown_plugin(self):
    #Just make sure to stop timers and publishers, unsubscribe from Topics etc in the shutdown_plugin method.
        print "Shutdown ..."
        #rospy.sleep(0.1)
        self.stateSubscriber.unregister()
        self.ghostSubscriber.unregister()
        self.wbc_robot_pub.unregister()
        print "Done!"

    def stateCallbackFnc(self, atlasState_msg):
        self.updateStateSignal.emit(atlasState_msg)

    def ghostCallbackFnc(self, ghost_joint_state_msg):
        self.updateGhostSignal.emit(ghost_joint_state_msg)

    def on_snapGhostPressed(self):
        print "Snap all joint values to current ghost joint positions"
        for controller in self.joint_control.controllers:
            controller.on_snapGhostPressed()

    def on_snapCurrentPressed(self):
        print "Snap all joint values to current joint positions"
        for controller in self.joint_control.controllers:
            controller.on_snapCurrentPressed()

    def on_applyRobotPressed(self):
        print "Send all latest joint values directly to robot"
        for controller in self.joint_control.controllers:
            controller.on_applyRobotPressed()

    def on_applyWBCRobotPressed(self):
        print "Send all latest joint values directly to robot as a WBC command"
        if self.first_time:
            print "Uninitialized !"
        else:
            joint_state = JointState()
            joint_state.header.stamp = rospy.Time.now()
            joint_state.name     =  copy.deepcopy(self.joint_states.name)
            joint_state.position =  list(copy.deepcopy(self.joint_states.position))
            joint_state.velocity =  list(copy.deepcopy(self.joint_states.velocity))

            #print "Positions: ",joint_state.position
            for i,name in enumerate(joint_state.name):
                # for each joint, retrieve the data from each controller to populate the WBC vectors
                for controller in self.joint_control.controllers:
                    for joint in controller.joints:
                        if joint.name == self.joint_states.name[i]:
                            print "     fetching value for "+name+" = "+str(joint.position)+"  at ndx="+str(i)
                            joint_state.position[i] = joint.position
                            joint_state.velocity[i] = joint.velocity
                            #joint_state.effort[i]   = joint.effort

            self.wbc_robot_pub.publish(joint_state)


    # this runs in the Qt GUI thread and can therefore update the GUI
    def on_updateState(self, atlasState_msg):
        self.joint_states = atlasState_msg
        self.joint_control.updateJointPositions(self.joint_states,self.first_time)
        if self.first_time:
            self.joint_control.resetCurrentJointSliders()
            self.first_time = False
            self.ghost_joint_states = copy.deepcopy(atlasState_msg);
            self.ghost_joint_states.position = list(self.ghost_joint_states.position)
            #print "Initial Ghost Stored:",self.ghost_joint_states

    # this runs in the Qt GUI thread and can therefore update the GUI
    def on_updateGhost(self, ghost_joint_state_msg):
        for i, new_joint_name in enumerate(list(ghost_joint_state_msg.name)):
            name_found = False
            for j,stored_joint_name in enumerate(self.ghost_joint_states.name):
                if (new_joint_name == stored_joint_name):
                    name_found = True
                    #print "ghost_joint_msg[",str(i),"]=",ghost_joint_state_msg.position[i]
                    #print "ghost_joint_states[",str(j),"]=",self.ghost_joint_states.position[i]
                    self.ghost_joint_states.position[j] = ghost_joint_state_msg.position[i]
                    #self.ghost_joint_states.velocity[j] = ghost_joint_state_msg.velocity[i]
            if (not name_found):
                # Update the list without regard to order
                self.ghost_joint_states.name.append(new_joint_name)
                self.ghost_joint_states.position.append(ghost_joint_state_msg.position[i])
Esempio n. 11
0
    def __init__(self, context):
        super(JointControlWidget, self).__init__()
        self.updateStateSignal.connect(self.on_update_state)
        self.updateGhostSignal.connect(self.on_update_ghost)

        self.joint_states = JointState()
        self.ghost_joint_states = JointState()
        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.radioGroup.addButton(self.radio_ghost_target, 0)
        self.radio_ghost_target.setChecked(True)
        self.radio_robot_target = QRadioButton()
        self.radio_robot_target.setText("Robot")
        self.radioGroup.addButton(self.radio_robot_target, 1)
        hbox_radio.addStretch()
        hbox_radio.addWidget(self.radio_ghost_target)
        hbox_radio.addStretch()
        hbox_radio.addWidget(self.radio_robot_target)
        hbox_radio.addStretch()
        radios.setLayout(hbox_radio)
        vbox.addWidget(radios)

        duration_box = QHBoxLayout()
        duration_box.setAlignment(Qt.AlignLeft)
        duration_box.addWidget(QLabel("Trajectory duration (s):"))
        self.traj_duration_spin = QDoubleSpinBox()
        self.traj_duration_spin.setValue(1.0)
        self.traj_duration_spin.valueChanged.connect(self.on_traj_duration_changed)
        duration_box.addWidget(self.traj_duration_spin)
        self.update_controllers_buttonn = QPushButton("Update Controllers")
        self.update_controllers_buttonn.pressed.connect(self.on_update_controllers)
        duration_box.addWidget(self.update_controllers_buttonn)
        vbox.addLayout(duration_box)

        widget = QWidget()
        hbox = QHBoxLayout()


        # Left to right layout
        self.joint_control = JointControl(self, hbox)

        widget.setLayout(hbox)

        vbox.addWidget(widget)

        print "Add buttons to apply all ..."
        all_widget = QWidget()
        all_box = QHBoxLayout()

        self.snap_to_ghost_button = QPushButton("SnapAllGhost")
        self.snap_to_ghost_button.pressed.connect(self.on_snap_ghost_pressed)
        all_box.addWidget(self.snap_to_ghost_button)
        self.snap_to_current_button = QPushButton("SnapAllCurrent")
        self.snap_to_current_button.pressed.connect(self.on_snap_current_pressed)
        all_box.addWidget(self.snap_to_current_button)
        self.apply_to_robot_button = QPushButton("ApplyAllRobot")
        self.apply_to_robot_button.pressed.connect(self.on_apply_robot_pressed)
        all_box.addWidget(self.apply_to_robot_button)
        self.apply_to_robot_button = QPushButton("Apply WBC Robot")
        self.apply_to_robot_button.pressed.connect(self.on_apply_wbc_robot_pressed)
        all_box.addWidget(self.apply_to_robot_button)

        all_widget.setLayout(all_box)
        vbox.addWidget(all_widget)

        override_box = QHBoxLayout()

        self.override = QCheckBox()
        self.override.setChecked(False)
        self.override.stateChanged.connect(self.on_override_changed)
        override_box.addWidget(self.override)

        override_label = QLabel("SAFETY OVERRIDE")
        override_label.setStyleSheet('QLabel { color: red }')

        override_box.addWidget(override_label)

        override_box.addStretch()

        vbox.addLayout(override_box)

        vbox.addStretch()

        self._widget.setLayout(vbox)

        self.first_time = True

        self.stateSubscriber = rospy.Subscriber('/joint_states', JointState, self.state_callback_fnc)
        self.ghostSubscriber = rospy.Subscriber('/flor/ghost/get_joint_states', JointState, self.ghost_callback_fnc)
        self.wbc_robot_pub = rospy.Publisher('/flor/wbc_controller/joint_states', JointState, queue_size=10)

        self.time_last_update_state = time.time()
        self.time_last_update_ghost = time.time()
Esempio n. 12
0
class JointControlWidget(QObject):
    updateStateSignal = Signal(object)
    updateGhostSignal = Signal(object)

    def __init__(self, context):
        super(JointControlWidget, self).__init__()
        self.updateStateSignal.connect(self.on_update_state)
        self.updateGhostSignal.connect(self.on_update_ghost)

        self.joint_states = JointState()
        self.ghost_joint_states = JointState()
        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.radioGroup.addButton(self.radio_ghost_target, 0)
        self.radio_ghost_target.setChecked(True)
        self.radio_robot_target = QRadioButton()
        self.radio_robot_target.setText("Robot")
        self.radioGroup.addButton(self.radio_robot_target, 1)
        hbox_radio.addStretch()
        hbox_radio.addWidget(self.radio_ghost_target)
        hbox_radio.addStretch()
        hbox_radio.addWidget(self.radio_robot_target)
        hbox_radio.addStretch()
        radios.setLayout(hbox_radio)
        vbox.addWidget(radios)

        duration_box = QHBoxLayout()
        duration_box.setAlignment(Qt.AlignLeft)
        duration_box.addWidget(QLabel("Trajectory duration (s):"))
        self.traj_duration_spin = QDoubleSpinBox()
        self.traj_duration_spin.setValue(1.0)
        self.traj_duration_spin.valueChanged.connect(self.on_traj_duration_changed)
        duration_box.addWidget(self.traj_duration_spin)
        self.update_controllers_buttonn = QPushButton("Update Controllers")
        self.update_controllers_buttonn.pressed.connect(self.on_update_controllers)
        duration_box.addWidget(self.update_controllers_buttonn)
        vbox.addLayout(duration_box)

        widget = QWidget()
        hbox = QHBoxLayout()


        # Left to right layout
        self.joint_control = JointControl(self, hbox)

        widget.setLayout(hbox)

        vbox.addWidget(widget)

        print "Add buttons to apply all ..."
        all_widget = QWidget()
        all_box = QHBoxLayout()

        self.snap_to_ghost_button = QPushButton("SnapAllGhost")
        self.snap_to_ghost_button.pressed.connect(self.on_snap_ghost_pressed)
        all_box.addWidget(self.snap_to_ghost_button)
        self.snap_to_current_button = QPushButton("SnapAllCurrent")
        self.snap_to_current_button.pressed.connect(self.on_snap_current_pressed)
        all_box.addWidget(self.snap_to_current_button)
        self.apply_to_robot_button = QPushButton("ApplyAllRobot")
        self.apply_to_robot_button.pressed.connect(self.on_apply_robot_pressed)
        all_box.addWidget(self.apply_to_robot_button)
        self.apply_to_robot_button = QPushButton("Apply WBC Robot")
        self.apply_to_robot_button.pressed.connect(self.on_apply_wbc_robot_pressed)
        all_box.addWidget(self.apply_to_robot_button)

        all_widget.setLayout(all_box)
        vbox.addWidget(all_widget)

        override_box = QHBoxLayout()

        self.override = QCheckBox()
        self.override.setChecked(False)
        self.override.stateChanged.connect(self.on_override_changed)
        override_box.addWidget(self.override)

        override_label = QLabel("SAFETY OVERRIDE")
        override_label.setStyleSheet('QLabel { color: red }')

        override_box.addWidget(override_label)

        override_box.addStretch()

        vbox.addLayout(override_box)

        vbox.addStretch()

        self._widget.setLayout(vbox)

        self.first_time = True

        self.stateSubscriber = rospy.Subscriber('/joint_states', JointState, self.state_callback_fnc)
        self.ghostSubscriber = rospy.Subscriber('/flor/ghost/get_joint_states', JointState, self.ghost_callback_fnc)
        self.wbc_robot_pub = rospy.Publisher('/flor/wbc_controller/joint_states', JointState, queue_size=10)

        self.time_last_update_state = time.time()
        self.time_last_update_ghost = time.time()

    def shutdown_plugin(self):
        # Just make sure to stop timers and publishers, unsubscribe from Topics etc in the shutdown_plugin method.
        print "Shutdown ..."
        self.stateSubscriber.unregister()
        self.ghostSubscriber.unregister()
        self.wbc_robot_pub.unregister()
        print "Done!"

    def state_callback_fnc(self, atlas_state_msg):
        self.updateStateSignal.emit(atlas_state_msg)

    def command_callback_fnc(self, atlas_command_msg):
        self.updateCommandSignal.emit(atlas_command_msg)

    def ghost_callback_fnc(self, ghost_joint_state_msg):
        self.updateGhostSignal.emit(ghost_joint_state_msg)

    def on_update_controllers(self):
        self.joint_control.update_controllers()

    def on_override_changed(self):
        if self.override.isChecked():
            print "WARNING: TURNING OFF SAFETY"
            for controller in self.joint_control.controllers:
                controller.set_override(True)
        else:
            print "Enabling safety checks"
            for controller in self.joint_control.controllers:
                controller.set_override(False)

    def on_snap_ghost_pressed(self):
        print "Snap all joint values to current ghost joint positions"
        for controller in self.joint_control.controllers:
            controller.on_snap_ghost_pressed()

    def on_snap_current_pressed(self):
        print "Snap all joint values to current joint positions"
        for controller in self.joint_control.controllers:
            controller.on_snap_current_pressed()

    def on_apply_robot_pressed(self):
        print "Send all latest joint values directly to robot"
        for controller in self.joint_control.controllers:
            controller.on_apply_robot_pressed()

    def on_traj_duration_changed(self, duration):
        self.joint_control.update_traj_duration(duration)

    def on_apply_wbc_robot_pressed(self):
        print "Send all latest joint values directly to robot as a WBC command"
        if self.first_time:
            print "Uninitialized !"
        else:
            joint_state = JointState()
            joint_state.header.stamp = rospy.Time.now()
            joint_state.name = copy.deepcopy(self.joint_states.name)
            joint_state.position = list(copy.deepcopy(self.joint_states.position))
            joint_state.velocity = list(copy.deepcopy(self.joint_states.velocity))

            # print "Positions: ",joint_state.position
            for i, name in enumerate(joint_state.name):
                # for each joint, retrieve the data from each controller to populate the WBC vectors
                for controller in self.joint_control.controllers:
                    for joint in controller.joints:
                        if joint.name == self.joint_states.name[i]:
                            print "     fetching value for " + name + " = " + str(joint.position) + "  at ndx=" + str(i)
                            joint_state.position[i] = joint.position
                            joint_state.velocity[i] = joint.velocity
                            # joint_state.effort[i]   = joint.effort

            self.wbc_robot_pub.publish(joint_state)

    def on_update_state(self, atlas_state_msg):
        if time.time() - self.time_last_update_state >= 0.2:
            self.joint_states = atlas_state_msg
            self.joint_control.update_joint_positions(self.joint_states)
            if self.first_time:
                self.joint_control.reset_current_joint_sliders()
                self.first_time = False
                self.ghost_joint_states = copy.deepcopy(atlas_state_msg)
                self.ghost_joint_states.position = list(self.ghost_joint_states.position)
            self.time_last_update_state = time.time()

    # this runs in the Qt GUI thread and can therefore update the GUI
    def on_update_ghost(self, ghost_joint_state_msg):
        if time.time() - self.time_last_update_ghost >= 0.2:
            for i, new_joint_name in enumerate(list(ghost_joint_state_msg.name)):
                name_found = False
                for j, stored_joint_name in enumerate(self.ghost_joint_states.name):
                    if new_joint_name == stored_joint_name:
                        name_found = True
                        self.ghost_joint_states.position[j] = ghost_joint_state_msg.position[i]
                if not name_found:
                    # Update the list without regard to order
                    self.ghost_joint_states.name.append(new_joint_name)
                    self.ghost_joint_states.position.append(ghost_joint_state_msg.position[i])
            self.time_last_update_ghost = time.time()