From 6fabe99bea65b99f11147413b8146a9c8be69335 Mon Sep 17 00:00:00 2001 From: Benteng Ma Date: Wed, 3 Jul 2024 15:11:52 +0100 Subject: [PATCH] Made adjust camera an idividual state machine that runs compleately independent. --- skills/CMakeLists.txt | 1 + skills/scripts/unit_test_adjust_camera.py | 18 + skills/src/lasr_skills/adjust_camera.py | 766 +++++++----------- .../lasr_skills/vision/get_cropped_image.py | 4 + 4 files changed, 305 insertions(+), 484 deletions(-) create mode 100644 skills/scripts/unit_test_adjust_camera.py diff --git a/skills/CMakeLists.txt b/skills/CMakeLists.txt index 6a5978ed6..1b97b6944 100644 --- a/skills/CMakeLists.txt +++ b/skills/CMakeLists.txt @@ -161,6 +161,7 @@ include_directories( ## in contrast to setup.py, you can choose the destination catkin_install_python(PROGRAMS scripts/unit_test_describe_people.py + scripts/unit_test_adjust_camera.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) diff --git a/skills/scripts/unit_test_adjust_camera.py b/skills/scripts/unit_test_adjust_camera.py new file mode 100644 index 000000000..51fe74251 --- /dev/null +++ b/skills/scripts/unit_test_adjust_camera.py @@ -0,0 +1,18 @@ +#!/usr/bin/env python3 + +import smach +import rospy + +from lasr_skills import AdjustCamera + +if __name__ == "__main__": + rospy.init_node("test_adjust_camera") + sm = smach.StateMachine(outcomes=["end"], output_keys=[]) + with sm: + sm.add( + "AdjustCamera", + AdjustCamera(debug=True), + transitions={"finished": "end", "failed": "end", "truncated": "end"}, + ) + sm.execute() + rospy.signal_shutdown("down") diff --git a/skills/src/lasr_skills/adjust_camera.py b/skills/src/lasr_skills/adjust_camera.py index 603ec11c4..6f4f3c3b5 100644 --- a/skills/src/lasr_skills/adjust_camera.py +++ b/skills/src/lasr_skills/adjust_camera.py @@ -1,32 +1,16 @@ -from typing import List import smach import rospy from lasr_vision_msgs.srv import ( BodyPixKeypointDetection, BodyPixKeypointDetectionRequest, ) +from .vision import GetCroppedImage +from lasr_skills.play_motion import PlayMotion +import rospkg +import rosparam +import os -# ALL_KEYS = { -# 'nose', -# 'leftEye', -# 'rightEye', -# 'leftEar', -# 'rightEar', -# 'leftShoulder', -# 'rightShoulder', -# # 'leftElbow', -# # 'rightElbow', -# 'leftWrist', -# 'rightWrist', -# 'leftHip', -# 'rightHip', -# # 'leftKnee', -# # 'rightKnee', -# # 'leftAnkle', -# # 'rightAnkle' -# } - LEFT = { 'leftEye', # 'leftEar', @@ -63,6 +47,7 @@ ALL_KEYS = ALL_KEYS_WITHOUT_TORSO.union(TORSO) +positions = ['u3l', 'u3m', 'u3r', 'u2l', 'u2m', 'u2r', 'u1l', 'u1m', 'u1r', 'ml', 'mm', 'mr',] position_dict = { (3, -1): 'u3l', @@ -80,484 +65,297 @@ } -class AdjustCamera(smach.State): +class AdjustCamera(smach.StateMachine): def __init__( self, - # keypoints_to_detect: List[str] = ALL_KEYS, bodypix_model: str = "resnet50", bodypix_confidence: float = 0.7, max_attempts=1000, + debug=False, ): - smach.State.__init__( + smach.StateMachine.__init__( self, - outcomes=["finished", "failed", 'u3l', 'u3m', 'u3r', 'u2l', 'u2m', 'u2r', 'u1l', 'u1m', 'u1r', 'ml', 'mm', 'mr',], - input_keys=["img_msg", "position", "counter",], - output_keys=["position", "counter",], + outcomes=["finished", "failed", "truncated",], + input_keys=[], + output_keys=[], ) - self.max_attempts = max_attempts - # self._keypoints_to_detect = keypoints_to_detect - self._bodypix_model = bodypix_model - self._bodypix_confidence = bodypix_confidence - self._bodypix_client = rospy.ServiceProxy( - "/bodypix/keypoint_detection", BodyPixKeypointDetection + r = rospkg.RosPack() + els = rosparam.load_file( + os.path.join(r.get_path("lasr_skills"), "config", "motions.yaml") ) - - self.position = [2, 0] - self.counter = 0 - - def execute(self, userdata): - - req = BodyPixKeypointDetectionRequest() - req.image_raw = userdata.img_msg - req.dataset = self._bodypix_model - req.confidence = self._bodypix_confidence - req.keep_out_of_bounds = True - - try: - res = self._bodypix_client(req) - except Exception as e: - print(e) - return "failed" - - detected_keypoints = res.normalized_keypoints - - keypoint_names = [keypoint.keypoint_name for keypoint in detected_keypoints] - rospy.logwarn(f"detected: {keypoint_names}") - keypoint_info = { - keypoint.keypoint_name: [keypoint.x, keypoint.y] - for keypoint in detected_keypoints - } - missing_keypoints = { - keypoint - for keypoint in ALL_KEYS - if keypoint not in keypoint_names - } - - has_both_shoulders = len(missing_keypoints.intersection(MIDDLE)) == 0 - has_both_eyes = len(missing_keypoints.intersection(HEAD)) == 0 - - has_more_than_one_shoulder = len(missing_keypoints.intersection(MIDDLE)) <= 1 - has_more_than_one_one_eye = len(missing_keypoints.intersection(HEAD)) <= 1 - - rospy.logwarn(f"missing keypoints: {missing_keypoints}") - rospy.logwarn(f"missing shoulders: {missing_keypoints.intersection(MIDDLE)}, missing eyes: {missing_keypoints.intersection(HEAD)}") - # has_torso = len(missing_keypoints.intersection(TORSO)) <= 1 - - if not has_more_than_one_shoulder and not has_more_than_one_one_eye: - # 'Try recovery behaviour or give up, need a bit polish - miss_head = len(missing_keypoints.intersection(HEAD)) >= 2 - miss_middle = len(missing_keypoints.intersection(MIDDLE)) >= 2 - miss_torso = len(missing_keypoints.intersection(TORSO)) >= 4 - miss_left = len(missing_keypoints.intersection(LEFT)) >= 1 - miss_right = len(missing_keypoints.intersection(RIGHT)) >= 1 - rospy.logwarn(f"Attempts: {self.counter}, Missing head: {miss_head}, middle: {miss_middle}, torso: {miss_torso}, left: {miss_left}, right: {miss_right}.") - needs_to_move_up = miss_head and (not miss_torso or not miss_middle) - needs_to_move_down = not miss_head and miss_middle and miss_torso - needs_to_move_left = miss_right - needs_to_move_right = miss_left - rospy.logwarn(f"Needs to move up: {needs_to_move_up}, down: {needs_to_move_down}, left: {needs_to_move_left}, right: {needs_to_move_right}.") + for param, ns in els: + rosparam.upload_params(ns, param) + + with self: + smach.StateMachine.add( + 'init_u2m', + PlayMotion(motion_name='u2m'), + transitions={ + "succeeded": "GET_IMAGE", + "aborted": "GET_IMAGE", + "preempted": "GET_IMAGE", + }, + ) + + if debug: + _transitions = { + "succeeded": "DECIDE_ADJUST_CAMERA", + "failed": "GET_IMAGE", + } + else: + _transitions = { + "succeeded": "DECIDE_ADJUST_CAMERA", + "failed": "failed", + } + smach.StateMachine.add( + "GET_IMAGE", + GetCroppedImage( + object_name="person", + method="centered", + use_mask=True, + ), + transitions={ + "succeeded": "DECIDE_ADJUST_CAMERA", + "failed": "failed", + }, + ) + + if debug: + _transitions = { + "finished": "GET_IMAGE", + "failed": "GET_IMAGE", + "truncated": "GET_IMAGE", + } + else: + _transitions = { + "finished": "finished", + "failed": "failed", + "truncated": "truncated", + } + for position in positions: + _transitions[position] = position + + smach.StateMachine.add( + "DECIDE_ADJUST_CAMERA", + self.DecideAdjustCamera( + bodypix_model=bodypix_model, + bodypix_confidence=bodypix_confidence, + max_attempts=max_attempts, + ), + transitions=_transitions, + ) + + for motion in positions: + smach.StateMachine.add( + motion, + PlayMotion(motion_name=motion), + transitions={ + "succeeded": "GET_IMAGE", + "aborted": "GET_IMAGE", + "preempted": "GET_IMAGE", + }, + ) + + class DecideAdjustCamera(smach.State): + def __init__( + self, + # keypoints_to_detect: List[str] = ALL_KEYS, + bodypix_model: str = "resnet50", + bodypix_confidence: float = 0.7, + max_attempts=1000, + ): + smach.State.__init__( + self, + outcomes=["finished", "failed", 'truncated',] + positions, + input_keys=["img_msg",], + output_keys=[], + ) + self.max_attempts = max_attempts + # self._keypoints_to_detect = keypoints_to_detect + self._bodypix_model = bodypix_model + self._bodypix_confidence = bodypix_confidence + self._bodypix_client = rospy.ServiceProxy( + "/bodypix/keypoint_detection", BodyPixKeypointDetection + ) + + self.position = [2, 0] + self.counter = 0 - # if counter > maxmum, check if head is in, if not, move up to get head, otherwise return finished. - if self.counter > self.max_attempts: - if not miss_head or self.counter > self.max_attempts + 2: - return "finished" + def execute(self, userdata): - self.counter += 1 - if not (needs_to_move_left and needs_to_move_right): - # return "failed" - if needs_to_move_left: - self.position = (self.position[0], self.position[1] - 1 if self.position[1] > -1 else self.position[1]) - return position_dict[self.position] - if needs_to_move_right: - self.position = (self.position[0], self.position[1] + 1 if self.position[1] < 1 else self.position[1]) - return position_dict[self.position] - if needs_to_move_up and needs_to_move_down: + req = BodyPixKeypointDetectionRequest() + req.image_raw = userdata.img_msg + req.dataset = self._bodypix_model + req.confidence = self._bodypix_confidence + req.keep_out_of_bounds = True + + try: + res = self._bodypix_client(req) + except Exception as e: + print(e) return "failed" - if needs_to_move_up: - self.position = (self.position[0] + 1 if self.position[0] < 3 else self.position[0], self.position[1]) - return position_dict[userdata.position] - if needs_to_move_down: - self.position = (self.position[0] - 1 if self.position[0] > 0 else self.position[0], self.position[1]) - return position_dict[userdata.position] - return "finished" - elif has_both_eyes and not has_both_shoulders: - # in this case try to make eyes into the upper 1/3 of the frame, - eyes_middle = ((keypoint_info["leftEye"][0] + keypoint_info["rightEye"][0]) / 2, (keypoint_info["leftEye"][1] + keypoint_info["rightEye"][1]) / 2) - # if y at down 1/5: down move 2 steps - if eyes_middle[1] >= 4/5: - self.position[0] -= 2 - # if y at down 1/2: down move 1 step - elif eyes_middle[1] >= 1/2: - self.position[0] -= 1 - # if y at upper 1/3: wonder why no shoulders but never mind in this case - else: + + detected_keypoints = res.normalized_keypoints + + keypoint_names = [keypoint.keypoint_name for keypoint in detected_keypoints] + rospy.logwarn(f"detected: {keypoint_names}") + keypoint_info = { + keypoint.keypoint_name: [keypoint.x, keypoint.y] + for keypoint in detected_keypoints + } + missing_keypoints = { + keypoint + for keypoint in ALL_KEYS + if keypoint not in keypoint_names + } + + has_both_shoulders = len(missing_keypoints.intersection(MIDDLE)) == 0 + has_both_eyes = len(missing_keypoints.intersection(HEAD)) == 0 + + has_more_than_one_shoulder = len(missing_keypoints.intersection(MIDDLE)) <= 1 + has_more_than_one_one_eye = len(missing_keypoints.intersection(HEAD)) <= 1 + + rospy.logwarn(f"missing keypoints: {missing_keypoints}") + rospy.logwarn(f"missing shoulders: {missing_keypoints.intersection(MIDDLE)}, missing eyes: {missing_keypoints.intersection(HEAD)}") + # has_torso = len(missing_keypoints.intersection(TORSO)) <= 1 + + if not has_more_than_one_shoulder and not has_more_than_one_one_eye: + # 'Try recovery behaviour or give up, need a bit polish + miss_head = len(missing_keypoints.intersection(HEAD)) >= 2 + miss_middle = len(missing_keypoints.intersection(MIDDLE)) >= 2 + miss_torso = len(missing_keypoints.intersection(TORSO)) >= 4 + miss_left = len(missing_keypoints.intersection(LEFT)) >= 1 + miss_right = len(missing_keypoints.intersection(RIGHT)) >= 1 + rospy.logwarn(f"Missing head: {miss_head}, middle: {miss_middle}, torso: {miss_torso}, left: {miss_left}, right: {miss_right}.") + needs_to_move_up = miss_head and (not miss_torso or not miss_middle) + needs_to_move_down = not miss_head and miss_middle and miss_torso + needs_to_move_left = miss_right + needs_to_move_right = miss_left + rospy.logwarn(f"Needs to move up: {needs_to_move_up}, down: {needs_to_move_down}, left: {needs_to_move_left}, right: {needs_to_move_right}.") + + # if counter > maxmum, check if head is in, if not, move up to get head, otherwise return finished. + if self.counter > self.max_attempts: + if not miss_head or self.counter > self.max_attempts + 2: + return "truncated" + + # self.counter += 1 + if not (needs_to_move_left and needs_to_move_right): + # return "failed" + if needs_to_move_left: + self.position = (self.position[0], self.position[1] - 1 if self.position[1] > -1 else self.position[1]) + return position_dict[self.position] + if needs_to_move_right: + self.position = (self.position[0], self.position[1] + 1 if self.position[1] < 1 else self.position[1]) + return position_dict[self.position] + if needs_to_move_up and needs_to_move_down: + return "failed" + if needs_to_move_up: + self.position = (self.position[0] + 1 if self.position[0] < 3 else self.position[0], self.position[1]) + return position_dict[userdata.position] + if needs_to_move_down: + self.position = (self.position[0] - 1 if self.position[0] > 0 else self.position[0], self.position[1]) + return position_dict[userdata.position] + return "finished" + elif has_both_eyes and not has_both_shoulders: + # in this case try to make eyes into the upper 1/3 of the frame, + eyes_middle = ((keypoint_info["leftEye"][0] + keypoint_info["rightEye"][0]) / 2, (keypoint_info["leftEye"][1] + keypoint_info["rightEye"][1]) / 2) + # if y at down 1/5: down move 2 steps + if eyes_middle[1] >= 4/5: + self.position[0] -= 2 + # if y at down 1/2: down move 1 step + elif eyes_middle[1] >= 1/2: + self.position[0] -= 1 + # if y at upper 1/3: wonder why no shoulders but never mind in this case + else: + pass + # if x at left 1/3 or left shoulder dissappear, move left 1 step + if eyes_middle[0] <= 1/3: + self.position[1] -= 1 + # if x at right 1/3 or right shoulder dissappear, move right 1 step + elif eyes_middle[0] >= 2/3: + self.position[1] += 1 pass - # if x at left 1/3 or left shoulder dissappear, move left 1 step - if eyes_middle[0] <= 1/3: - self.position[1] -= 1 - # if x at right 1/3 or right shoulder dissappear, move right 1 step - elif eyes_middle[0] >= 2/3: - self.position[1] += 1 - pass - elif not has_both_eyes and has_both_shoulders: - shoulders_middle = ((keypoint_info["leftShoulder"][0] + keypoint_info["rightShoulder"][0]) / 2, (keypoint_info["leftEye"][1] + keypoint_info["rightEye"][1]) / 2) - # if y at down 1/5: down move 1 step - if shoulders_middle[1] >= 4/5: - self.position[0] -= 1 - # if y at upper 1/4: up move 1 step - elif shoulders_middle[1] <= 1/4: - self.position[0] += 1 - # if x at left 1/3, move left 1 step - if shoulders_middle[0] <= 1/3: - self.position[1] -= 1 - # if x at right 1/3, move right 1 step - elif shoulders_middle[0] >= 2/3: - self.position[1] += 1 - pass - elif has_both_eyes and has_both_shoulders: - eyes_middle = ((keypoint_info["leftEye"][0] + keypoint_info["rightEye"][0]) / 2, (keypoint_info["leftEye"][1] + keypoint_info["rightEye"][1]) / 2) - shoulders_middle = ((keypoint_info["leftShoulder"][0] + keypoint_info["rightShoulder"][0]) / 2, (keypoint_info["leftEye"][1] + keypoint_info["rightEye"][1]) / 2) - very_middle = ((eyes_middle[0] + shoulders_middle[0]) / 2, (eyes_middle[1] + shoulders_middle[1]) / 2) - rospy.logwarn(f"very middle {very_middle}") - # if y at upper 1/5 for eyes: move up 1 step - if eyes_middle[1] <= 1/5: - self.position[0] += 1 - print('if y at upper 1/5 for eyes: move up 1 step') - else: - if 1/4 <= very_middle[1] <= 2/3 and 1/3 <= very_middle[0] <= 2/3: - print('finished.') - return "finished" - # if y at down 1/3: down move 1 step - if very_middle[1] >= 2/3: + elif not has_both_eyes and has_both_shoulders: + shoulders_middle = ((keypoint_info["leftShoulder"][0] + keypoint_info["rightShoulder"][0]) / 2, (keypoint_info["leftEye"][1] + keypoint_info["rightEye"][1]) / 2) + # if y at down 1/5: down move 1 step + if shoulders_middle[1] >= 4/5: self.position[0] -= 1 - print('if y at down 1/3: down move 1 step.') # if y at upper 1/4: up move 1 step - elif very_middle[1] <= 1/4: + elif shoulders_middle[1] <= 1/4: self.position[0] += 1 - print('if y at upper 1/3: up move 1 step.') - # if x at left 1/3, move left 1 step - if very_middle[0] <= 1/3: - self.position[1] -= 1 - print('if x at left 1/3, move left 1 step.') - # if x at right 1/3, move right 1 step - elif very_middle[0] >= 2/3: - self.position[1] += 1 - print('if x at right 1/3, move right 1 step.') - pass - elif has_more_than_one_shoulder: # but not both - # shoulders_middle = ((keypoint_info["leftShoulder"][0] + keypoint_info["rightShoulder"][0]) / 2, (keypoint_info["leftEye"][1] + keypoint_info["rightEye"][1]) / 2) - # # move one step opposite left or right - # # if x at left 1/3, move left 1 step - # if shoulders_middle[0] <= 1/3: - # position[1] -= 1 - # # if x at right 1/3, move right 1 step - # elif shoulders_middle[0] >= 2/3: - # position[1] += 1 - # pass - # if not has_more_than_one_one_eye: - # # move up! - # position[0] += 1 - # pass - pass - else: # has_more_than_one_one_eye: - # eyes_middle = ((keypoint_info["leftEye"][0] + keypoint_info["rightEye"][0]) / 2, (keypoint_info["leftEye"][1] + keypoint_info["rightEye"][1]) / 2) - # # move one step opposite, - # # if x at left 1/3, move left 1 step - # if eyes_middle[0] <= 1/3: - # position[1] += 1 - # # if x at right 1/3, move right 1 step - # elif eyes_middle[0] >= 2/3: - # position[1] -= 1 - # # probably move down - # position[0] -= 1 - # pass - pass - - if self.position[0] < 0: - self.position[0] = 0 - elif self.position[0] > 3: - self.position[0] = 3 - if self.position[1] < -1: - self.position[1] = -1 - elif self.position[1] > 1: - self.position[1] = 1 - - return position_dict[(self.position[0], self.position[1])] - - -# class AdjustCamera(smach.State): -# def __init__( -# self, -# # keypoints_to_detect: List[str] = ALL_KEYS, -# bodypix_model: str = "resnet50", -# bodypix_confidence: float = 0.7, -# max_attempts=1000, -# ): -# smach.State.__init__( -# self, -# outcomes=["finished", "failed", 'u3l', 'u3m', 'u3r', 'u2l', 'u2m', 'u2r', 'u1l', 'u1m', 'u1r', 'ml', 'mm', 'mr',], -# input_keys=["img_msg", "position", "counter",], -# output_keys=["position", "counter",], -# ) -# self.max_attempts = max_attempts -# # self._keypoints_to_detect = keypoints_to_detect -# self._bodypix_model = bodypix_model -# self._bodypix_confidence = bodypix_confidence -# self._bodypix_client = rospy.ServiceProxy( -# "/bodypix/keypoint_detection", BodyPixKeypointDetection -# ) - -# self.position = (2, 0) -# self.counter = 0 - -# def execute(self, userdata): - -# req = BodyPixKeypointDetectionRequest() -# req.image_raw = userdata.img_msg -# req.dataset = self._bodypix_model -# req.confidence = self._bodypix_confidence -# req.keep_out_of_bounds = True - -# try: -# res = self._bodypix_client(req) -# except Exception as e: -# print(e) -# return "failed" - -# detected_keypoints = res.normalized_keypoints - -# keypoint_names = [keypoint.keypoint_name for keypoint in detected_keypoints] -# rospy.logwarn(f"detected: {keypoint_names}") -# keypoint_info = { -# keypoint.keypoint_name: [keypoint.x, keypoint.y] -# for keypoint in detected_keypoints -# } -# missing_keypoints = { -# keypoint -# for keypoint in ALL_KEYS -# if keypoint not in keypoint_names -# } -# try: -# position = [*userdata.position] -# counter = userdata.counter -# except KeyError: -# userdata.position = (2, 0) -# position = [*userdata.position] -# userdata.counter = 0 -# counter = userdata.counter - -# print(position) - -# has_both_shoulders = len(missing_keypoints.intersection(MIDDLE)) == 0 -# has_both_eyes = len(missing_keypoints.intersection(HEAD)) == 0 - -# has_more_than_one_shoulder = len(missing_keypoints.intersection(MIDDLE)) <= 1 -# has_more_than_one_one_eye = len(missing_keypoints.intersection(HEAD)) <= 1 + # if x at left 1/3, move left 1 step + if shoulders_middle[0] <= 1/3: + self.position[1] -= 1 + # if x at right 1/3, move right 1 step + elif shoulders_middle[0] >= 2/3: + self.position[1] += 1 + pass + elif has_both_eyes and has_both_shoulders: + eyes_middle = ((keypoint_info["leftEye"][0] + keypoint_info["rightEye"][0]) / 2, (keypoint_info["leftEye"][1] + keypoint_info["rightEye"][1]) / 2) + shoulders_middle = ((keypoint_info["leftShoulder"][0] + keypoint_info["rightShoulder"][0]) / 2, (keypoint_info["leftEye"][1] + keypoint_info["rightEye"][1]) / 2) + very_middle = ((eyes_middle[0] + shoulders_middle[0]) / 2, (eyes_middle[1] + shoulders_middle[1]) / 2) + rospy.logwarn(f"very middle {very_middle}") + # if y at upper 1/5 for eyes: move up 1 step + if eyes_middle[1] <= 1/5: + self.position[0] += 1 + print('if y at upper 1/5 for eyes: move up 1 step') + else: + if 1/4 <= very_middle[1] <= 2/3 and 1/3 <= very_middle[0] <= 2/3: + print('finished.') + return "finished" + # if y at down 1/3: down move 1 step + if very_middle[1] >= 2/3: + self.position[0] -= 1 + print('if y at down 1/3: down move 1 step.') + # if y at upper 1/4: up move 1 step + elif very_middle[1] <= 1/4: + self.position[0] += 1 + print('if y at upper 1/3: up move 1 step.') + # if x at left 1/3, move left 1 step + if very_middle[0] <= 1/3: + self.position[1] -= 1 + print('if x at left 1/3, move left 1 step.') + # if x at right 1/3, move right 1 step + elif very_middle[0] >= 2/3: + self.position[1] += 1 + print('if x at right 1/3, move right 1 step.') + pass + elif has_more_than_one_shoulder: # but not both + # shoulders_middle = ((keypoint_info["leftShoulder"][0] + keypoint_info["rightShoulder"][0]) / 2, (keypoint_info["leftEye"][1] + keypoint_info["rightEye"][1]) / 2) + # # move one step opposite left or right + # # if x at left 1/3, move left 1 step + # if shoulders_middle[0] <= 1/3: + # position[1] -= 1 + # # if x at right 1/3, move right 1 step + # elif shoulders_middle[0] >= 2/3: + # position[1] += 1 + # pass + # if not has_more_than_one_one_eye: + # # move up! + # position[0] += 1 + # pass + pass + else: # has_more_than_one_one_eye: + # eyes_middle = ((keypoint_info["leftEye"][0] + keypoint_info["rightEye"][0]) / 2, (keypoint_info["leftEye"][1] + keypoint_info["rightEye"][1]) / 2) + # # move one step opposite, + # # if x at left 1/3, move left 1 step + # if eyes_middle[0] <= 1/3: + # position[1] += 1 + # # if x at right 1/3, move right 1 step + # elif eyes_middle[0] >= 2/3: + # position[1] -= 1 + # # probably move down + # position[0] -= 1 + # pass + pass -# rospy.logwarn(f"missing keypoints: {missing_keypoints}") -# rospy.logwarn(f"missing shoulders: {missing_keypoints.intersection(MIDDLE)}, missing eyes: {missing_keypoints.intersection(HEAD)}") -# # has_torso = len(missing_keypoints.intersection(TORSO)) <= 1 - -# if not has_more_than_one_shoulder and not has_more_than_one_one_eye: -# # 'Try recovery behaviour or give up, need a bit polish -# miss_head = len(missing_keypoints.intersection(HEAD)) >= 2 -# miss_middle = len(missing_keypoints.intersection(MIDDLE)) >= 2 -# miss_torso = len(missing_keypoints.intersection(TORSO)) >= 4 -# miss_left = len(missing_keypoints.intersection(LEFT)) >= 1 -# miss_right = len(missing_keypoints.intersection(RIGHT)) >= 1 -# rospy.logwarn(f"Attempts: {counter}, Missing head: {miss_head}, middle: {miss_middle}, torso: {miss_torso}, left: {miss_left}, right: {miss_right}.") -# needs_to_move_up = miss_head and (not miss_torso or not miss_middle) -# needs_to_move_down = not miss_head and miss_middle and miss_torso -# needs_to_move_left = miss_right -# needs_to_move_right = miss_left -# rospy.logwarn(f"Needs to move up: {needs_to_move_up}, down: {needs_to_move_down}, left: {needs_to_move_left}, right: {needs_to_move_right}.") + if self.position[0] < 0: + self.position[0] = 0 + elif self.position[0] > 3: + self.position[0] = 3 + if self.position[1] < -1: + self.position[1] = -1 + elif self.position[1] > 1: + self.position[1] = 1 -# # if counter > maxmum, check if head is in, if not, move up to get head, otherwise return finished. -# if counter > self.max_attempts: -# if not miss_head or counter > self.max_attempts + 2: -# return "finished" - -# counter += 1 -# userdata.counter = counter -# if not (needs_to_move_left and needs_to_move_right): -# # return "failed" -# if needs_to_move_left: -# userdata.position = (position[0], position[1] - 1 if position[1] > -1 else position[1]) -# return position_dict[userdata.position] -# if needs_to_move_right: -# userdata.position = (position[0], position[1] + 1 if position[1] < 1 else position[1]) -# return position_dict[userdata.position] -# if needs_to_move_up and needs_to_move_down: -# return "failed" -# if needs_to_move_up: -# userdata.position = (position[0] + 1 if position[0] < 3 else position[0], position[1]) -# return position_dict[userdata.position] -# if needs_to_move_down: -# userdata.position = (position[0] - 1 if position[0] > 0 else position[0], position[1]) -# return position_dict[userdata.position] -# return "finished" -# elif has_both_eyes and not has_both_shoulders: -# # in this case try to make eyes into the upper 1/3 of the frame, -# eyes_middle = ((keypoint_info["leftEye"][0] + keypoint_info["rightEye"][0]) / 2, (keypoint_info["leftEye"][1] + keypoint_info["rightEye"][1]) / 2) -# # if y at down 1/5: down move 2 steps -# if eyes_middle[1] >= 4/5: -# position[0] -= 2 -# # if y at down 1/2: down move 1 step -# elif eyes_middle[1] >= 1/2: -# position[0] -= 1 -# # if y at upper 1/3: wonder why no shoulders but never mind in this case -# else: -# pass -# # if x at left 1/3 or left shoulder dissappear, move left 1 step -# if eyes_middle[0] <= 1/3: -# position[1] -= 1 -# # if x at right 1/3 or right shoulder dissappear, move right 1 step -# elif eyes_middle[0] >= 2/3: -# position[1] += 1 -# pass -# elif not has_both_eyes and has_both_shoulders: -# shoulders_middle = ((keypoint_info["leftShoulder"][0] + keypoint_info["rightShoulder"][0]) / 2, (keypoint_info["leftEye"][1] + keypoint_info["rightEye"][1]) / 2) -# # if y at down 1/5: down move 1 step -# if shoulders_middle[1] >= 4/5: -# position[0] -= 1 -# # if y at upper 1/4: up move 1 step -# elif shoulders_middle[1] <= 1/4: -# position[0] += 1 -# # if x at left 1/3, move left 1 step -# if shoulders_middle[0] <= 1/3: -# position[1] -= 1 -# # if x at right 1/3, move right 1 step -# elif shoulders_middle[0] >= 2/3: -# position[1] += 1 -# pass -# elif has_both_eyes and has_both_shoulders: -# eyes_middle = ((keypoint_info["leftEye"][0] + keypoint_info["rightEye"][0]) / 2, (keypoint_info["leftEye"][1] + keypoint_info["rightEye"][1]) / 2) -# shoulders_middle = ((keypoint_info["leftShoulder"][0] + keypoint_info["rightShoulder"][0]) / 2, (keypoint_info["leftEye"][1] + keypoint_info["rightEye"][1]) / 2) -# very_middle = ((eyes_middle[0] + shoulders_middle[0]) / 2, (eyes_middle[1] + shoulders_middle[1]) / 2) -# rospy.logwarn(f"very middle {very_middle}") -# # if y at upper 1/5 for eyes: move up 1 step -# if eyes_middle[1] <= 1/5: -# position[0] += 1 -# print('if y at upper 1/5 for eyes: move up 1 step') -# else: -# if 1/4 <= very_middle[1] <= 2/3 and 1/3 <= very_middle[0] <= 2/3: -# print('finished.') -# return "finished" -# # if y at down 1/3: down move 1 step -# if very_middle[1] >= 2/3: -# position[0] -= 1 -# print('if y at down 1/3: down move 1 step.') -# # if y at upper 1/4: up move 1 step -# elif very_middle[1] <= 1/4: -# position[0] += 1 -# print('if y at upper 1/3: up move 1 step.') -# # if x at left 1/3, move left 1 step -# if very_middle[0] <= 1/3: -# position[1] -= 1 -# print('if x at left 1/3, move left 1 step.') -# # if x at right 1/3, move right 1 step -# elif very_middle[0] >= 2/3: -# position[1] += 1 -# print('if x at right 1/3, move right 1 step.') -# pass -# elif has_more_than_one_shoulder: # but not both -# # shoulders_middle = ((keypoint_info["leftShoulder"][0] + keypoint_info["rightShoulder"][0]) / 2, (keypoint_info["leftEye"][1] + keypoint_info["rightEye"][1]) / 2) -# # # move one step opposite left or right -# # # if x at left 1/3, move left 1 step -# # if shoulders_middle[0] <= 1/3: -# # position[1] -= 1 -# # # if x at right 1/3, move right 1 step -# # elif shoulders_middle[0] >= 2/3: -# # position[1] += 1 -# # pass -# # if not has_more_than_one_one_eye: -# # # move up! -# # position[0] += 1 -# # pass -# pass -# else: # has_more_than_one_one_eye: -# # eyes_middle = ((keypoint_info["leftEye"][0] + keypoint_info["rightEye"][0]) / 2, (keypoint_info["leftEye"][1] + keypoint_info["rightEye"][1]) / 2) -# # # move one step opposite, -# # # if x at left 1/3, move left 1 step -# # if eyes_middle[0] <= 1/3: -# # position[1] += 1 -# # # if x at right 1/3, move right 1 step -# # elif eyes_middle[0] >= 2/3: -# # position[1] -= 1 -# # # probably move down -# # position[0] -= 1 -# # pass -# pass - -# if position[0] < 0: -# position[0] = 0 -# elif position[0] > 3: -# position[0] = 3 -# if position[1] < -1: -# position[1] = -1 -# elif position[1] > 1: -# position[1] = 1 - -# userdata.position = (position[0], position[1]) -# print(userdata.position) -# return position_dict[userdata.position] - - -# class AdjustCamera(smach.State): - -# def __init__(self, max_attempts=1000,): -# smach.State.__init__( -# self, -# outcomes=["finished", "failed", 'u3l', 'u3m', 'u3r', 'u2l', 'u2m', 'u2r', 'u1l', 'u1m', 'u1r', 'ml', 'mm', 'mr',], -# input_keys=["missing_keypoints", "position", "counter",], -# output_keys=["position", "counter",], -# ) -# self.max_attempts = max_attempts - -# def execute(self, userdata): -# try: -# position = userdata.position -# counter = userdata.counter -# except KeyError: -# userdata.position = (2, 0) -# position = userdata.position -# userdata.counter = 0 -# counter = userdata.counter -# print(userdata.position) -# missing_keypoints = {key for key in userdata.missing_keypoints} -# miss_head = len(missing_keypoints.intersection(HEAD)) >= 2 -# miss_middle = len(missing_keypoints.intersection(MIDDLE)) >= 2 -# miss_torso = len(missing_keypoints.intersection(TORSO)) >= 4 -# miss_left = len(missing_keypoints.intersection(LEFT)) >= 1 -# miss_right = len(missing_keypoints.intersection(RIGHT)) >= 1 -# rospy.logwarn(f"Attempts: {counter}, Missing head: {miss_head}, middle: {miss_middle}, torso: {miss_torso}, left: {miss_left}, right: {miss_right}.") -# needs_to_move_up = miss_head and (not miss_torso or not miss_middle) -# needs_to_move_down = not miss_head and miss_middle and miss_torso -# needs_to_move_left = miss_right -# needs_to_move_right = miss_left -# rospy.logwarn(f"Needs to move up: {needs_to_move_up}, down: {needs_to_move_down}, left: {needs_to_move_left}, right: {needs_to_move_right}.") - -# # if counter > maxmum, check if head is in, if not, move up to get head, otherwise return finished. -# if counter > self.max_attempts: -# if not miss_head or counter > self.max_attempts + 2: -# return "finished" - -# counter += 1 -# userdata.counter = counter -# if not (needs_to_move_left and needs_to_move_right): -# # return "failed" -# if needs_to_move_left: -# userdata.position = (position[0], position[1] - 1 if position[1] > -1 else position[1]) -# return position_dict[userdata.position] -# if needs_to_move_right: -# userdata.position = (position[0], position[1] + 1 if position[1] < 1 else position[1]) -# return position_dict[userdata.position] -# if needs_to_move_up and needs_to_move_down: -# return "failed" -# if needs_to_move_up: -# userdata.position = (position[0] + 1 if position[0] < 3 else position[0], position[1]) -# return position_dict[userdata.position] -# if needs_to_move_down: -# userdata.position = (position[0] - 1 if position[0] > 0 else position[0], position[1]) -# return position_dict[userdata.position] -# return "finished" + return position_dict[(self.position[0], self.position[1])] diff --git a/skills/src/lasr_skills/vision/get_cropped_image.py b/skills/src/lasr_skills/vision/get_cropped_image.py index f804063d5..ac37ba367 100755 --- a/skills/src/lasr_skills/vision/get_cropped_image.py +++ b/skills/src/lasr_skills/vision/get_cropped_image.py @@ -12,6 +12,10 @@ class GetCroppedImage(smach.State): + """ + This state calls CroppedDetection service instead of running on its own. + THis is a much faster version than the older one. + """ def __init__( self, object_name: str,