Exemple #1
0
    def __init__(self):
        super(CalibrationMovementsGUI, self).__init__()
        move_group_name = rospy.get_param('~move_group', 'manipulator')
        self.angle_delta = math.radians(
            rospy.get_param('~rotation_delta_degrees', 25))
        self.translation_delta = rospy.get_param('~translation_delta_meters',
                                                 0.1)
        max_velocity_scaling = rospy.get_param('~max_velocity_scaling', 0.5)
        max_acceleration_scaling = rospy.get_param('~max_acceleration_scaling',
                                                   0.5)
        self.local_mover = CalibrationMovements(move_group_name,
                                                max_velocity_scaling,
                                                max_acceleration_scaling)
        self.current_pose = -1
        self.current_plan = None
        self.state = CalibrationMovementsGUI.NOT_INITED_YET

        self.layout = QVBoxLayout()
        self.labels_layout = QHBoxLayout()
        self.buttons_layout = QHBoxLayout()

        self.progress_bar = QProgressBar()
        self.pose_number_lbl = QLabel('0/8')
        self.bad_plan_lbl = QLabel('No plan yet')
        self.guide_lbl = QLabel('Hello')
        self.guide_lbl.setWordWrap(True)

        self.check_start_pose_btn = QPushButton('Check starting pose')
        self.check_start_pose_btn.clicked.connect(
            self.handle_check_current_state)

        self.next_pose_btn = QPushButton('Next Pose')
        self.next_pose_btn.clicked.connect(self.handle_next_pose)

        self.plan_btn = QPushButton('Plan')
        self.plan_btn.clicked.connect(self.handle_plan)

        self.execute_btn = QPushButton('Execute')
        self.execute_btn.clicked.connect(self.handle_execute)

        self.labels_layout.addWidget(self.pose_number_lbl)
        self.labels_layout.addWidget(self.bad_plan_lbl)

        self.buttons_layout.addWidget(self.check_start_pose_btn)
        self.buttons_layout.addWidget(self.next_pose_btn)
        self.buttons_layout.addWidget(self.plan_btn)
        self.buttons_layout.addWidget(self.execute_btn)

        self.layout.addWidget(self.progress_bar)
        self.layout.addLayout(self.labels_layout)
        self.layout.addWidget(self.guide_lbl)
        self.layout.addLayout(self.buttons_layout)

        self.setLayout(self.layout)

        self.plan_btn.setEnabled(False)
        self.execute_btn.setEnabled(False)

        self.setWindowTitle('Local Mover')
        self.show()
    def __init__(self):
        super(CalibrationMovementsGUI, self).__init__()
        self.handeye_client = HandeyeClient()
        self.current_target_pose = -1  # -1 is home
        self.target_poses = None
        self.plan_was_successful = None
        self.state = CalibrationMovementsGUI.NOT_INITED_YET

        self.layout = QVBoxLayout()
        self.labels_layout = QHBoxLayout()
        self.buttons_layout = QHBoxLayout()

        self.progress_bar = QProgressBar()
        self.pose_number_lbl = QLabel('0/0')
        self.bad_plan_lbl = QLabel('No plan yet')
        self.bad_plan_lbl.setAlignment(Qt.AlignCenter)
        self.guide_lbl = QLabel('Hello')
        self.guide_lbl.setWordWrap(True)

        self.check_start_pose_btn = QPushButton('Check starting pose')
        self.check_start_pose_btn.clicked.connect(
            self.handle_check_current_state)

        self.next_pose_btn = QPushButton('Next Pose')
        self.next_pose_btn.clicked.connect(self.handle_next_pose)

        self.plan_btn = QPushButton('Plan')
        self.plan_btn.clicked.connect(self.handle_plan)

        self.execute_btn = QPushButton('Execute')
        self.execute_btn.clicked.connect(self.handle_execute)

        self.labels_layout.addWidget(self.pose_number_lbl)
        self.labels_layout.addWidget(self.bad_plan_lbl)

        self.buttons_layout.addWidget(self.check_start_pose_btn)
        self.buttons_layout.addWidget(self.next_pose_btn)
        self.buttons_layout.addWidget(self.plan_btn)
        self.buttons_layout.addWidget(self.execute_btn)

        self.layout.addWidget(self.progress_bar)
        self.layout.addLayout(self.labels_layout)
        self.layout.addWidget(self.guide_lbl)
        self.layout.addLayout(self.buttons_layout)

        self.setLayout(self.layout)

        self.plan_btn.setEnabled(False)
        self.execute_btn.setEnabled(False)

        self.setWindowTitle('Local Mover')
        self.show()
    def initUI(self):
        self.layout = QVBoxLayout()
        self.labels_layout = QHBoxLayout()
        self.buttons_layout = QHBoxLayout()

        self.progress_bar = QProgressBar()
        self.pose_number_lbl = QLabel('0/8')
        self.bad_plan_lbl = QLabel('No plan yet')
        self.guide_lbl = QLabel('Hello')

        self.check_start_pose_btn = QPushButton('Check starting pose')
        self.check_start_pose_btn.clicked.connect(
            self.handle_check_current_state)

        self.next_pose_btn = QPushButton('Next Pose')
        self.next_pose_btn.clicked.connect(self.handle_next_pose)

        self.plan_btn = QPushButton('Plan')
        self.plan_btn.clicked.connect(self.handle_plan)

        self.execute_btn = QPushButton('Execute')
        self.execute_btn.clicked.connect(self.handle_execute)

        self.labels_layout.addWidget(self.pose_number_lbl)
        self.labels_layout.addWidget(self.bad_plan_lbl)

        self.buttons_layout.addWidget(self.check_start_pose_btn)
        self.buttons_layout.addWidget(self.next_pose_btn)
        self.buttons_layout.addWidget(self.plan_btn)
        self.buttons_layout.addWidget(self.execute_btn)

        self.layout.addWidget(self.progress_bar)
        self.layout.addLayout(self.labels_layout)
        self.layout.addWidget(self.guide_lbl)
        self.layout.addLayout(self.buttons_layout)

        self.setLayout(self.layout)

        self.plan_btn.setEnabled(False)
        self.execute_btn.setEnabled(False)

        self.setWindowTitle('Local Mover')
        self.show()
class CalibrationMovementsGUI(QWidget):
    NOT_INITED_YET = 0
    BAD_PLAN = 1
    GOOD_PLAN = 2
    MOVED_TO_POSE = 3
    BAD_STARTING_POSITION = 4
    GOOD_STARTING_POSITION = 5

    def __init__(self):
        super(CalibrationMovementsGUI, self).__init__()
        move_group_name = rospy.get_param('~move_group', 'manipulator')
        self.angle_delta = math.radians(
            rospy.get_param('~rotation_delta_degrees', 25))
        self.translation_delta = rospy.get_param('~translation_delta_meters',
                                                 0.1)
        max_velocity_scaling = rospy.get_param('~max_velocity_scaling', 0.5)
        max_acceleration_scaling = rospy.get_param('~max_acceleration_scaling',
                                                   0.5)
        self.local_mover = CalibrationMovements(move_group_name,
                                                max_velocity_scaling,
                                                max_acceleration_scaling)
        self.current_pose = -1
        self.current_plan = None
        self.initUI()
        self.state = CalibrationMovementsGUI.NOT_INITED_YET

    def initUI(self):
        self.layout = QVBoxLayout()
        self.labels_layout = QHBoxLayout()
        self.buttons_layout = QHBoxLayout()

        self.progress_bar = QProgressBar()
        self.pose_number_lbl = QLabel('0/8')
        self.bad_plan_lbl = QLabel('No plan yet')
        self.guide_lbl = QLabel('Hello')

        self.check_start_pose_btn = QPushButton('Check starting pose')
        self.check_start_pose_btn.clicked.connect(
            self.handle_check_current_state)

        self.next_pose_btn = QPushButton('Next Pose')
        self.next_pose_btn.clicked.connect(self.handle_next_pose)

        self.plan_btn = QPushButton('Plan')
        self.plan_btn.clicked.connect(self.handle_plan)

        self.execute_btn = QPushButton('Execute')
        self.execute_btn.clicked.connect(self.handle_execute)

        self.labels_layout.addWidget(self.pose_number_lbl)
        self.labels_layout.addWidget(self.bad_plan_lbl)

        self.buttons_layout.addWidget(self.check_start_pose_btn)
        self.buttons_layout.addWidget(self.next_pose_btn)
        self.buttons_layout.addWidget(self.plan_btn)
        self.buttons_layout.addWidget(self.execute_btn)

        self.layout.addWidget(self.progress_bar)
        self.layout.addLayout(self.labels_layout)
        self.layout.addWidget(self.guide_lbl)
        self.layout.addLayout(self.buttons_layout)

        self.setLayout(self.layout)

        self.plan_btn.setEnabled(False)
        self.execute_btn.setEnabled(False)

        self.setWindowTitle('Local Mover')
        self.show()

    def updateUI(self):
        self.progress_bar.setMaximum(len(self.local_mover.poses))
        self.progress_bar.setValue(self.current_pose + 1)
        self.pose_number_lbl.setText('{}/{}'.format(
            self.current_pose + 1, len(self.local_mover.poses)))

        if self.state == CalibrationMovementsGUI.BAD_PLAN:
            self.bad_plan_lbl.setText('BAD plan!! Don\'t do it!!!!')
            self.bad_plan_lbl.setStyleSheet('QLabel { background-color : red}')
        elif self.state == CalibrationMovementsGUI.GOOD_PLAN:
            self.bad_plan_lbl.setText('Good plan')
            self.bad_plan_lbl.setStyleSheet(
                'QLabel { background-color : green}')
        else:
            self.bad_plan_lbl.setText('No plan yet')
            self.bad_plan_lbl.setStyleSheet('')

        if self.state == CalibrationMovementsGUI.NOT_INITED_YET:
            self.guide_lbl.setText(
                'Bring the robot to a plausible position and check if it is a suitable starting pose'
            )
        elif self.state == CalibrationMovementsGUI.BAD_STARTING_POSITION:
            self.guide_lbl.setText('Cannot calibrate from current position')
        elif self.state == CalibrationMovementsGUI.GOOD_STARTING_POSITION:
            self.guide_lbl.setText('Ready to start: click on next pose')
        elif self.state == CalibrationMovementsGUI.GOOD_PLAN:
            self.guide_lbl.setText(
                'The plan seems good: press execute to move the robot')
        elif self.state == CalibrationMovementsGUI.BAD_PLAN:
            self.guide_lbl.setText('Planning failed: try again')
        elif self.state == CalibrationMovementsGUI.MOVED_TO_POSE:
            self.guide_lbl.setText(
                'Pose reached: take a sample and go on to next pose')

        can_plan = self.state == CalibrationMovementsGUI.GOOD_STARTING_POSITION
        self.plan_btn.setEnabled(can_plan)
        can_move = self.state == CalibrationMovementsGUI.GOOD_PLAN
        self.execute_btn.setEnabled(can_move)

    def handle_check_current_state(self):
        self.local_mover.compute_poses_around_current_state(
            self.angle_delta, self.translation_delta)

        joint_limits = [math.radians(90)] * 5 + [math.radians(180)] + [
            math.radians(350)
        ]  # TODO: make param
        if self.local_mover.check_poses(joint_limits):
            self.state = CalibrationMovementsGUI.GOOD_STARTING_POSITION
        else:
            self.state = CalibrationMovementsGUI.BAD_STARTING_POSITION
        self.current_pose = -1

        self.updateUI()

    def handle_next_pose(self):
        self.guide_lbl.setText('Going to center position')
        if self.current_pose != -1:
            plan = self.local_mover.plan_to_start_pose()
            if plan is None:
                self.guide_lbl.setText(
                    'Failed planning to center position: try again')
            else:
                self.local_mover.execute_plan(plan)
        if self.current_pose < len(self.local_mover.poses) - 1:
            self.current_pose += 1
        self.state = CalibrationMovementsGUI.GOOD_STARTING_POSITION
        self.updateUI()

    def handle_plan(self):
        self.guide_lbl.setText(
            'Planning to the next position. Click on execute when a good one was found'
        )
        if self.current_pose >= 0:
            self.current_plan = self.local_mover.plan_to_pose(
                self.local_mover.poses[self.current_pose])
            if CalibrationMovements.is_crazy_plan(
                    self.current_plan, self.local_mover.fallback_joint_limits
            ):  #TODO: sort out this limits story
                self.state = CalibrationMovementsGUI.BAD_PLAN
            else:
                self.state = CalibrationMovementsGUI.GOOD_PLAN
        self.updateUI()

    def handle_execute(self):
        if self.current_plan is not None:
            self.guide_lbl.setText('Going to the selected pose')
            self.local_mover.execute_plan(self.current_plan)
            self.state = CalibrationMovementsGUI.MOVED_TO_POSE
            self.updateUI()
class CalibrationMovementsGUI(QWidget):
    NOT_INITED_YET = 0
    BAD_PLAN = 1
    GOOD_PLAN = 2
    MOVED_TO_POSE = 3
    BAD_STARTING_POSITION = 4
    GOOD_STARTING_POSITION = 5
    CHECKING_STARTING_POSITION = 6
    MOVEMENT_FAILED = 7

    def __init__(self):
        super(CalibrationMovementsGUI, self).__init__()
        self.handeye_client = HandeyeClient()
        self.current_target_pose = -1  # -1 is home
        self.target_poses = None
        self.plan_was_successful = None
        self.state = CalibrationMovementsGUI.NOT_INITED_YET

        self.layout = QVBoxLayout()
        self.labels_layout = QHBoxLayout()
        self.buttons_layout = QHBoxLayout()

        self.progress_bar = QProgressBar()
        self.pose_number_lbl = QLabel('0/0')
        self.bad_plan_lbl = QLabel('No plan yet')
        self.bad_plan_lbl.setAlignment(Qt.AlignCenter)
        self.guide_lbl = QLabel('Hello')
        self.guide_lbl.setWordWrap(True)

        self.check_start_pose_btn = QPushButton('Check starting pose')
        self.check_start_pose_btn.clicked.connect(
            self.handle_check_current_state)

        self.next_pose_btn = QPushButton('Next Pose')
        self.next_pose_btn.clicked.connect(self.handle_next_pose)

        self.plan_btn = QPushButton('Plan')
        self.plan_btn.clicked.connect(self.handle_plan)

        self.execute_btn = QPushButton('Execute')
        self.execute_btn.clicked.connect(self.handle_execute)

        self.labels_layout.addWidget(self.pose_number_lbl)
        self.labels_layout.addWidget(self.bad_plan_lbl)

        self.buttons_layout.addWidget(self.check_start_pose_btn)
        self.buttons_layout.addWidget(self.next_pose_btn)
        self.buttons_layout.addWidget(self.plan_btn)
        self.buttons_layout.addWidget(self.execute_btn)

        self.layout.addWidget(self.progress_bar)
        self.layout.addLayout(self.labels_layout)
        self.layout.addWidget(self.guide_lbl)
        self.layout.addLayout(self.buttons_layout)

        self.setLayout(self.layout)

        self.plan_btn.setEnabled(False)
        self.execute_btn.setEnabled(False)

        self.setWindowTitle('Local Mover')
        self.show()

    def update_ui(self):
        if self.target_poses:
            count_target_poses = len(self.target_poses)
        else:
            count_target_poses = 1
        self.progress_bar.setMaximum(count_target_poses)
        self.progress_bar.setValue(self.current_target_pose + 1)
        self.pose_number_lbl.setText('{}/{}'.format(
            self.current_target_pose + 1, count_target_poses))

        if self.state == CalibrationMovementsGUI.BAD_PLAN:
            self.bad_plan_lbl.setText('BAD plan!! Don\'t do it!!!!')
            self.bad_plan_lbl.setStyleSheet('QLabel { background-color : red}')
        elif self.state == CalibrationMovementsGUI.GOOD_PLAN:
            self.bad_plan_lbl.setText('Good plan')
            self.bad_plan_lbl.setStyleSheet(
                'QLabel { background-color : green}')
        else:
            self.bad_plan_lbl.setText('No plan yet')
            self.bad_plan_lbl.setStyleSheet('')

        if self.state == CalibrationMovementsGUI.NOT_INITED_YET:
            self.guide_lbl.setText(
                'Bring the robot to a plausible position and check if it is a suitable starting pose'
            )
        elif self.state == CalibrationMovementsGUI.CHECKING_STARTING_POSITION:
            self.guide_lbl.setText(
                'Checking if the robot can translate and rotate in all directions from the current pose'
            )
        elif self.state == CalibrationMovementsGUI.BAD_STARTING_POSITION:
            self.guide_lbl.setText('Cannot calibrate from current position')
        elif self.state == CalibrationMovementsGUI.GOOD_STARTING_POSITION:
            self.guide_lbl.setText('Ready to start: click on next pose')
        elif self.state == CalibrationMovementsGUI.GOOD_PLAN:
            self.guide_lbl.setText(
                'The plan seems good: press execute to move the robot')
        elif self.state == CalibrationMovementsGUI.BAD_PLAN:
            self.guide_lbl.setText('Planning failed: try again')
        elif self.state == CalibrationMovementsGUI.MOVED_TO_POSE:
            self.guide_lbl.setText(
                'Pose reached: take a sample and go on to next pose')

        can_plan = self.state == CalibrationMovementsGUI.GOOD_STARTING_POSITION
        self.plan_btn.setEnabled(can_plan)
        can_move = self.state == CalibrationMovementsGUI.GOOD_PLAN
        self.execute_btn.setEnabled(can_move)
        QCoreApplication.processEvents()

    def handle_check_current_state(self):
        self.state = CalibrationMovementsGUI.CHECKING_STARTING_POSITION
        self.update_ui()
        res = self.handeye_client.check_starting_pose()
        if res.can_calibrate:
            self.state = CalibrationMovementsGUI.GOOD_STARTING_POSITION
        else:
            self.state = CalibrationMovementsGUI.BAD_STARTING_POSITION
        self.current_target_pose = res.target_poses.current_target_pose_index
        self.target_poses = res.target_poses.target_poses
        self.plan_was_successful = None

        self.update_ui()

    def handle_next_pose(self):
        res = self.handeye_client.select_target_pose(self.current_target_pose +
                                                     1)
        self.current_target_pose = res.target_poses.current_target_pose_index
        self.target_poses = res.target_poses.target_poses
        self.plan_was_successful = None

        self.state = CalibrationMovementsGUI.GOOD_STARTING_POSITION
        self.update_ui()

    def handle_plan(self):
        self.guide_lbl.setText(
            'Planning to the next position. Click on execute when a good one was found'
        )
        res = self.handeye_client.plan_to_selected_target_pose()
        self.plan_was_successful = res.success
        if self.plan_was_successful:
            self.state = CalibrationMovementsGUI.GOOD_PLAN
        else:
            self.state = CalibrationMovementsGUI.BAD_PLAN
        self.update_ui()

    def handle_execute(self):
        if self.plan_was_successful:
            self.guide_lbl.setText('Going to the selected pose')
            res = self.handeye_client.execute_plan()
            if res.success:
                self.state = CalibrationMovementsGUI.MOVED_TO_POSE
            else:
                self.state = CalibrationMovementsGUI.MOVEMENT_FAILED
            self.update_ui()
    def __init__(self, context):
        # super(BDIPelvisPoseWidget, self).__init__(context)
        # self.setObjectName('BDIPelvisPoseWidget')
        super(BDIPelvisPoseWidget, self).__init__()
        self.name = "BDIPelvisPoseWidget"

        self.updateStateSignal.connect(self.on_updateState)

        # self._widget = QWidget()
        self._widget = context
        vbox = QVBoxLayout()

        self.forward_position = 0.0
        self.lateral_position = 0.0
        self.height_position = 0.91
        self.roll_position = 0.0
        self.pitch_position = 0.0
        self.yaw_position = 0.0

        self.currentForward = 0.0
        self.currentLateral = 0.0
        self.currentHeight = 0.91
        self.currentRoll = 0.0
        self.currentPitch = 0.0
        self.currentYaw = 0.0
        # Define checkboxes
        vbox = QVBoxLayout()
        label = QLabel()
        label.setText("BDI Pelvis Height (Manipulate Mode Only)")  # todo - connect controller mode
        vbox.addWidget(label)

        self.enable_checkbox = QCheckBox("Enable")
        self.enable_checkbox.stateChanged.connect(self.on_enable_check)
        vbox.addWidget(self.enable_checkbox)

        self.snap_to_current_button = QPushButton("Snap to Current")
        self.snap_to_current_button.pressed.connect(self.on_snapCurrentPressed)
        vbox.addWidget(self.snap_to_current_button)

        self.roll_slider = QSlider(Qt.Horizontal)
        self.roll_label = QLabel()
        self.roll_label.setText("Roll")
        vbox.addWidget(self.roll_label)

        self.roll_slider.setRange(int(-100), int(101))
        self.roll_slider.setValue(int(0))
        self.roll_slider.setSingleStep((200) / 50)
        self.roll_slider.setTickInterval(25)
        self.roll_slider.valueChanged.connect(self.on_rollSliderMoved)
        vbox.addWidget(self.roll_slider)

        self.roll_progress_bar = QProgressBar()
        self.roll_progress_bar.setRange(int(-100), int(101))
        self.roll_progress_bar.setValue((6.0 / math.pi) * self.currentRoll * 100)
        self.roll_progress_bar.setFormat("%.6f" % self.currentRoll)
        vbox.addWidget(self.roll_progress_bar)

        self.pitch_slider = QSlider(Qt.Horizontal)
        self.pitch_label = QLabel()
        self.pitch_label.setText("Pitch")
        vbox.addWidget(self.pitch_label)

        self.pitch_slider.setRange(int(-100), int(101))
        self.pitch_slider.setValue(int(0))
        self.pitch_slider.setSingleStep((200) / 50)
        self.pitch_slider.setTickInterval(25)
        self.pitch_slider.valueChanged.connect(self.on_pitchSliderMoved)
        vbox.addWidget(self.pitch_slider)

        self.pitch_progress_bar = QProgressBar()
        self.pitch_progress_bar.setRange(int(-100), int(101))
        self.pitch_progress_bar.setValue((6.0 / math.pi) * self.currentPitch * 100)
        self.pitch_progress_bar.setFormat("%.6f" % self.currentPitch)
        vbox.addWidget(self.pitch_progress_bar)

        self.yaw_slider = QSlider(Qt.Horizontal)
        self.yaw_label = QLabel()
        self.yaw_label.setText("Yaw")
        vbox.addWidget(self.yaw_label)

        self.yaw_slider.setRange(int(-100), int(101))
        self.yaw_slider.setValue(int(0))
        self.yaw_slider.setSingleStep((200) / 50)
        self.yaw_slider.setTickInterval(25)
        self.yaw_slider.valueChanged.connect(self.on_yawSliderMoved)
        vbox.addWidget(self.yaw_slider)

        self.yaw_progress_bar = QProgressBar()
        self.yaw_progress_bar.setRange(int(-100), int(101))
        self.yaw_progress_bar.setValue((4.0 / math.pi) * self.currentYaw * 100)
        self.yaw_progress_bar.setFormat("%.6f" % self.currentYaw)
        vbox.addWidget(self.yaw_progress_bar)

        self.forward_label = QLabel()
        self.forward_label.setText("Forward")
        vbox.addWidget(self.forward_label)

        widget = QWidget()
        hbox = QHBoxLayout()
        hbox.addStretch()
        self.forward_slider = QSlider(Qt.Vertical)
        # self.forward_slider.setText("Height")
        self.forward_slider.setRange(int(-101), int(100))
        self.forward_slider.setValue(int(0))
        self.forward_slider.setSingleStep(1)
        self.forward_slider.setTickInterval(10)
        self.forward_slider.valueChanged.connect(self.on_forwardSliderMoved)
        hbox.addWidget(self.forward_slider)

        self.forward_progress_bar = QProgressBar()
        self.forward_progress_bar.setOrientation(Qt.Vertical)
        self.forward_progress_bar.setRange(int(-100), int(101))
        self.forward_progress_bar.setValue((self.currentForward / 0.075) * 100)
        self.forward_progress_bar.setTextVisible(False)
        hbox.addWidget(self.forward_progress_bar)

        self.forward_progress_bar_label = QLabel()
        self.forward_progress_bar_label.setText("%.6f" % self.currentForward)
        hbox.addWidget(self.forward_progress_bar_label)
        hbox.addStretch()
        widget.setLayout(hbox)
        vbox.addWidget(widget)

        self.lateral_label = QLabel()
        self.lateral_label.setText("Lateral")
        vbox.addWidget(self.lateral_label)

        self.lateral_slider = QSlider(Qt.Horizontal)
        # self.lateral_slider.setText("Lateral")
        self.lateral_slider.setRange(int(-100), int(101))
        self.lateral_slider.setValue(int(0))
        self.lateral_slider.setSingleStep((200) / 50)
        self.lateral_slider.setTickInterval(25)
        self.lateral_slider.valueChanged.connect(self.on_lateralSliderMoved)
        vbox.addWidget(self.lateral_slider)

        self.lateral_progress_bar = QProgressBar()
        self.lateral_progress_bar.setRange(int(-100), int(101))
        self.lateral_progress_bar.setValue((self.currentLateral / 0.15) * 100)
        self.lateral_progress_bar.setFormat("%.6f" % self.currentLateral)
        vbox.addWidget(self.lateral_progress_bar)

        self.height_label = QLabel()
        self.height_label.setText("Height")
        vbox.addWidget(self.height_label)

        widget = QWidget()
        hbox = QHBoxLayout()
        hbox.addStretch()
        self.height_slider = QSlider(Qt.Vertical)
        # self.height_slider.setText("Height")
        self.height_slider.setRange(int(55), int(105))
        self.height_slider.setValue(int(91))
        self.height_slider.setSingleStep(2)
        self.height_slider.setTickInterval(10)
        self.height_slider.valueChanged.connect(self.on_heightSliderMoved)
        hbox.addWidget(self.height_slider)

        self.height_progress_bar = QProgressBar()
        self.height_progress_bar.setOrientation(Qt.Vertical)
        self.height_progress_bar.setRange(int(55), int(105))
        self.height_progress_bar.setValue(self.currentHeight * 100)
        self.height_progress_bar.setTextVisible(False)
        hbox.addWidget(self.height_progress_bar)

        self.height_progress_bar_label = QLabel()
        self.height_progress_bar_label.setText("%.6f" % self.currentHeight)
        hbox.addWidget(self.height_progress_bar_label)
        hbox.addStretch()
        widget.setLayout(hbox)
        vbox.addWidget(widget)
        vbox.addStretch()

        self._widget.setLayout(vbox)
        # context.add_widget(self._widget)
        self.height_position = 0.91
        self.lateral_position = 0.0
        self.yaw_position = 0.0
        self.first_time = True
        self.enable_checkbox.setChecked(False)
        self.yaw_slider.setEnabled(False)
        self.roll_slider.setEnabled(False)
        self.pitch_slider.setEnabled(False)
        self.forward_slider.setEnabled(False)
        self.lateral_slider.setEnabled(False)
        self.height_slider.setEnabled(False)

        self.pub_robot = rospy.Publisher("/flor/controller/bdi_desired_pelvis_pose", PoseStamped, queue_size=10)
        # self.stateSubscriber = rospy.Subscriber('/flor/pelvis_controller/current_states',JointState, self.stateCallbackFnc)
        # self.pub_bdi_pelvis  = rospy.Publisher('/bdi_manipulate_pelvis_pose_rpy',PoseStamped)
        self.pelvis_trajectory_pub = rospy.Publisher(
            "/robot_controllers/pelvis_traj_controller/command", JointTrajectory, queue_size=10
        )
        self.stateSubscriber = rospy.Subscriber(
            "/robot_controllers/pelvis_traj_controller/state", JointTrajectoryControllerState, self.stateCallbackFnc
        )
class BDIPelvisPoseWidget(QObject):
    updateStateSignal = Signal(object)

    def __init__(self, context):
        # super(BDIPelvisPoseWidget, self).__init__(context)
        # self.setObjectName('BDIPelvisPoseWidget')
        super(BDIPelvisPoseWidget, self).__init__()
        self.name = "BDIPelvisPoseWidget"

        self.updateStateSignal.connect(self.on_updateState)

        # self._widget = QWidget()
        self._widget = context
        vbox = QVBoxLayout()

        self.forward_position = 0.0
        self.lateral_position = 0.0
        self.height_position = 0.91
        self.roll_position = 0.0
        self.pitch_position = 0.0
        self.yaw_position = 0.0

        self.currentForward = 0.0
        self.currentLateral = 0.0
        self.currentHeight = 0.91
        self.currentRoll = 0.0
        self.currentPitch = 0.0
        self.currentYaw = 0.0
        # Define checkboxes
        vbox = QVBoxLayout()
        label = QLabel()
        label.setText("BDI Pelvis Height (Manipulate Mode Only)")  # todo - connect controller mode
        vbox.addWidget(label)

        self.enable_checkbox = QCheckBox("Enable")
        self.enable_checkbox.stateChanged.connect(self.on_enable_check)
        vbox.addWidget(self.enable_checkbox)

        self.snap_to_current_button = QPushButton("Snap to Current")
        self.snap_to_current_button.pressed.connect(self.on_snapCurrentPressed)
        vbox.addWidget(self.snap_to_current_button)

        self.roll_slider = QSlider(Qt.Horizontal)
        self.roll_label = QLabel()
        self.roll_label.setText("Roll")
        vbox.addWidget(self.roll_label)

        self.roll_slider.setRange(int(-100), int(101))
        self.roll_slider.setValue(int(0))
        self.roll_slider.setSingleStep((200) / 50)
        self.roll_slider.setTickInterval(25)
        self.roll_slider.valueChanged.connect(self.on_rollSliderMoved)
        vbox.addWidget(self.roll_slider)

        self.roll_progress_bar = QProgressBar()
        self.roll_progress_bar.setRange(int(-100), int(101))
        self.roll_progress_bar.setValue((6.0 / math.pi) * self.currentRoll * 100)
        self.roll_progress_bar.setFormat("%.6f" % self.currentRoll)
        vbox.addWidget(self.roll_progress_bar)

        self.pitch_slider = QSlider(Qt.Horizontal)
        self.pitch_label = QLabel()
        self.pitch_label.setText("Pitch")
        vbox.addWidget(self.pitch_label)

        self.pitch_slider.setRange(int(-100), int(101))
        self.pitch_slider.setValue(int(0))
        self.pitch_slider.setSingleStep((200) / 50)
        self.pitch_slider.setTickInterval(25)
        self.pitch_slider.valueChanged.connect(self.on_pitchSliderMoved)
        vbox.addWidget(self.pitch_slider)

        self.pitch_progress_bar = QProgressBar()
        self.pitch_progress_bar.setRange(int(-100), int(101))
        self.pitch_progress_bar.setValue((6.0 / math.pi) * self.currentPitch * 100)
        self.pitch_progress_bar.setFormat("%.6f" % self.currentPitch)
        vbox.addWidget(self.pitch_progress_bar)

        self.yaw_slider = QSlider(Qt.Horizontal)
        self.yaw_label = QLabel()
        self.yaw_label.setText("Yaw")
        vbox.addWidget(self.yaw_label)

        self.yaw_slider.setRange(int(-100), int(101))
        self.yaw_slider.setValue(int(0))
        self.yaw_slider.setSingleStep((200) / 50)
        self.yaw_slider.setTickInterval(25)
        self.yaw_slider.valueChanged.connect(self.on_yawSliderMoved)
        vbox.addWidget(self.yaw_slider)

        self.yaw_progress_bar = QProgressBar()
        self.yaw_progress_bar.setRange(int(-100), int(101))
        self.yaw_progress_bar.setValue((4.0 / math.pi) * self.currentYaw * 100)
        self.yaw_progress_bar.setFormat("%.6f" % self.currentYaw)
        vbox.addWidget(self.yaw_progress_bar)

        self.forward_label = QLabel()
        self.forward_label.setText("Forward")
        vbox.addWidget(self.forward_label)

        widget = QWidget()
        hbox = QHBoxLayout()
        hbox.addStretch()
        self.forward_slider = QSlider(Qt.Vertical)
        # self.forward_slider.setText("Height")
        self.forward_slider.setRange(int(-101), int(100))
        self.forward_slider.setValue(int(0))
        self.forward_slider.setSingleStep(1)
        self.forward_slider.setTickInterval(10)
        self.forward_slider.valueChanged.connect(self.on_forwardSliderMoved)
        hbox.addWidget(self.forward_slider)

        self.forward_progress_bar = QProgressBar()
        self.forward_progress_bar.setOrientation(Qt.Vertical)
        self.forward_progress_bar.setRange(int(-100), int(101))
        self.forward_progress_bar.setValue((self.currentForward / 0.075) * 100)
        self.forward_progress_bar.setTextVisible(False)
        hbox.addWidget(self.forward_progress_bar)

        self.forward_progress_bar_label = QLabel()
        self.forward_progress_bar_label.setText("%.6f" % self.currentForward)
        hbox.addWidget(self.forward_progress_bar_label)
        hbox.addStretch()
        widget.setLayout(hbox)
        vbox.addWidget(widget)

        self.lateral_label = QLabel()
        self.lateral_label.setText("Lateral")
        vbox.addWidget(self.lateral_label)

        self.lateral_slider = QSlider(Qt.Horizontal)
        # self.lateral_slider.setText("Lateral")
        self.lateral_slider.setRange(int(-100), int(101))
        self.lateral_slider.setValue(int(0))
        self.lateral_slider.setSingleStep((200) / 50)
        self.lateral_slider.setTickInterval(25)
        self.lateral_slider.valueChanged.connect(self.on_lateralSliderMoved)
        vbox.addWidget(self.lateral_slider)

        self.lateral_progress_bar = QProgressBar()
        self.lateral_progress_bar.setRange(int(-100), int(101))
        self.lateral_progress_bar.setValue((self.currentLateral / 0.15) * 100)
        self.lateral_progress_bar.setFormat("%.6f" % self.currentLateral)
        vbox.addWidget(self.lateral_progress_bar)

        self.height_label = QLabel()
        self.height_label.setText("Height")
        vbox.addWidget(self.height_label)

        widget = QWidget()
        hbox = QHBoxLayout()
        hbox.addStretch()
        self.height_slider = QSlider(Qt.Vertical)
        # self.height_slider.setText("Height")
        self.height_slider.setRange(int(55), int(105))
        self.height_slider.setValue(int(91))
        self.height_slider.setSingleStep(2)
        self.height_slider.setTickInterval(10)
        self.height_slider.valueChanged.connect(self.on_heightSliderMoved)
        hbox.addWidget(self.height_slider)

        self.height_progress_bar = QProgressBar()
        self.height_progress_bar.setOrientation(Qt.Vertical)
        self.height_progress_bar.setRange(int(55), int(105))
        self.height_progress_bar.setValue(self.currentHeight * 100)
        self.height_progress_bar.setTextVisible(False)
        hbox.addWidget(self.height_progress_bar)

        self.height_progress_bar_label = QLabel()
        self.height_progress_bar_label.setText("%.6f" % self.currentHeight)
        hbox.addWidget(self.height_progress_bar_label)
        hbox.addStretch()
        widget.setLayout(hbox)
        vbox.addWidget(widget)
        vbox.addStretch()

        self._widget.setLayout(vbox)
        # context.add_widget(self._widget)
        self.height_position = 0.91
        self.lateral_position = 0.0
        self.yaw_position = 0.0
        self.first_time = True
        self.enable_checkbox.setChecked(False)
        self.yaw_slider.setEnabled(False)
        self.roll_slider.setEnabled(False)
        self.pitch_slider.setEnabled(False)
        self.forward_slider.setEnabled(False)
        self.lateral_slider.setEnabled(False)
        self.height_slider.setEnabled(False)

        self.pub_robot = rospy.Publisher("/flor/controller/bdi_desired_pelvis_pose", PoseStamped, queue_size=10)
        # self.stateSubscriber = rospy.Subscriber('/flor/pelvis_controller/current_states',JointState, self.stateCallbackFnc)
        # self.pub_bdi_pelvis  = rospy.Publisher('/bdi_manipulate_pelvis_pose_rpy',PoseStamped)
        self.pelvis_trajectory_pub = rospy.Publisher(
            "/robot_controllers/pelvis_traj_controller/command", JointTrajectory, queue_size=10
        )
        self.stateSubscriber = rospy.Subscriber(
            "/robot_controllers/pelvis_traj_controller/state", JointTrajectoryControllerState, self.stateCallbackFnc
        )

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

    def shutdown_plugin(self):
        # Just make sure to stop timers and publishers, unsubscribe from Topics etc in the shutdown_plugin method.
        print "Shutdown BDI pelvis height "
        self.pub_robot.unregister()
        self.stateSubscriber.unregister()

    def publishRobotPelvisPose(self):
        print "publishing new pelvis pose ..."
        bdi_pelvis_pose = PoseStamped()
        bdi_pelvis_pose.header.stamp = rospy.Time.now()
        bdi_pelvis_pose.pose.position.x = self.forward_position
        bdi_pelvis_pose.pose.position.y = self.lateral_position
        bdi_pelvis_pose.pose.position.z = self.height_position

        # Use BDI yaw*roll*pitch concatenation
        xaxis, yaxis, zaxis = (1, 0, 0), (0, 1, 0), (0, 0, 1)
        Rx = tf.transformations.rotation_matrix(self.roll_position, xaxis)
        Ry = tf.transformations.rotation_matrix(self.pitch_position, yaxis)
        Rz = tf.transformations.rotation_matrix(self.yaw_position, zaxis)
        R = tf.transformations.concatenate_matrices(Rz, Rx, Ry)
        q = tf.transformations.quaternion_from_matrix(R)

        bdi_pelvis_pose.pose.orientation.w = q[3]
        bdi_pelvis_pose.pose.orientation.x = q[0]
        bdi_pelvis_pose.pose.orientation.y = q[1]
        bdi_pelvis_pose.pose.orientation.z = q[2]

        # w   = math.cos(self.yaw_position*0.5)
        # bdi_pelvis_pose.pose.orientation.x   = 0.0
        # bdi_pelvis_pose.pose.orientation.y   = 0.0
        # bdi_pelvis_pose.pose.orientation.z   = math.sin(self.yaw_position*0.5)

        print bdi_pelvis_pose
        print q
        euler = tf.transformations.euler_from_quaternion(q)
        print euler
        self.pub_robot.publish(bdi_pelvis_pose)

        # Now publish the trajectory form for the new controllers
        trajectory = JointTrajectory()
        trajectory.header.stamp = rospy.Time.now()
        trajectory.joint_names = ["com_v1", "com_v0", "pelvis_height", "pelvis_roll", "pelvis_pitch", "pelvis_yaw"]

        trajectory.points = [JointTrajectoryPoint()]
        trajectory.points[0].positions = [
            self.lateral_position,
            self.forward_position,
            self.height_position,
            self.roll_position,
            self.pitch_position,
            self.yaw_position,
        ]
        trajectory.points[0].velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        trajectory.points[0].time_from_start = rospy.Duration(0.75)
        self.pelvis_trajectory_pub.publish(trajectory)

    ###################################
    def on_snapCurrentPressed(self):
        print "Snap ", self.name, " values to current pelvis positions"
        self.blockSignals(True)
        self.resetCurrentPelvisSliders()
        self.blockSignals(False)

    def blockSignals(self, block):
        self.yaw_slider.blockSignals(block)
        self.roll_slider.blockSignals(block)
        self.pitch_slider.blockSignals(block)
        self.forward_slider.blockSignals(block)
        self.lateral_slider.blockSignals(block)
        self.height_slider.blockSignals(block)

    def resetCurrentPelvisSliders(self):
        self.yaw_slider.setValue((4.0 / math.pi) * self.currentYaw * 100)
        self.roll_slider.setValue((6.0 / math.pi) * self.currentRoll * 100)
        self.pitch_slider.setValue((6.0 / math.pi) * self.currentPitch * 100)
        self.forward_slider.setValue((self.currentForward / 0.075) * 100)
        self.lateral_slider.setValue((self.currentLateral / 0.15) * 100)
        self.height_slider.setValue(self.currentHeight * 100)
        self.yaw_label.setText("Yaw: " + str(self.currentYaw))
        self.roll_label.setText("Roll: " + str(self.currentRoll))
        self.pitch_label.setText("Pitch: " + str(self.currentPitch))
        self.forward_label.setText("Forward: " + str(self.currentForward))
        self.lateral_label.setText("Lateral: " + str(self.currentLateral))
        self.height_label.setText("Height: " + str(self.currentHeight))
        self.forward_position = self.currentForward
        self.lateral_position = self.currentLateral
        self.height_position = self.currentHeight
        self.roll_position = self.currentRoll
        self.pitch_position = self.currentPitch
        self.yaw_position = self.currentYaw

    def on_updateState(self, jointState_msg):

        self.currentLateral = jointState_msg.actual.positions[0]
        self.currentForward = jointState_msg.actual.positions[1]
        self.currentHeight = jointState_msg.actual.positions[2]
        self.currentRoll = jointState_msg.actual.positions[3]
        self.currentPitch = jointState_msg.actual.positions[4]
        self.currentYaw = jointState_msg.actual.positions[5]

        self.yaw_progress_bar.setValue((4.0 / math.pi) * self.currentYaw * 100)
        self.yaw_progress_bar.setFormat("%.6f" % self.currentYaw)

        self.roll_progress_bar.setValue((6.0 / math.pi) * self.currentRoll * 100)
        self.roll_progress_bar.setFormat("%.6f" % self.currentRoll)

        self.pitch_progress_bar.setValue((6.0 / math.pi) * self.currentPitch * 100)
        self.pitch_progress_bar.setFormat("%.6f" % self.currentPitch)

        self.forward_progress_bar.setValue((self.currentForward / 0.075) * 100)
        self.forward_progress_bar_label.setText("%.6f" % self.currentForward)

        self.lateral_progress_bar.setValue((self.currentLateral / 0.15) * 100)
        self.lateral_progress_bar.setFormat("%.6f" % self.currentYaw)

        self.height_progress_bar.setValue(self.currentHeight * 100)
        self.height_progress_bar_label.setText("%.6f" % self.currentHeight)

    ####################################

    def on_enable_check(self, value):
        print "Toggle the enabling checkbox - current state is ", value
        self.yaw_slider.setEnabled(self.enable_checkbox.isChecked())
        self.roll_slider.setEnabled(self.enable_checkbox.isChecked())
        self.pitch_slider.setEnabled(self.enable_checkbox.isChecked())
        self.forward_slider.setEnabled(self.enable_checkbox.isChecked())
        self.lateral_slider.setEnabled(self.enable_checkbox.isChecked())
        self.height_slider.setEnabled(self.enable_checkbox.isChecked())

    def on_yawSliderMoved(self, value):
        self.yaw_position = (value / 100.0) * math.pi / 4.0
        # print "New yaw=",self.yaw_position
        self.yaw_label.setText("Yaw: " + str(self.yaw_position))

        if self.enable_checkbox.isChecked():
            self.publishRobotPelvisPose()

    def on_rollSliderMoved(self, value):
        self.roll_position = (value / 100.0) * math.pi / 6.0
        self.roll_label.setText("Roll: " + str(self.roll_position))

        if self.enable_checkbox.isChecked():
            self.publishRobotPelvisPose()

    def on_pitchSliderMoved(self, value):
        self.pitch_position = (value / 100.0) * math.pi / 6.0
        self.pitch_label.setText("Pitch: " + str(self.pitch_position))

        if self.enable_checkbox.isChecked():
            self.publishRobotPelvisPose()

    def on_forwardSliderMoved(self, value):
        self.forward_position = (value / 100.0) * 0.075
        # print "New forward=",self.forward_position
        self.forward_label.setText("Forward: " + str(self.forward_position))

        if self.enable_checkbox.isChecked():
            self.publishRobotPelvisPose()

    def on_lateralSliderMoved(self, value):
        self.lateral_position = (value / 100.0) * 0.15
        # print "New lateral=",self.lateral_position
        self.lateral_label.setText("Lateral: " + str(self.lateral_position))

        if self.enable_checkbox.isChecked():
            self.publishRobotPelvisPose()

    def on_heightSliderMoved(self, value):
        self.height_position = value / 100.0
        # print "New height=",self.height_position
        self.height_label.setText("Height: " + str(self.height_position))

        if self.enable_checkbox.isChecked():
            self.publishRobotPelvisPose()