Ejemplo n.º 1
0
    def __init__(self, context):
        super(SimpleGUI, self).__init__(context)
        self.setObjectName('SimpleTeleopGUI')
        self._widget = QWidget()

        # INIT CLIENTS
        self._gripper_client = GripperClient()
        self._base_publisher = BasePublisher()
        self._sound_client = SoundClient()
        self._head_client = HeadClient()

        rospy.Subscriber('robotsound', SoundRequest, self.sound_cb)
        self.sound_sig.connect(self.sound_sig_cb)

        self._base_speed = 0.5
        
        # OVERALL LAYOUT
        large_box = QtGui.QVBoxLayout()

        upper_box = QtGui.QVBoxLayout()
        large_box.addLayout(upper_box)
        
        lower_box = QtGui.QHBoxLayout()
        large_box.addLayout(lower_box)

        gripper_box = QtGui.QHBoxLayout()
        lower_box.addLayout(gripper_box)

        # GRIPPER CONTROLS
        gripper_box_label = QtGui.QLabel('Grippers: ')
        gripper_box.addWidget(gripper_box_label)

        gripper_left_btns = QtGui.QVBoxLayout()
        g_left_open_btn = QtGui.QPushButton('Open L', self._widget)
        g_left_open_btn.clicked.connect(self.gripper_cb)
        gripper_left_btns.addWidget(g_left_open_btn)
        g_left_close_btn = QtGui.QPushButton('Close L', self._widget)
        g_left_close_btn.clicked.connect(self.gripper_cb)
        gripper_left_btns.addWidget(g_left_close_btn)
        gripper_box.addLayout(gripper_left_btns)

        gripper_right_btns = QtGui.QVBoxLayout()
        g_right_open_btn = QtGui.QPushButton('Open R', self._widget)
        g_right_open_btn.clicked.connect(self.gripper_cb)
        gripper_right_btns.addWidget(g_right_open_btn)
        g_right_close_btn = QtGui.QPushButton('Close R', self._widget)
        g_right_close_btn.clicked.connect(self.gripper_cb)
        gripper_right_btns.addWidget(g_right_close_btn)
        gripper_box.addLayout(gripper_right_btns)

        lower_box.addItem(QtGui.QSpacerItem(100,20))

        # SPEECH CONTROLS
        speech_box = QtGui.QHBoxLayout()
        
        speech_label = QtGui.QLabel('Speech: ')
        speech_box.addWidget(speech_label)

        self.speech_text = QtGui.QLineEdit(self._widget);
        speech_box.addWidget(self.speech_text);

        speech_box.addWidget(self.create_btn('Speak'))
        speech_box.addStretch(1)
        lower_box.addLayout(speech_box)

        large_box.addItem(QtGui.QSpacerItem(100,20))
        
#        speech_box = QtGui.QHBoxLayout()
#        self.speech_label = QtGui.QLabel('Robot has not spoken yet')
#        palette = QtGui.QPalette()
#        palette.setColor(QtGui.QPalette.Foreground,QtCore.Qt.blue)
#        self.speech_label.setPalette(palette)
#        speech_box.addWidget(self.speech_label)

#        large_box.addLayout(speech_box)


        # BASE MOVEMENT CONTROLS
        base_controls = QtGui.QHBoxLayout()
        base_label = QtGui.QLabel('Base Movement: ')
        base_label.setGeometry(100,100,100,100)
        

        move_speed_controls = QtGui.QVBoxLayout()
        move_speed_controls.addWidget(base_label)

        move_speed_label = QtGui.QLabel('Move Speed')
        move_speed_controls.addWidget(move_speed_label)
        move_speed_sldr = self.create_vert_sldr()
        move_speed_sldr.valueChanged[int].connect(self._change_move_speed)
        move_speed_controls.addWidget(move_speed_sldr)


        move_base_controls = QtGui.QVBoxLayout()

        move_fwd_controls = QtGui.QHBoxLayout()
        move_fwd_right_btn = self.create_toggle_btn("Forward Left")
        move_fwd_right_btn.clicked.connect(self.make_move_base_cb(0.3))
        move_fwd_controls.addWidget(move_fwd_right_btn)
        move_fwd_btn = self.create_toggle_btn("Forward")
        move_fwd_btn.clicked.connect(self.make_move_base_cb(0.3))
        move_fwd_controls.addWidget(move_fwd_btn)
        move_fwd_left_btn = self.create_toggle_btn("Forward Right")
        move_fwd_left_btn.clicked.connect(self.make_move_base_cb(0.3))
        move_fwd_controls.addWidget(move_fwd_left_btn)

        turn_controls = QtGui.QHBoxLayout()
        turn_left_btn = self.create_toggle_btn("Turn Left")
        turn_left_btn.clicked.connect(self.make_move_base_cb(0.3))
        turn_controls.addWidget(turn_left_btn)
        stop_btn = self.create_toggle_btn("STOP")
        turn_controls.addWidget(stop_btn)
        turn_right_btn = self.create_toggle_btn("Turn Right")
        turn_right_btn.clicked.connect(self.make_move_base_cb(0.3))
        turn_controls.addWidget(turn_right_btn)

        move_bkwd_controls = QtGui.QHBoxLayout()
        move_bkwd_right_btn = self.create_toggle_btn("Backward Left")
        move_bkwd_right_btn.clicked.connect(self.make_move_base_cb(0.3))
        move_bkwd_controls.addWidget(move_bkwd_right_btn)
        move_bkwd_btn = self.create_toggle_btn("Backward")
        move_bkwd_btn.clicked.connect(self.make_move_base_cb(0.3))
        move_bkwd_controls.addWidget(move_bkwd_btn)
        move_bkwd_left_btn = self.create_toggle_btn("Backward Right")
        move_bkwd_left_btn.clicked.connect(self.make_move_base_cb(0.3))
        move_bkwd_controls.addWidget(move_bkwd_left_btn)

        move_base_controls.addLayout(move_fwd_controls)
        move_base_controls.addLayout(turn_controls)
        move_base_controls.addLayout(move_bkwd_controls)


        base_controls.addLayout(move_speed_controls)
        base_controls.addLayout(move_base_controls)

        upper_box.addLayout(base_controls)


        # HEAD CONTROLS
        move_head_controls = QtGui.QHBoxLayout()

        head_label = QtGui.QLabel('Head Position: ')
        move_head_controls.addWidget(head_label)

        move_vert_controls = QtGui.QHBoxLayout()
        move_vert_sldr = self.create_vert_sldr()
        move_vert_sldr.valueChanged[int].connect(self.move_head_vert)

        move_hor_controls = QtGui.QHBoxLayout()
        move_hor_sldr = self.create_hor_sldr()
        move_hor_sldr.valueChanged[int].connect(self.move_head_hor)

        move_vert_controls.addWidget(move_vert_sldr)
        move_hor_controls.addWidget(move_hor_sldr)

        move_head_controls.addLayout(move_vert_controls)
        move_head_controls.addLayout(move_hor_controls)

        upper_box.addLayout(move_head_controls)
       
        # ARM CONTROLS

        arm_controls = QtGui.QVBoxLayout()
        arm_r_controls = QtGui.QVBoxLayout()
        arm_l_controls = QtGui.QVBoxLayout()
        arm_r_controls.addWidget(QtGui.QLabel('Right Arm: '))
        arm_l_controls.addWidget(QtGui.QLabel('Left Arm: '))
        
        arm_l_control_grid = QtGui.QGridLayout()
        arm_r_control_grid = QtGui.QGridLayout()

        #for i in range(1, 4):
         #   slider = QtGui.QSlider(QtCore.Qt.Vertical, self._widget)
          #  slider.setFocusPolicy(QtCore.Qt.NoFocus)
           # slider.setGeometry(30, 40, 30, 100)
            #arm_l_control_grid.addWidget(slider, 0, i-1)
            #arm_l_control_grid.addWidget(QtGui.QLabel(str(i)), 1, i-1)


        #for i in range(1, 4):
         #   slider = QtGui.QSlider(QtCore.Qt.Vertical, self._widget)
          #  slider.setFocusPolicy(QtCore.Qt.NoFocus)
           # slider.setGeometry(30, 40, 30, 100)
            #arm_r_control_grid.addWidget(slider, 0, i-1)
            #arm_r_control_grid.addWidget(QtGui.QLabel(str(i)), 1, i-1)

        arm_l_controls.addLayout(arm_l_control_grid)
        arm_r_controls.addLayout(arm_r_control_grid)

        arm_controls.addLayout(arm_l_controls)
        arm_controls.addLayout(arm_r_controls)
      
        # large_box.addLayout(arm_controls)

        # SET EVERYTHING
        QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10))
        self._widget.setObjectName('SimpleGUI')
        self._widget.setLayout(large_box)
        context.add_widget(self._widget)
Ejemplo n.º 2
0
class SimpleGUI(Plugin):

    sound_sig = Signal(SoundRequest)

    def __init__(self, context):
        super(SimpleGUI, self).__init__(context)
        self.setObjectName('SimpleTeleopGUI')
        self._widget = QWidget()

        # INIT CLIENTS
        self._gripper_client = GripperClient()
        self._base_publisher = BasePublisher()
        self._sound_client = SoundClient()
        self._head_client = HeadClient()

        rospy.Subscriber('robotsound', SoundRequest, self.sound_cb)
        self.sound_sig.connect(self.sound_sig_cb)

        self._base_speed = 0.5
        
        # OVERALL LAYOUT
        large_box = QtGui.QVBoxLayout()

        upper_box = QtGui.QVBoxLayout()
        large_box.addLayout(upper_box)
        
        lower_box = QtGui.QHBoxLayout()
        large_box.addLayout(lower_box)

        gripper_box = QtGui.QHBoxLayout()
        lower_box.addLayout(gripper_box)

        # GRIPPER CONTROLS
        gripper_box_label = QtGui.QLabel('Grippers: ')
        gripper_box.addWidget(gripper_box_label)

        gripper_left_btns = QtGui.QVBoxLayout()
        g_left_open_btn = QtGui.QPushButton('Open L', self._widget)
        g_left_open_btn.clicked.connect(self.gripper_cb)
        gripper_left_btns.addWidget(g_left_open_btn)
        g_left_close_btn = QtGui.QPushButton('Close L', self._widget)
        g_left_close_btn.clicked.connect(self.gripper_cb)
        gripper_left_btns.addWidget(g_left_close_btn)
        gripper_box.addLayout(gripper_left_btns)

        gripper_right_btns = QtGui.QVBoxLayout()
        g_right_open_btn = QtGui.QPushButton('Open R', self._widget)
        g_right_open_btn.clicked.connect(self.gripper_cb)
        gripper_right_btns.addWidget(g_right_open_btn)
        g_right_close_btn = QtGui.QPushButton('Close R', self._widget)
        g_right_close_btn.clicked.connect(self.gripper_cb)
        gripper_right_btns.addWidget(g_right_close_btn)
        gripper_box.addLayout(gripper_right_btns)

        lower_box.addItem(QtGui.QSpacerItem(100,20))

        # SPEECH CONTROLS
        speech_box = QtGui.QHBoxLayout()
        
        speech_label = QtGui.QLabel('Speech: ')
        speech_box.addWidget(speech_label)

        self.speech_text = QtGui.QLineEdit(self._widget);
        speech_box.addWidget(self.speech_text);

        speech_box.addWidget(self.create_btn('Speak'))
        speech_box.addStretch(1)
        lower_box.addLayout(speech_box)

        large_box.addItem(QtGui.QSpacerItem(100,20))
        
#        speech_box = QtGui.QHBoxLayout()
#        self.speech_label = QtGui.QLabel('Robot has not spoken yet')
#        palette = QtGui.QPalette()
#        palette.setColor(QtGui.QPalette.Foreground,QtCore.Qt.blue)
#        self.speech_label.setPalette(palette)
#        speech_box.addWidget(self.speech_label)

#        large_box.addLayout(speech_box)


        # BASE MOVEMENT CONTROLS
        base_controls = QtGui.QHBoxLayout()
        base_label = QtGui.QLabel('Base Movement: ')
        base_label.setGeometry(100,100,100,100)
        

        move_speed_controls = QtGui.QVBoxLayout()
        move_speed_controls.addWidget(base_label)

        move_speed_label = QtGui.QLabel('Move Speed')
        move_speed_controls.addWidget(move_speed_label)
        move_speed_sldr = self.create_vert_sldr()
        move_speed_sldr.valueChanged[int].connect(self._change_move_speed)
        move_speed_controls.addWidget(move_speed_sldr)


        move_base_controls = QtGui.QVBoxLayout()

        move_fwd_controls = QtGui.QHBoxLayout()
        move_fwd_right_btn = self.create_toggle_btn("Forward Left")
        move_fwd_right_btn.clicked.connect(self.make_move_base_cb(0.3))
        move_fwd_controls.addWidget(move_fwd_right_btn)
        move_fwd_btn = self.create_toggle_btn("Forward")
        move_fwd_btn.clicked.connect(self.make_move_base_cb(0.3))
        move_fwd_controls.addWidget(move_fwd_btn)
        move_fwd_left_btn = self.create_toggle_btn("Forward Right")
        move_fwd_left_btn.clicked.connect(self.make_move_base_cb(0.3))
        move_fwd_controls.addWidget(move_fwd_left_btn)

        turn_controls = QtGui.QHBoxLayout()
        turn_left_btn = self.create_toggle_btn("Turn Left")
        turn_left_btn.clicked.connect(self.make_move_base_cb(0.3))
        turn_controls.addWidget(turn_left_btn)
        stop_btn = self.create_toggle_btn("STOP")
        turn_controls.addWidget(stop_btn)
        turn_right_btn = self.create_toggle_btn("Turn Right")
        turn_right_btn.clicked.connect(self.make_move_base_cb(0.3))
        turn_controls.addWidget(turn_right_btn)

        move_bkwd_controls = QtGui.QHBoxLayout()
        move_bkwd_right_btn = self.create_toggle_btn("Backward Left")
        move_bkwd_right_btn.clicked.connect(self.make_move_base_cb(0.3))
        move_bkwd_controls.addWidget(move_bkwd_right_btn)
        move_bkwd_btn = self.create_toggle_btn("Backward")
        move_bkwd_btn.clicked.connect(self.make_move_base_cb(0.3))
        move_bkwd_controls.addWidget(move_bkwd_btn)
        move_bkwd_left_btn = self.create_toggle_btn("Backward Right")
        move_bkwd_left_btn.clicked.connect(self.make_move_base_cb(0.3))
        move_bkwd_controls.addWidget(move_bkwd_left_btn)

        move_base_controls.addLayout(move_fwd_controls)
        move_base_controls.addLayout(turn_controls)
        move_base_controls.addLayout(move_bkwd_controls)


        base_controls.addLayout(move_speed_controls)
        base_controls.addLayout(move_base_controls)

        upper_box.addLayout(base_controls)


        # HEAD CONTROLS
        move_head_controls = QtGui.QHBoxLayout()

        head_label = QtGui.QLabel('Head Position: ')
        move_head_controls.addWidget(head_label)

        move_vert_controls = QtGui.QHBoxLayout()
        move_vert_sldr = self.create_vert_sldr()
        move_vert_sldr.valueChanged[int].connect(self.move_head_vert)

        move_hor_controls = QtGui.QHBoxLayout()
        move_hor_sldr = self.create_hor_sldr()
        move_hor_sldr.valueChanged[int].connect(self.move_head_hor)

        move_vert_controls.addWidget(move_vert_sldr)
        move_hor_controls.addWidget(move_hor_sldr)

        move_head_controls.addLayout(move_vert_controls)
        move_head_controls.addLayout(move_hor_controls)

        upper_box.addLayout(move_head_controls)
       
        # ARM CONTROLS

        arm_controls = QtGui.QVBoxLayout()
        arm_r_controls = QtGui.QVBoxLayout()
        arm_l_controls = QtGui.QVBoxLayout()
        arm_r_controls.addWidget(QtGui.QLabel('Right Arm: '))
        arm_l_controls.addWidget(QtGui.QLabel('Left Arm: '))
        
        arm_l_control_grid = QtGui.QGridLayout()
        arm_r_control_grid = QtGui.QGridLayout()

        #for i in range(1, 4):
         #   slider = QtGui.QSlider(QtCore.Qt.Vertical, self._widget)
          #  slider.setFocusPolicy(QtCore.Qt.NoFocus)
           # slider.setGeometry(30, 40, 30, 100)
            #arm_l_control_grid.addWidget(slider, 0, i-1)
            #arm_l_control_grid.addWidget(QtGui.QLabel(str(i)), 1, i-1)


        #for i in range(1, 4):
         #   slider = QtGui.QSlider(QtCore.Qt.Vertical, self._widget)
          #  slider.setFocusPolicy(QtCore.Qt.NoFocus)
           # slider.setGeometry(30, 40, 30, 100)
            #arm_r_control_grid.addWidget(slider, 0, i-1)
            #arm_r_control_grid.addWidget(QtGui.QLabel(str(i)), 1, i-1)

        arm_l_controls.addLayout(arm_l_control_grid)
        arm_r_controls.addLayout(arm_r_control_grid)

        arm_controls.addLayout(arm_l_controls)
        arm_controls.addLayout(arm_r_controls)
      
        # large_box.addLayout(arm_controls)

        # SET EVERYTHING
        QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10))
        self._widget.setObjectName('SimpleGUI')
        self._widget.setLayout(large_box)
        context.add_widget(self._widget)
        
    def sound_cb(self, sound_request):
        qWarning('Received sound.')
        self.sound_sig.emit(sound_request)
        
    def create_btn(self, name):
        btn = QtGui.QPushButton(name, self._widget)
        btn.clicked.connect(self.command_cb)
        return btn

    def create_toggle_btn(self, name):
        btn = QtGui.QPushButton(name, self._widget)
        btn.setAutoRepeat(True)
        btn.setAutoRepeatInterval(10)
        return btn

    def create_vert_sldr(self):
        sldr = QtGui.QSlider()
        sldr.setSliderPosition(50)
        return sldr

    def create_hor_sldr(self):
        sldr = QtGui.QSlider(QtCore.Qt.Horizontal)
        sldr.setSliderPosition(50)
        return sldr

    def sound_sig_cb(self, sound_request):
        qWarning('Received sound signal.')
        #if (sound_request.command == SoundRequest.SAY):
        qWarning('Robot said: ' + sound_request.arg)
        #self.speech_label.setText('Robot said: ' + sound_request.arg)

    def command_cb(self):
        btn_name = self._widget.sender().text()
        if (btn_name == 'Speak'):
            text = self.speech_text.text()
            qWarning('Robot will say: ' + text)
            self._sound_client.say(text)

    def gripper_cb(self):
        btn_name = self._widget.sender().text()
        if btn_name == 'Open L':
            self._gripper_client.command(True, False)
        elif btn_name == 'Open R':
            self._gripper_client.command(False, False)
        elif btn_name == 'Close L':
            self._gripper_client.command(True, True)
        elif btn_name == 'Close R':
            self._gripper_client.command(False, True)

    def make_move_base_cb(self, speed):
        def callback():
            self.move_base_cb(self._base_speed)
        return callback

    def move_base_cb(self, speed):
        qWarning('Received movement command.')
        btn_name = self._widget.sender().text()
        if btn_name == 'Forward':
            self._base_publisher.move_fwd_straight(speed)
        elif btn_name == 'Forward Right':
            self._base_publisher.move_fwd_right(speed)
        elif btn_name == 'Forward Left':
            self._base_publisher.move_fwd_left(speed)
        elif btn_name == 'Turn Left':
            self._base_publisher.turn_left(speed)
        elif btn_name == 'Turn Right':
            self._base_publisher.turn_right(speed)
        elif btn_name == 'Backward':
            self._base_publisher.move_bkwd_straight(speed)
        elif btn_name == 'Backward Right':
            self._base_publisher.move_bkwd_right(speed)
        elif btn_name == 'Backward Left':
            self._base_publisher.move_bkwd_left(speed)

    def _change_move_speed(self, speed_percent):
        self._base_speed = speed_percent / 100.0

    def move_head_vert(self, pos):
        self._head_client.move_head_vert(pos)

    def move_head_hor(self, pos):
        self._head_client.move_head_hor(pos)
 
    def shutdown_plugin(self):
        # nothing to cleanup
        pass

    def save_settings(self, plugin_settings, instance_settings):
        # instance_settings.set_value(k, v)
        pass

    def restore_settings(self, plugin_settings, instance_settings):
        # v = instance_settings.value(k)
        pass