Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Revert "Quick uopdate for torsal adjustment." #239

Merged
merged 1 commit into from
Jul 6, 2024
Merged

Revert "Quick uopdate for torsal adjustment." #239

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
12 changes: 6 additions & 6 deletions skills/config/motions.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -84,33 +84,33 @@ play_motion:
u3l:
joints: [torso_lift_joint, head_1_joint, head_2_joint]
points:
- positions: [0.4, 0.35, 0.05]
- positions: [0.4, 0.35, 0.15]
time_from_start: 0.0
u3m:
joints: [torso_lift_joint, head_1_joint, head_2_joint]
points:
- positions: [0.4, 0.0, 0.05]
- positions: [0.4, 0.0, 0.15]
time_from_start: 0.0
u3r:
joints: [torso_lift_joint, head_1_joint, head_2_joint]
points:
- positions: [0.4, -0.35, 0.05]
- positions: [0.4, -0.35, 0.15]
time_from_start: 0.0

u2l:
joints: [torso_lift_joint, head_1_joint, head_2_joint]
points:
- positions: [0.35, 0.35, 0.05]
- positions: [0.4, 0.35, 0.05]
time_from_start: 0.0
u2m:
joints: [torso_lift_joint, head_1_joint, head_2_joint]
points:
- positions: [0.35, 0.0, 0.05]
- positions: [0.4, 0.0, 0.05]
time_from_start: 0.0
u2r:
joints: [torso_lift_joint, head_1_joint, head_2_joint]
points:
- positions: [0.35, -0.35, 0.05]
- positions: [0.4, -0.35, 0.05]
time_from_start: 0.0
u1l:
joints: [torso_lift_joint, head_1_joint, head_2_joint]
Expand Down
6 changes: 1 addition & 5 deletions skills/scripts/unit_test_adjust_camera.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,11 +11,7 @@
with sm:
sm.add(
"AdjustCamera",
AdjustCamera(
max_attempts=1000,
debug=True,
init_state="u1m",
),
AdjustCamera(debug=True),
transitions={"finished": "end", "failed": "end", "truncated": "end"},
)
sm.execute()
Expand Down
150 changes: 89 additions & 61 deletions skills/src/lasr_skills/adjust_camera.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -77,17 +77,14 @@
(0, 1): "mr",
}

inverse_position_dict = {value: key for key, value in position_dict.items()}


class AdjustCamera(smach.StateMachine):
def __init__(
self,
bodypix_model: str = "resnet50",
bodypix_confidence: float = 0.7,
max_attempts=5,
max_attempts=1000,
debug=False,
init_state="u1m",
):
smach.StateMachine.__init__(
self,
Expand All @@ -108,8 +105,8 @@ def __init__(

with self:
smach.StateMachine.add(
"init",
PlayMotion(motion_name=init_state),
"init_u2m",
PlayMotion(motion_name="u2m"),
transitions={
"succeeded": "GET_IMAGE",
"aborted": "GET_IMAGE",
Expand All @@ -125,7 +122,7 @@ def __init__(
else:
_transitions = {
"succeeded": "DECIDE_ADJUST_CAMERA",
"failed": "GET_IMAGE",
"failed": "failed",
}
smach.StateMachine.add(
"GET_IMAGE",
Expand All @@ -134,7 +131,10 @@ def __init__(
method="closest",
use_mask=True,
),
transitions=_transitions,
transitions={
"succeeded": "DECIDE_ADJUST_CAMERA",
"failed": "failed",
},
)

if debug:
Expand All @@ -158,7 +158,6 @@ def __init__(
bodypix_model=bodypix_model,
bodypix_confidence=bodypix_confidence,
max_attempts=max_attempts,
init_state=init_state,
),
transitions=_transitions,
)
Expand All @@ -177,10 +176,10 @@ def __init__(
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,
init_state="u1m",
):
smach.State.__init__(
self,
Expand All @@ -196,16 +195,18 @@ def __init__(
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 = [i for i in inverse_position_dict[init_state]]
self.position = [2, 0]
self.counter = 0

def execute(self, userdata):

req = BodyPixKeypointDetectionRequest()
req.image_raw = userdata.img_msg
req.dataset = self._bodypix_model
Expand Down Expand Up @@ -242,15 +243,10 @@ def execute(self, userdata):
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:
# This is the case that not any centre points can be used,
# In this case most likely it is the guest standing either too close or not in the camera at all.
# However we may still try to get this person back into the frame if some part of them are detected.
# Otherwise we say something like "Please stand in front of me but keep a bit distance.".
rospy.logwarn(
"The person might not actually be in the frame, trying to recover."
)
# '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
Expand All @@ -267,7 +263,14 @@ def execute(self, userdata):
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],
Expand All @@ -277,7 +280,8 @@ def execute(self, userdata):
else self.position[1]
),
)
elif needs_to_move_right:
return position_dict[self.position]
if needs_to_move_right:
self.position = (
self.position[0],
(
Expand All @@ -286,26 +290,30 @@ def execute(self, userdata):
else self.position[1]
),
)
if not needs_to_move_up and needs_to_move_down:
if needs_to_move_up:
self.position = (
(
self.position[0] + 1
if self.position[0] < 3
else self.position[0]
),
self.position[1],
)
elif 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[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 = (
Expand All @@ -321,14 +329,13 @@ def execute(self, userdata):
# if y at upper 1/3: wonder why no shoulders but never mind in this case
else:
pass
# if x at left 2/7 or left shoulder dissappear, move left 1 step
if eyes_middle[0] <= 2 / 7:
# 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 2/7 or right shoulder dissappear, move right 1 step
elif eyes_middle[0] >= 5 / 7:
# 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 = (
(
Expand All @@ -344,14 +351,13 @@ def execute(self, userdata):
# if y at upper 1/4: up move 1 step
elif shoulders_middle[1] <= 1 / 4:
self.position[0] += 1
# if x at left 2/7, move left 1 step
if shoulders_middle[0] <= 2 / 7:
# if x at left 1/3, move left 1 step
if shoulders_middle[0] <= 1 / 3:
self.position[1] -= 1
# if x at right 2/7, move right 1 step
elif shoulders_middle[0] >= 5 / 7:
# 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,
Expand Down Expand Up @@ -389,17 +395,44 @@ def execute(self, userdata):
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 2/7, move left 1 step
if very_middle[0] <= 2 / 7:
# 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 2/7, move left 1 step.")
# if x at right 2/7, move right 1 step
elif very_middle[0] >= 5 / 7:
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 2/7, move right 1 step.")
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

# keep the position in the range.
if self.position[0] < 0:
self.position[0] = 0
elif self.position[0] > 3:
Expand All @@ -409,9 +442,4 @@ def execute(self, userdata):
elif self.position[1] > 1:
self.position[1] = 1

# if counter > maxmum.
if self.counter > self.max_attempts:
return "truncated"
self.counter += 1

return position_dict[(self.position[0], self.position[1])]
2 changes: 1 addition & 1 deletion tasks/receptionist/src/receptionist/state_machine.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -279,4 +279,4 @@ def _guide_guest(self, guest_id: int) -> None:
"preempted": "failed",
"aborted": "failed",
},
)
)
Loading
Loading