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)
Esempio n. 2
0
    def __init__(self,
                 options,
                 title='Checkboxes',
                 selected_indexes=[],
                 parent=None):
        super(CheckBoxGroup, self).__init__()
        self.setTitle(title)
        self.setLayout(QVBoxLayout())
        self._button_group = QButtonGroup()
        self._button_group.setExclusive(False)
        self._options = options
        if parent == None:
            parent = self

        for (button_id, option) in enumerate(self._options):

            checkbox = QCheckBox(option.get('title', 'option %d' % button_id))
            checkbox.setEnabled(option.get('enabled', True))
            checkbox.setChecked(button_id in selected_indexes)
            checkbox.setToolTip(option.get('tooltip', ''))

            self._button_group.addButton(checkbox, button_id)
            parent.layout().addWidget(checkbox)
            if 'description' in option:
                parent.layout().addWidget(QLabel(option['description']))
Esempio n. 3
0
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}
    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 setup_group_frame(self, group):
        layout = QVBoxLayout()

        # grid for buttons for named targets
        grid = QGridLayout()
        grid.setSpacing(1)

        self.button_group = QButtonGroup(self)

        row = 0
        column = 0
        named_targets = self.get_named_targets(group)
        for target in named_targets:
            button = QPushButton(target)
            self.button_group.addButton(button)
            grid.addWidget(button, row, column)
            column += 1
            if column >= self.MAX_COLUMNS:
                row += 1
                column = 0

        self.button_group.buttonClicked.connect(self._handle_button_clicked)

        # grid for show joint value and move arm buttons/text field
        joint_values_grid = QGridLayout()
        joint_values_grid.setSpacing(1)

        button_show_joint_values = QPushButton('Current Joint Values')
        button_show_joint_values.clicked[bool].connect(
            self._handle_show_joint_values_clicked)

        line_edit = QLineEdit()
        self.text_joint_values.append(line_edit)

        button_move_to = QPushButton('Move to')
        button_move_to.clicked[bool].connect(self._handle_move_to_clicked)

        joint_values_grid.addWidget(button_show_joint_values, 0, 1)
        joint_values_grid.addWidget(line_edit, 0, 2)
        joint_values_grid.addWidget(button_move_to, 0, 3)

        layout.addLayout(grid)
        line = QFrame()
        line.setFrameShape(QFrame.HLine)
        line.setFrameShadow(QFrame.Sunken)
        layout.addWidget(line)
        layout.addLayout(joint_values_grid)

        frame = QFrame()
        frame.setLayout(layout)
        return frame
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}
Esempio n. 7
0
class CheckBoxGroup(QGroupBox):
    """
    Creates a button group of non-exclusive checkbox options. 

    Options must be a dict with following keys: 'enabled','title','description','tooltip'
    """
    def __init__(self,
                 options,
                 title='Checkboxes',
                 selected_indexes=[],
                 parent=None):
        super(CheckBoxGroup, self).__init__()
        self.setTitle(title)
        self.setLayout(QVBoxLayout())
        self._button_group = QButtonGroup()
        self._button_group.setExclusive(False)
        self._options = options
        if parent == None:
            parent = self

        for (button_id, option) in enumerate(self._options):

            checkbox = QCheckBox(option.get('title', 'option %d' % button_id))
            checkbox.setEnabled(option.get('enabled', True))
            checkbox.setChecked(button_id in selected_indexes)
            checkbox.setToolTip(option.get('tooltip', ''))

            self._button_group.addButton(checkbox, button_id)
            parent.layout().addWidget(checkbox)
            if 'description' in option:
                parent.layout().addWidget(QLabel(option['description']))

    def get_settings(self):
        """Returns dictionary with selected_indexes (array) and selected_options (array) keys."""
        selected_indexes = []
        selected_options = []
        for button in self._button_group.buttons():
            if button.isChecked():
                selected_indexes.append(self._button_group.id(button))
                selected_options.append(
                    self._options[self._button_group.id(button)])
        return {
            'selected_indexes': selected_indexes,
            'selected_options': selected_options
        }
Esempio n. 8
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. 9
0
    def __init__(self, options, title='Checkboxes', selected_indexes=[], parent=None):
        super(CheckBoxGroup, self).__init__()
        self.setTitle(title)
        self.setLayout(QVBoxLayout())
        self._button_group = QButtonGroup()
        self._button_group.setExclusive(False)
        self._options = options
        if parent == None:
            parent = self
        
        for (button_id, option) in enumerate(self._options):

            checkbox = QCheckBox(option.get('title', 'option %d' % button_id))
            checkbox.setEnabled(option.get('enabled', True))
            checkbox.setChecked(button_id in selected_indexes)
            checkbox.setToolTip(option.get('tooltip', ''))

            self._button_group.addButton(checkbox, button_id)
            parent.layout().addWidget(checkbox)
            if 'description' in option:
                parent.layout().addWidget(QLabel(option['description']))
Esempio n. 10
0
class CheckBoxGroup(QGroupBox):
    """
    Creates a button group of non-exclusive checkbox options. 

    Options must be a dict with following keys: 'enabled','title','description','tooltip'
    """

    def __init__(self, options, title='Checkboxes', selected_indexes=[], parent=None):
        super(CheckBoxGroup, self).__init__()
        self.setTitle(title)
        self.setLayout(QVBoxLayout())
        self._button_group = QButtonGroup()
        self._button_group.setExclusive(False)
        self._options = options
        if parent == None:
            parent = self
        
        for (button_id, option) in enumerate(self._options):

            checkbox = QCheckBox(option.get('title', 'option %d' % button_id))
            checkbox.setEnabled(option.get('enabled', True))
            checkbox.setChecked(button_id in selected_indexes)
            checkbox.setToolTip(option.get('tooltip', ''))

            self._button_group.addButton(checkbox, button_id)
            parent.layout().addWidget(checkbox)
            if 'description' in option:
                parent.layout().addWidget(QLabel(option['description']))

    def get_settings(self):
        """Returns dictionary with selected_indexes (array) and selected_options (array) keys."""
        selected_indexes = []
        selected_options = []
        for button in self._button_group.buttons():
            if button.isChecked():
                selected_indexes.append(self._button_group.id(button))
                selected_options.append(self._options[self._button_group.id(button)])
        return {'selected_indexes': selected_indexes, 'selected_options': selected_options}
class MoveitCommanderWidget(QWidget):
    """
    Provides a UI for Moveit Commander
    - parses rosparam 'robot_description_semantic' to get content of robot srdf
    - there are tabs for each move_group group
    - for each group, there is a set of buttons with the named targets
    - for each group, there is a button to get current joint values and a button to move to specific joint values
    """
    def __init__(self, context):
        super(MoveitCommanderWidget, self).__init__()
        self.setObjectName('MoveitCommanderWidget')

        self.groups = self.get_groups()
        self.commanders = [
            moveit_commander.MoveGroupCommander(group) for group in self.groups
        ]

        # list of QLineEdit fields used to display joint values
        self.text_joint_values = []

        self.MAX_COLUMNS = 4

        self.tab_widget = QTabWidget()
        for g in self.groups:
            frame = self.setup_group_frame(g)
            self.tab_widget.addTab(frame, g)

        widget_layout = QVBoxLayout()
        widget_layout.addWidget(self.tab_widget)
        self.setLayout(widget_layout)

    def setup_group_frame(self, group):
        layout = QVBoxLayout()

        # grid for buttons for named targets
        grid = QGridLayout()
        grid.setSpacing(1)

        self.button_group = QButtonGroup(self)

        row = 0
        column = 0
        named_targets = self.get_named_targets(group)
        for target in named_targets:
            button = QPushButton(target)
            self.button_group.addButton(button)
            grid.addWidget(button, row, column)
            column += 1
            if column >= self.MAX_COLUMNS:
                row += 1
                column = 0

        self.button_group.buttonClicked.connect(self._handle_button_clicked)

        # grid for show joint value and move arm buttons/text field
        joint_values_grid = QGridLayout()
        joint_values_grid.setSpacing(1)

        button_show_joint_values = QPushButton('Current Joint Values')
        button_show_joint_values.clicked[bool].connect(
            self._handle_show_joint_values_clicked)

        line_edit = QLineEdit()
        self.text_joint_values.append(line_edit)

        button_move_to = QPushButton('Move to')
        button_move_to.clicked[bool].connect(self._handle_move_to_clicked)

        joint_values_grid.addWidget(button_show_joint_values, 0, 1)
        joint_values_grid.addWidget(line_edit, 0, 2)
        joint_values_grid.addWidget(button_move_to, 0, 3)

        layout.addLayout(grid)
        line = QFrame()
        line.setFrameShape(QFrame.HLine)
        line.setFrameShadow(QFrame.Sunken)
        layout.addWidget(line)
        layout.addLayout(joint_values_grid)

        frame = QFrame()
        frame.setLayout(layout)
        return frame

    def _handle_button_clicked(self, button):
        commander = self.commanders[self.tab_widget.currentIndex()]
        rospy.loginfo('Moving ' + commander.get_name() + ' to: ' +
                      button.text())
        try:
            commander.set_named_target(str(button.text()))
        except Exception as e:
            rospy.logerr('Unable to set target: %s' % (str(e)))
            return

        error_code = commander.go(wait=True)
        if error_code == moveit_msgs.msg.MoveItErrorCodes.SUCCESS:
            rospy.loginfo("Succeeded")
        else:
            rospy.logerr("Movement failed with error code: %d", error_code)

    def _handle_show_joint_values_clicked(self, checked):
        commander = self.commanders[self.tab_widget.currentIndex()]
        l = commander.get_current_joint_values()
        l = [round(ll, 6) for ll in l]
        self.text_joint_values[self.tab_widget.currentIndex()].setText(str(l))

    def _handle_move_to_clicked(self, checked):
        commander = self.commanders[self.tab_widget.currentIndex()]
        # parse string as list
        l = ast.literal_eval(
            str(self.text_joint_values[self.tab_widget.currentIndex()].text()))

        if all(isinstance(x, float) for x in l):
            try:
                commander.set_joint_value_target(l)
            except Exception as e:
                rospy.logerr('Unable to set target position: %s' % (str(e)))
                return
            error_code = commander.go(wait=True)
            if error_code == moveit_msgs.msg.MoveItErrorCodes.SUCCESS:
                rospy.loginfo("Succeeded")
            else:
                rospy.logerr("Movement failed with error code: %d", error_code)

        else:
            rospy.logerr("Malformed joint angle list")

    def get_named_targets(self, group_name):
        s = rospy.get_param('/robot_description_semantic')
        rex = "group_state[ \\\\\n\r\f\v\t]*name=\"[a-zA-Z0-9_/\n\r ]*\"[ \\\\\n\r\f\v\t]*group=\"[ \\\\\n\r\f\v\t]*" \
              + group_name + "\""
        p = re.compile(rex)

        # returns a list of 'group_state name="named_target"' entries
        l = p.findall(s)

        pp = re.compile("name=\"[a-zA-Z0-9_/\n\r ]*\"")
        named_targets = [pp.findall(ll)[0][6:-1].strip() for ll in l]

        return named_targets

    def get_groups(self):
        s = rospy.get_param('/robot_description_semantic')
        p = re.compile("group[ \\\\\n\r\f\v\t]*name=\"[a-zA-Z0-9_/\n\r ]*\"")

        # returns a list of 'group name="group_name"' entries
        l = p.findall(s)

        pp = re.compile("name=\"[a-zA-Z0-9_/\n\r ]*\"")
        groups = [pp.findall(ll)[0][6:-1].strip() for ll in l]

        return groups
Esempio n. 12
0
    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)
Esempio n. 13
0
    def __init__(self, model):
        """
        Initializes the Widget.
        
        :param model: the model of the widget
        :type model: ROSModel
        """
        super(SelectionWidget, self).__init__()
        self.setObjectName('selection_widget')
        self.__model = model

        # Get path to UI file which is a sibling of this file
        self.rp = rospkg.RosPack()
        ui_file = os.path.join(self.rp.get_path('arni_rqt_detail_plugin'), 'resources', 'SelectionWidget.ui')
        # Extend the widget with all attributes and children from UI file
        loadUi(ui_file, self)
        self.setObjectName('SelectionWidgetUi')

        self.__selected_item = None


        self.__draw_graphs = True
        self.__current_combo_box_index = 0

        self.__last_update = rospy.Time.now()

        try:
            if rospy.get_param("/enable_statistics") == False:
                raise KeyError('/enable_statistics')
        except KeyError:
            self.__overview_widget = None
            raise EnvironmentError("/enable_statistics is either not set or set to false - arni gui would not work correctly. Please make sure to start "
                             "roscore with the neccesary parameters or to load these while running (see init_params.launch)")

        self.__values_dict = {
            "bandwidth_mean": 0,
            "bandwidth_stddev": 0,
            "bandwidth_max": 0,
        }

        self.__logger = self.__model.get_logger()
        self.__log_filter_proxy = LogFilterProxy()       
        self.__log_filter_proxy.filter_by_item(self.__selected_item)
        self.__log_filter_proxy.setDynamicSortFilter(True)        
        self.__log_filter_proxy.setSourceModel(self.__logger.get_representation())
        self.log_tab_tree_view.setModel(self.__log_filter_proxy)
        self.__log_delegate = LogDelegate()
        self.log_tab_tree_view.setItemDelegate(self.__log_delegate)

        self.__style_string = ".detailed_data {\n" \
                               "    font-size: 12\n;" \
                               "}\n"
        self.__style_string = ".erroneous_entry {\n" \
                               "    color: red\n;" \
                               "}\n"

        self.information_tab_text_browser.setStyleSheet(self.__style_string)

        self.range_combo_box.clear()
        self.range_combo_box.addItem("10 " + self.tr("Seconds"))
        self.range_combo_box.addItem("30 " + self.tr("Seconds"))
        self.range_combo_box.addItem("60 " + self.tr("Seconds"))
        self.range_combo_box.setCurrentIndex(0)

        #self.scrollAreaWidgetContents_2.setWidget(self.host_node_label)

        self.tab_widget.setTabText(0, self.tr("Information"))
        self.tab_widget.setTabText(1, self.tr("Graphs"))
        self.tab_widget.setTabText(2, self.tr("Log"))
        self.tab_widget.setTabText(3, self.tr("Actions"))

        self.stop_push_button.setText(self.tr("Stop Node"))
        self.restart_push_button.setText(self.tr("Restart Node"))
        self.stop_push_button.setEnabled(False)
        self.restart_push_button.setEnabled(False)
        
        ### CARSON ADDED ###
        # set default values for throttle rate and window sliders
        self.throttle_rate_slider.setFocusPolicy(Qt.StrongFocus)
        self.throttle_rate_slider.setValue(5000) 
        self.throttle_window_slider.setFocusPolicy(Qt.StrongFocus)
        self.throttle_window_slider.setValue(500)
        
        # set up validator for throttle rate and window text fields 
        # only allows floating point numbers
        regex = QRegExp(r'[0-9]*\.?[0-9]+')
        validator = QRegExpValidator(regex)
        self.throttle_rate.setValidator(validator)
        self.throttle_window.setValidator(validator)
        
        # set up QButtonGroup for message/bandwidth throttle type radio buttons
        self.throttle_radio_group = QButtonGroup()
        self.throttle_radio_group.addButton(self.throttle_message_radio)
        self.throttle_radio_group.addButton(self.throttle_bandwidth_radio)
        self.throttle_radio_group.buttonClicked.connect(self.__on_type_button_clicked)
        
        ### ###
        
        self.selected_label.setText(self.tr("Selected") + ":")
        self.range_label.setText(self.tr("Range") + ":")
        
        self.log_tab_tree_view.setRootIsDecorated(False)
        self.log_tab_tree_view.setAlternatingRowColors(True)
        self.log_tab_tree_view.setSortingEnabled(True)
        self.log_tab_tree_view.sortByColumn(1, Qt.AscendingOrder)

        self.__current_range_combo_box_index = 0
        self.__current_selected_combo_box_index = 0

        self.set_selected_item(self.__selected_item)
        self.__model.layoutChanged.connect(self.update)

        self.__state = "ok"
        self.__previous_state = "ok"

        self.__selected_item_changed = True

        self.__deleted = False
        
        pg.setConfigOption('background', 'w')
        pg.setConfigOption('foreground', 'k')
        self.__graph_layout = ResizeableGraphicsLayoutWidget(self.__on_graph_window_size_changed)
        self.graph_scroll_area.setWidget(self.__graph_layout)
        self.__plotable_items = None#self.__selected_item.get_plotable_items()
        self.__items_per_group = 1
        self.__expected_items_per_group = 1
        self.__number_of_groups = 1

        self.__update_graphs_lock = Lock()
        self.__first_update_pending = True

        self.__graph_dict = {}

        self.__plotted_curves = {}
        #self.create_graphs()

        self.__timer = Timer(Duration(secs=1.0), self.update_graphs)
Esempio n. 14
0
class SelectionWidget(QWidget):
    """
    The SelectionWidget of the ArniGuiDetail-Plugin.
    """
    
    def __init__(self, model):
        """
        Initializes the Widget.
        
        :param model: the model of the widget
        :type model: ROSModel
        """
        super(SelectionWidget, self).__init__()
        self.setObjectName('selection_widget')
        self.__model = model

        # Get path to UI file which is a sibling of this file
        self.rp = rospkg.RosPack()
        ui_file = os.path.join(self.rp.get_path('arni_rqt_detail_plugin'), 'resources', 'SelectionWidget.ui')
        # Extend the widget with all attributes and children from UI file
        loadUi(ui_file, self)
        self.setObjectName('SelectionWidgetUi')

        self.__selected_item = None


        self.__draw_graphs = True
        self.__current_combo_box_index = 0

        self.__last_update = rospy.Time.now()

        try:
            if rospy.get_param("/enable_statistics") == False:
                raise KeyError('/enable_statistics')
        except KeyError:
            self.__overview_widget = None
            raise EnvironmentError("/enable_statistics is either not set or set to false - arni gui would not work correctly. Please make sure to start "
                             "roscore with the neccesary parameters or to load these while running (see init_params.launch)")

        self.__values_dict = {
            "bandwidth_mean": 0,
            "bandwidth_stddev": 0,
            "bandwidth_max": 0,
        }

        self.__logger = self.__model.get_logger()
        self.__log_filter_proxy = LogFilterProxy()       
        self.__log_filter_proxy.filter_by_item(self.__selected_item)
        self.__log_filter_proxy.setDynamicSortFilter(True)        
        self.__log_filter_proxy.setSourceModel(self.__logger.get_representation())
        self.log_tab_tree_view.setModel(self.__log_filter_proxy)
        self.__log_delegate = LogDelegate()
        self.log_tab_tree_view.setItemDelegate(self.__log_delegate)

        self.__style_string = ".detailed_data {\n" \
                               "    font-size: 12\n;" \
                               "}\n"
        self.__style_string = ".erroneous_entry {\n" \
                               "    color: red\n;" \
                               "}\n"

        self.information_tab_text_browser.setStyleSheet(self.__style_string)

        self.range_combo_box.clear()
        self.range_combo_box.addItem("10 " + self.tr("Seconds"))
        self.range_combo_box.addItem("30 " + self.tr("Seconds"))
        self.range_combo_box.addItem("60 " + self.tr("Seconds"))
        self.range_combo_box.setCurrentIndex(0)

        #self.scrollAreaWidgetContents_2.setWidget(self.host_node_label)

        self.tab_widget.setTabText(0, self.tr("Information"))
        self.tab_widget.setTabText(1, self.tr("Graphs"))
        self.tab_widget.setTabText(2, self.tr("Log"))
        self.tab_widget.setTabText(3, self.tr("Actions"))

        self.stop_push_button.setText(self.tr("Stop Node"))
        self.restart_push_button.setText(self.tr("Restart Node"))
        self.stop_push_button.setEnabled(False)
        self.restart_push_button.setEnabled(False)
        
        ### CARSON ADDED ###
        # set default values for throttle rate and window sliders
        self.throttle_rate_slider.setFocusPolicy(Qt.StrongFocus)
        self.throttle_rate_slider.setValue(5000) 
        self.throttle_window_slider.setFocusPolicy(Qt.StrongFocus)
        self.throttle_window_slider.setValue(500)
        
        # set up validator for throttle rate and window text fields 
        # only allows floating point numbers
        regex = QRegExp(r'[0-9]*\.?[0-9]+')
        validator = QRegExpValidator(regex)
        self.throttle_rate.setValidator(validator)
        self.throttle_window.setValidator(validator)
        
        # set up QButtonGroup for message/bandwidth throttle type radio buttons
        self.throttle_radio_group = QButtonGroup()
        self.throttle_radio_group.addButton(self.throttle_message_radio)
        self.throttle_radio_group.addButton(self.throttle_bandwidth_radio)
        self.throttle_radio_group.buttonClicked.connect(self.__on_type_button_clicked)
        
        ### ###
        
        self.selected_label.setText(self.tr("Selected") + ":")
        self.range_label.setText(self.tr("Range") + ":")
        
        self.log_tab_tree_view.setRootIsDecorated(False)
        self.log_tab_tree_view.setAlternatingRowColors(True)
        self.log_tab_tree_view.setSortingEnabled(True)
        self.log_tab_tree_view.sortByColumn(1, Qt.AscendingOrder)

        self.__current_range_combo_box_index = 0
        self.__current_selected_combo_box_index = 0

        self.set_selected_item(self.__selected_item)
        self.__model.layoutChanged.connect(self.update)

        self.__state = "ok"
        self.__previous_state = "ok"

        self.__selected_item_changed = True

        self.__deleted = False
        
        pg.setConfigOption('background', 'w')
        pg.setConfigOption('foreground', 'k')
        self.__graph_layout = ResizeableGraphicsLayoutWidget(self.__on_graph_window_size_changed)
        self.graph_scroll_area.setWidget(self.__graph_layout)
        self.__plotable_items = None#self.__selected_item.get_plotable_items()
        self.__items_per_group = 1
        self.__expected_items_per_group = 1
        self.__number_of_groups = 1

        self.__update_graphs_lock = Lock()
        self.__first_update_pending = True

        self.__graph_dict = {}

        self.__plotted_curves = {}
        #self.create_graphs()

        self.__timer = Timer(Duration(secs=1.0), self.update_graphs)

    def __del__(self):
        self.__deleted = True

    def connect_slots(self):
        """
        Connects the slots.
        """
        # : tab_widget
        self.tab_widget.currentChanged.connect(self.__on_current_tab_changed)
        #: restart_push_button
        self.restart_push_button.clicked.connect(self.__on_restart_push_button_clicked)
        #: stop_push_button
        self.stop_push_button.clicked.connect(self.__on_stop_push_button_clicked)
        #: range_combo_box
        self.range_combo_box.currentIndexChanged.connect(self.__on_range_combo_box_index_changed)
        #pause button
        self.pause_button.clicked.connect(self.__on_pause_button_clicked)
        # selected combo box
        self.selected_combo_box.currentIndexChanged.connect(self.__on_selected_combo_box_index_changed)
        
        ### CARSON ADDED ###
        # connect sliders to text fields and update text fields for first time
        self.throttle_rate_slider.valueChanged.connect(self.__on_throttle_rate_slider_changed)
        self.throttle_window_slider.valueChanged.connect(self.__on_throttle_window_slider_changed)
        self.__on_throttle_rate_slider_changed(self.throttle_rate_slider.value())
        self.__on_throttle_window_slider_changed(self.throttle_window_slider.value())
        
        # connect text fields to sliders
        self.throttle_rate.editingFinished.connect(self.__on_throttle_rate_changed)
        self.throttle_window.editingFinished.connect(self.__on_throttle_window_changed)
        
        # connect buttons to actions
        self.throttle_start_button.clicked.connect(self.__on_throttle_start_button_clicked)
        self.throttle_stop_button.clicked.connect(self.__on_throttle_stop_button_clicked)
        self.throttle_reset_button.clicked.connect(self.__on_throttle_reset_button_clicked)
        ### ###
        
    ### CARSON ADDED ###
    def __on_throttle_rate_slider_changed(self, value):
        """Called whenever throttle rate slider changes value.
        Updates the throttle rate text field to match the slider value.
        Performs conversion if necessary.

        Args:
            value (int): new value of throttle rate slider
        """
        if self.throttle_radio_group.checkedButton() is self.throttle_message_radio:
            value = convert_from_slider(value, THROTTLE_RATE_SLIDER_DPI)
        self.throttle_rate.setText(str(value))
    
    def __on_throttle_rate_changed(self):
        """Called whenever throttle rate text field emits the "finishedEditing" signal.
        Updates the throttle rate slider to match the throttle rate text field, using
        necessary conversions and checking for endpoints.
        """
        text = self.throttle_rate.text()
        value = float(text)
        if self.throttle_radio_group.checkedButton() is self.throttle_message_radio:
            value = convert_to_slider(value, THROTTLE_RATE_SLIDER_DPI)
            if value%1 != 0:
                value = int(value)
        else:
            value = int(value)
        # check for endpoints
        if value > self.throttle_rate_slider.maximum():
            value = self.throttle_rate_slider.maximum()
        if value < self.throttle_rate_slider.minimum():
            value = self.throttle_rate_slider.minimum()
        # call slider update method manually if value is same (keeps text field in sync)
        if self.throttle_rate_slider.value() == value:
            self.__on_throttle_rate_slider_changed(value)
        self.throttle_rate_slider.setValue(value)
        
    def __on_throttle_window_slider_changed(self, value):
        """Called whenever throttle window slider changes value.
        Updates the throttle window text field to match the slider value
        using the proper conversion.

        Args:
            value (int): new value of throttle window slider
        """
        self.throttle_window.setText(str(convert_from_slider(value, THROTTLE_WINDOW_SLIDER_DPI)))
    
    def __on_throttle_window_changed(self):
        """Called whenever throttle window text field emits the "finishedEditing" signal.
        Updates the throttle window slider to match the throttle window text field, using
        necessary conversions and checking for endpoints.
        """
        text = self.throttle_window.text()
        value = convert_to_slider(float(text), THROTTLE_WINDOW_SLIDER_DPI)
        # check for endpoints
        if value > self.throttle_window_slider.maximum():
            value = self.throttle_window_slider.maximum()
        if value < self.throttle_window_slider.minimum():
            value = self.throttle_window_slider.minimum()
        # call slider update method manually if value is same (keeps text field in sync)
        if self.throttle_window_slider.value() == value:
            self.__on_throttle_window_slider_changed(value)
        self.throttle_window_slider.setValue(value)

    def __on_type_button_clicked(self, button):
        """Called whenever a throttle type radio button is clicked.
        Performs the necessary GUI updates for the new throttle type.
        
        Args:
            button (QRadioButton): currently checked radio button.
        """
        throttle = self.__selected_item.topic_item.throttle
        if button is self.throttle_message_radio:
            self.throttle_rate_label.setText('Rate (Hz)')
            self.__update_throttle_window_gui(False)
            self.throttle_rate_slider.setMaximum(convert_to_slider(1000, THROTTLE_RATE_SLIDER_DPI))    
            self.throttle_rate_slider.setTickInterval(convert_to_slider(100, THROTTLE_RATE_SLIDER_DPI))    
            value = 0
            if throttle is None or not self.typeButtonMatchesThrottleType(throttle):
                value = convert_to_slider(500, THROTTLE_RATE_SLIDER_DPI)
            else:
                value = convert_to_slider(throttle.rate, THROTTLE_RATE_SLIDER_DPI)
            self.throttle_rate_slider.setValue(value)
            self.throttle_rate.setMaxLength(5)
        else:
            self.throttle_rate_label.setText('Rate (Kb/s)')
            self.__update_throttle_window_gui(True)
            self.throttle_rate_slider.setMaximum(1e6)    
            self.throttle_rate_slider.setTickInterval(1e5)    
            value = 0
            if throttle is None or not self.typeButtonMatchesThrottleType(throttle):
                value = 5e5
            else:
                value = throttle.bandwidth
            self.throttle_rate_slider.setValue(value)
            self.throttle_rate.setMaxLength(7)
            self.throttle_window_slider.setValue(convert_to_slider(5, THROTTLE_WINDOW_SLIDER_DPI))
            
    def __on_throttle_start_button_clicked(self):
        """Called whenever the Start Throttle button is clicked.

        Starts or updates the throttle as necessary according to which radio button
        is active (message or bandwidth)
        """
        # grab topic name and inner topic item from selected_item
        # this is necessary because only the TopicItem class has the new throttle attribute
        topic_name = self.__selected_item.seuid.split('/')[-1]
        topic_item = self.__selected_item.topic_item
        # convert rate field to number 
        throttle_rate = float(self.throttle_rate.text())            
        
        # determine if we need to make a new throttle or update an existing one
        if topic_item.throttle is None or not self.typeButtonMatchesThrottleType(topic_item.throttle):
            if topic_item.throttle is not None:
                # stop existing throttle if there is one
                print(topic_item.throttle.stop())
            if self.throttle_radio_group.checkedButton() is self.throttle_message_radio:
                topic_item.throttle = MessageThrottle(topic_name, topic_name + '_message_throttled', throttle_rate)
                # self.throttle_message_radio.setPalette(self.active_label_palette)
                print(topic_item.throttle.start())
                print('started message throttler')
            else:
                # addtionally grab window value for a bandwidth throttle
                throttle_window = float(self.throttle_window.text())
                topic_item.throttle = BandwidthThrottle(topic_name, topic_name + '_bandwidth_throttled', kb_to_bytes(throttle_rate), throttle_window)
                # self.throttle_window_radio.setPalette(self.active_label_palette)
                print(topic_item.throttle.start())
                print('started bandwidth throttler')
            # enable stop button now that a throttle is active
            self.throttle_stop_button.setEnabled(True)
        else:
            if self.throttle_radio_group.checkedButton() is self.throttle_message_radio:
                if throttle_rate != topic_item.throttle.rate:
                    success = topic_item.throttle.update(rate=throttle_rate)
                    print(success)
                    if success == None:
                        print('no running throttle')
                else:
                    print('no updates to throttle necessary')
            else:
                throttle_window = float(self.throttle_window.text())
                if throttle_rate != topic_item.throttle.bandwidth or throttle_window != topic_item.throttle.window:
                    success = topic_item.throttle.update(bandwidth=throttle_rate, window=throttle_window)
                    print(success)
                    if success == None:
                        print('no running throttle')
                else:
                    print('no updates to throttle necessary')
                

    def __on_throttle_stop_button_clicked(self):
        """Called whenever the Stop Throttle button is clicked.
        Stops the running throttle, resets the topic's throttle attribute, and disables
        the Stop Throttle button.
        """
        print(self.__selected_item.topic_item.throttle.stop())
        self.__selected_item.topic_item.throttle = None
        self.throttle_stop_button.setEnabled(False)
        
    def __on_throttle_reset_button_clicked(self):
        """Called whenever the Reset Throttle button is clicked.
        Resets the throttle parameters to current throttle values or defaults if no
        throttle is running.
        """
        throttle = self.__selected_item.topic_item.throttle
        if throttle is None or not self.typeButtonMatchesThrottleType(throttle):
            self.__on_type_button_clicked(self.throttle_radio_group.checkedButton())
        else:
            if self.throttle_radio_group.checkedButton() is self.throttle_message_radio:
                self.throttle_rate_slider.setValue(convert_to_slider(throttle.rate, THROTTLE_RATE_SLIDER_DPI))
            else:
                self.throttle_rate_slider.setValue(bytes_to_kb(throttle.bandwidth))
                self.throttle_window_slider.setValue(convert_to_slider(throttle.window, THROTTLE_WINDOW_SLIDER_DPI))

    def typeButtonMatchesThrottleType(self, throttle):
        """Determines if the active type radio button matches the type of the
        provided throttle.

        Args:
            throttle (rosthrottle.MessageThrottle or rosthrottle.BandwidthThrottle): throttle to compare with
        """
        message_match = self.throttle_radio_group.checkedButton() is self.throttle_message_radio and isinstance(throttle, MessageThrottle)
        bandwidth_match = self.throttle_radio_group.checkedButton() is self.throttle_bandwidth_radio and isinstance(throttle, BandwidthThrottle)
        return message_match or bandwidth_match


    def __update_throttle_window_gui(self, mode):
        """Updates the throttle window GUI components to the given mode.
        
        Args:
            mode (bool): true if enabling throttle window GUI components, false if disabling
        """
        self.throttle_window_label.setEnabled(mode)
        self.throttle_window_slider.setEnabled(mode)
        self.throttle_window.setEnabled(mode)

    def __update_throttle_gui(self, mode):
        """Updates the throttle GUI to the given mode.
        
        Args:
            mode (bool): true if enabling throttle GUI components, false if disabling
        """
        if self.throttle_radio_group.checkedButton() is self.throttle_bandwidth_radio:
            self.__update_throttle_window_gui(mode)
        self.throttle_rate_slider.setEnabled(mode)
        if self.__selected_item.can_execute_throttles():
            if self.__selected_item.topic_item.throttle is not None:
                self.throttle_stop_button.setEnabled(True)
                self.__update_throttle_params(throttle=self.__selected_item.topic_item.throttle)
            else:
                self.throttle_stop_button.setEnabled(False)
                self.__update_throttle_params()
        else:
            self.throttle_stop_button.setEnabled(mode)
        self.throttle_start_button.setEnabled(mode)
        self.throttle_rate_label.setEnabled(mode)
        self.throttle_bandwidth_radio.setEnabled(mode)
        self.throttle_message_radio.setEnabled(mode)
        self.throttle_rate.setEnabled(mode)
        self.throttle_reset_button.setEnabled(mode)
        
    def __update_throttle_params(self, throttle=None):
        """Updates the throttle parameters based on the given throttle.

        Args:
            throttle (optional, rosthrottle.MessageThrottle or rosthrottle.BandwidthThrottle): throttle
                to use as source of throttle parameters. If not provided, default values are used.
        """
        if throttle is None:
            self.throttle_message_radio.setChecked(True)
            self.__on_type_button_clicked(self.throttle_message_radio)
        else:
            if isinstance(throttle, MessageThrottle):
                self.throttle_message_radio.setChecked(True)
                self.__on_type_button_clicked(self.throttle_message_radio)
                self.throttle_rate_slider.setValue(convert_to_slider(throttle.rate, THROTTLE_RATE_SLIDER_DPI))
                self.throttle_window_slider.setValue(500)
            else:
                self.throttle_bandwidth_radio.setChecked(True)
                self.__on_type_button_clicked(self.throttle_bandwidth_radio)
                self.throttle_rate_slider.setValue(bytes_to_kb(throttle.bandwidth))
                self.throttle_window_slider.setValue(convert_to_slider(throttle.window, THROTTLE_WINDOW_SLIDER_DPI))
    ### ###

    def create_graphs(self):
        """
        Creates the graphs for the plot.
        """
        self.__update_graphs_lock.acquire()
        if self.__selected_item is not None:
            first_iteration = True
            first_view = None
            i = 0
            self.__expected_items_per_group = 0
            self.__graph_layout.clear()

            if len(self.__plotable_items) is 0:
                raise UserWarning()
            for key in self.__plotable_items[min(self.__current_selected_combo_box_index *
                                                self.__items_per_group, len(self.__plotable_items)):
                                                min((self.__current_selected_combo_box_index + 1)
                                                * self.__items_per_group, len(self.__plotable_items))]:
                plot_widget = None
                if first_iteration:
                    first_iteration = False
                    date_axis = DateAxis(orientation="bottom")
                    first_view = pg.ViewBox()

                    plot_widget = self.__graph_layout.addPlot(title=self.tr(key), axisItems={'bottom': date_axis}, viewBox=first_view)
                else:

                    date_axis = DateAxis(orientation="bottom")
                    view_box = pg.ViewBox()
                    plot_widget = self.__graph_layout.addPlot(title=self.tr(key), viewBox=view_box, axisItems={'bottom': date_axis})
                    view_box.setXLink(first_view)

                #performance enhancements when only a short range of the plot is shown
                #plot_widget.setClipToView(clip=True)
                plot_widget.setYRange(-1, 1)
                self.__graph_dict[key] = plot_widget
                self.__graph_layout.nextRow()
                plot_widget = self.__graph_dict[key]
                plot_widget.showGrid(x=True, y=True)
                plot_widget.setMenuEnabled(enableMenu=True)
                plot_widget.enableAutoRange('xy', True)
                x = np.array([1])
                y = np.array([int(str(Time.now()))/1000000000])
                self.__plotted_curves[key] = plot_widget.plot(x=x, y=y, fillLevel=0, brush=(50, 50, 200, 100),
                                                              pen=(255, 0, 0))
                self.__expected_items_per_group += 1
            self.__first_update_pending = True
        self.__update_graphs_lock.release()


    def __on_graph_window_size_changed(self):
        # getting the size
        if self.__selected_item is not None:
            size = self.__graph_layout.size()
            items_per_group = max(int(math.ceil((size.height() - 100) / 200 + 1)), 1)
            if items_per_group is not self.__items_per_group or self.__selected_item_changed:
                self.__graph_layout.set_blocked(True)
                self.__items_per_group = items_per_group
                self.__number_of_groups = int(math.ceil(len(self.__plotable_items) / float(self.__items_per_group)))
                # change the groups in the widget
                self.selected_combo_box.clear()
                for group in range(0, self.__number_of_groups):
                    list = self.__plotable_items[min(group *
                                        self.__items_per_group, len(self.__plotable_items)):min((group + 1)
                                                                * self.__items_per_group, len(self.__plotable_items))]
                    content = ""
                    for i in range(0, len(list) - 1):
                        content += self.tr(list[i])
                        content += ", "
                    content += list[len(list) - 1]
                    self.selected_combo_box.addItem(content)
                # redraw
                self.create_graphs()
                self.update_graphs(None)
                self.__graph_layout.set_blocked(False)

    def __on_selected_combo_box_index_changed(self, index):
        """
        Updates what is shown in the graphs

        :param index: the index of the selected range
        :type index: int
        """
        if index is not -1:
            self.__current_selected_combo_box_index = index
            self.create_graphs()
            self.update_graphs(None)


    def __on_pause_button_clicked(self):
        """
        To be called whenever the pause button is clicked. Stops the graphs from updating until the pause button
        is clicked again and the other way.
        """
        if self.__draw_graphs:
            self.__draw_graphs = False
            self.pause_button.setText(self.tr("Continue"))
        else:
            self.__draw_graphs = True
            self.pause_button.setText(self.tr("Pause"))


    def set_selected_item(self, index):
        """
        Sets the selected item.

        :param index: the index of the selected item
        :type selected_item: QModelIndex
        """
        if index is not None:
            self.__update_graphs_lock.acquire()
            src_index = index.model().mapToSource(index)
            self.__selected_item = src_index.internalPointer()
            self.__log_filter_proxy.filter_by_item(self.__selected_item)
            if self.__selected_item is not None:
                self.__plotable_items = self.__selected_item.get_plotable_items()
                for key in self.__selected_item.get_list_items():
                    self.__plotable_items.remove(key)
                # check if actions can be executed
                if self.__selected_item.can_execute_actions():
                    self.stop_push_button.setEnabled(True)
                    self.restart_push_button.setEnabled(True)
                else:
                    self.stop_push_button.setEnabled(False)
                    self.restart_push_button.setEnabled(False)
                # check if throttles can be executed
                if self.__selected_item.can_execute_throttles():
                    self.__update_throttle_gui(True)
                else:
                    self.__update_throttle_gui(False)
                self.__selected_item_changed = True
            self.__update_graphs_lock.release()
            self.update()
            self.__on_graph_window_size_changed()


    def __on_current_tab_changed(self, tab):
        """
        Will be called when switching between tabs.

        :param tab: index of the current tab
        :type tab: int
        """
        if tab is 1:
            self.__draw_graphs = True
        else:
            self.__draw_graphs = False


    def __on_restart_push_button_clicked(self):
        """
        Handels the restart button and restarts a host or node.
        """
        if self.__selected_item is not None:
            self.__selected_item.execute_action("restart")


    def __on_stop_push_button_clicked(self):
        """Handels the stop button and stops a host or node."""
        if self.__selected_item is not None:
            self.__selected_item.execute_action("stop")


    def __on_range_combo_box_index_changed(self, index):
        """
        Handels the change of the graph range.

        :param index: the index of the selected range
        :type index: int
        """
        self.__current_combo_box_index = index


    def update_graphs(self, event):
        """
        Updates and redraws the graphs.
        """
        self.__update_graphs_lock.acquire()
        if self.__selected_item is not None:
            if self.__draw_graphs or self.__first_update_pending:
                #print("items per group: " + str(self.__items_per_group))
                #print("combo index: " + str(self.__current_selected_combo_box_index))
                #print("len plot " + str(len(self.__plotable_items)))
                plotable_items = self.__plotable_items[min(self.__current_selected_combo_box_index *
                                    self.__items_per_group, len(self.__plotable_items)):min((self.__current_selected_combo_box_index + 1)
                                                            * self.__items_per_group, len(self.__plotable_items))]
                plotable_data = self.__selected_item.get_items_younger_than(
                    #Time.now() - Duration(secs=self.__combo_box_index_to_seconds(self.__current_range_combo_box_index)),
                    Time.now() - (Duration(secs=(self.__combo_box_index_to_seconds(self.__current_range_combo_box_index) + 10 )) if int(Duration(secs=self.__combo_box_index_to_seconds(self.__current_range_combo_box_index)).to_sec()) <= int(Time.now().to_sec()) else Time(0) ),
                    "window_stop", *plotable_items)
                if "window_stop" in plotable_data:
                    if plotable_data["window_stop"]:
                        temp_time = []
                        temp_content = []

                        length = len(plotable_data["window_stop"])
                        modulo = (length / 200) + 1

                        for i in range(0, length, modulo):
                            # now having maximally 100 items to plot :)
                            temp_time.append(plotable_data["window_stop"][i].to_sec())
                        x = np.array(temp_time)

                        list_entries = self.__selected_item.get_list_items()
                        time_entries = self.__selected_item.get_time_items()

                        for key in plotable_items:
                            if key in list_entries:
                                pass
                            else:
                                if key in time_entries:
                                    for i in range(0, length, modulo):
                                        temp_content.append(float(str(plotable_data[key][i]))/1000000000)
                                else:
                                    for i in range(0, length, modulo):
                                        temp_content.append(plotable_data[key][i])
                                y = np.array(temp_content)
                                del temp_content[:]
                                self.__plotted_curves[key].setData(x=x, y=y)
                    else:
                        pass

            self.__first_update_pending = False
        self.__update_graphs_lock.release()
      
    
    def update(self):
        """
        Updates the widget.
        """
        if not self.__deleted:
            if self.__selected_item is not None:
                data_dict = self.__selected_item.get_latest_data()
                self.__state = data_dict["state"]
                self.host_node_label.setText(self.tr(self.__selected_item.get_seuid()))

                if self.__previous_state is not self.__state:
                    self.__previous_state = self.__state
                    if self.__state == "ok":
                        self.current_status_label.setText(self.tr("ok"))
                        #self.host_node_label.setText(self.tr("Current Status: Ok"))
                        pixmap = QPixmap(os.path.join(self.rp.get_path('arni_rqt_detail_plugin'), 'resources/graphics',
                                                      'block_green.png'))
                    elif self.__state == "warning":
                        self.current_status_label.setText(self.tr("warning"))
                        #self.host_node_label.setText(self.tr("Current Status: Warning"))
                        pixmap = QPixmap(os.path.join(self.rp.get_path('arni_rqt_detail_plugin'), 'resources/graphics',
                                                      'block_orange.png'))
                    elif self.__state == "unknown":
                        self.current_status_label.setText(self.tr("unkown"))
                        #self.host_node_label.setText(self.tr("Current Status: Unkown"))
                        pixmap = QPixmap(os.path.join(self.rp.get_path('arni_rqt_detail_plugin'), 'resources/graphics',
                                                      'block_grey.png'))
                    else: # error or offline
                        self.current_status_label.setText(self.tr(self.__state))
                        pixmap = QPixmap(os.path.join(self.rp.get_path('arni_rqt_detail_plugin'), 'resources/graphics',
                                                      'block_red.png'))
                    self.status_light_label.setPixmap(pixmap)
                content = self.__selected_item.get_detailed_data()

                scroll_value = self.information_tab_text_browser.verticalScrollBar().value()
                self.information_tab_text_browser.setHtml(content)
                self.information_tab_text_browser.verticalScrollBar().setSliderPosition(scroll_value)
            else:
                self.host_node_label.setText(self.tr("No item selected"))
                self.current_status_label.setText(self.tr("Offline"))
                self.information_tab_text_browser.setText(self.tr("Please select an item in the TreeView to get more information"
                                                          " about it"))


    def __combo_box_index_to_seconds(self, index):
        """
        Calculates the range from the combo-box index.
        
        :param index: the index of teh combo-box
        :type index: int
        
        :returns: the seconds of the selected index 
        :rtype: int
        """
        if self.__current_combo_box_index == 0:
            return 10
        elif self.__current_combo_box_index == 1:
            return 30
        else:
            return 60


    def get_current_tab(self):
        """
        Returns the current tab.
        
        :returns: the current tab 
        :rtype: int
        """
        return self.tab_widget.currentIndex()


    def set_current_tab(self, index=0):
        """
        Sets the default tab.
        
        :param index: the index of the tab
        :type index: int
        """
        if index is None:
            index = 0
        self.tab_widget.setCurrentIndex(index)


    def get_range_combo_box_index(self):
        """
        Returns the index of the combo-box.
        
        :returns: the index
        :rtype: int
        """
        return self.range_combo_box.currentIndex()


    def set_range_combo_box_index(self, index=0):
        """
        Sets the default value of the combo-box.
        
        :param index: the index of the combo-box
        :type index: int
        """
        if index is None:
            index = 0
        self.range_combo_box.setCurrentIndex(index)
Esempio n. 15
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()
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])
    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)
Esempio n. 18
0
    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);
Esempio n. 19
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()