Skip to content

Commit

Permalink
Revert "Quick update for torso adjustment. (#238)"
Browse files Browse the repository at this point in the history 
This reverts commit 66cd8b2.
  • Loading branch information
@jws-1
jws-1 authored Jul 6, 2024
1 parent 66cd8b2 commit f4efc7d
Show file tree
Hide file tree
Showing 9 changed files with 189 additions and 264 deletions.
12 changes: 6 additions & 6 deletions skills/config/motions.yaml
View file
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
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
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
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

0 comments on commit f4efc7d

Please sign in to comment.