class AutomaticMovements():
NOT_INITED_YET = 0
BAD_PLAN = 1
GOOD_PLAN = 2
MOVED_TO_POSE = 3
BAD_STARTING_POSITION = 4
GOOD_STARTING_POSITION = 5
SAMPLE_TOOK = 6
MOVEMENT_FAILED = 7
def __init__(self):
self.handeye_client = HandeyeClient()
self.state = AutomaticMovements.NOT_INITED_YET
self.samples_taken = 0
self.current_target_pose = -1 # -1 is home
self.target_poses = None
def handle_check_pose(self):
check_pose_result = self.handeye_client.check_starting_pose()
if check_pose_result.can_calibrate:
print('{} Good starting pose'.format(rospy.get_namespace()))
self.state = AutomaticMovements.GOOD_STARTING_POSITION
else:
print('{} Bad starting pose'.format(rospy.get_namespace()))
self.state = AutomaticMovements.BAD_STARTING_POSITION
self.current_target_pose = check_pose_result.target_poses.current_target_pose_index
self.target_poses = check_pose_result.target_poses.target_poses
self.plan_was_successful = None
print(self.current_target_pose)
def handle_next_pose(self):
next_pose_result = self.handeye_client.select_target_pose(
self.current_target_pose + 1)
self.current_target_pose = next_pose_result.target_poses.current_target_pose_index
self.target_poses = next_pose_result.target_poses.target_poses
self.plan_was_successful = None
self.state = AutomaticMovements.GOOD_STARTING_POSITION
print("{} Current target pose {}".format(rospy.get_namespace(),
self.current_target_pose))
def handle_plan(self):
plan_result = self.handeye_client.plan_to_selected_target_pose()
self.plan_was_successful = plan_result.success
if self.plan_was_successful:
print("{} Plan successful".format(rospy.get_namespace()))
self.state = AutomaticMovements.GOOD_PLAN
else:
print("{} Plan failed".format(rospy.get_namespace()))
self.state = AutomaticMovements.BAD_PLAN
def handle_execute(self):
if self.plan_was_successful:
execution_result = self.handeye_client.execute_plan()
if execution_result.success:
self.state = AutomaticMovements.MOVED_TO_POSE
print("{} Execution successful".format(rospy.get_namespace()))
else:
self.state = AutomaticMovements.MOVEMENT_FAILED
print("{} Execution failed".format(rospy.get_namespace()))
def handle_take_sample(self):
sample_list = self.handeye_client.take_sample()
self.samples_taken += 1
print("{} Sample {} taken ".format(rospy.get_namespace(),
self.samples_taken))
self.state = AutomaticMovements.SAMPLE_TOOK
def handle_compute_calibration(self):
result = self.handeye_client.compute_calibration()
if result.valid:
print("{} Calibration successful".format(rospy.get_namespace()))
else:
print("{} Calibration failed".format(rospy.get_namespace()))
def handle_save_calibration(self):
self.handeye_client.save()
print("{} Calibration saved".format(rospy.get_namespace()))
def handle_logic(self):
if self.state == AutomaticMovements.NOT_INITED_YET:
self.handle_check_pose()
while self.samples_taken < 15:
if self.state == AutomaticMovements.SAMPLE_TOOK:
self.handle_next_pose()
if self.state == AutomaticMovements.GOOD_STARTING_POSITION:
self.handle_plan()
if self.state == AutomaticMovements.GOOD_PLAN:
self.handle_execute()
if self.state == AutomaticMovements.MOVED_TO_POSE:
rospy.sleep(1.)
self.handle_take_sample()
self.handle_compute_calibration()
self.handle_save_calibration()
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()