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()