Updated detect_keypoints in bodipix.

LASR-at-Home · Jun 27, 2024 · 83d3329 · 83d3329
1 parent 049ceab
commit 83d3329
Show file tree

Hide file tree

Showing 3 changed files with 13 additions and 4 deletions.
diff --git a/common/vision/lasr_vision_bodypix/src/lasr_vision_bodypix/bodypix.py b/common/vision/lasr_vision_bodypix/src/lasr_vision_bodypix/bodypix.py
@@ -150,7 +150,11 @@ def detect_keypoints(
             x = int(keypoint.position.x)
             y = int(keypoint.position.y)
             try:
-                if mask[y, x] == 0:
+                # if mask[y, x] == 0:
+                #     continue
+                if x < 0.0 or y < 0.0:
+                    continue
+                if x >= mask.shape[1] or y >= mask.shape[0]:
                     continue
             # Throws an error if the keypoint is out of bounds
             # but not clear what type (some TF stuff)

diff --git a/skills/src/lasr_skills/adjust_camera.py b/skills/src/lasr_skills/adjust_camera.py
@@ -83,4 +83,9 @@ def execute(self, userdata):
         miss_left = len(missing_keypoints.intersection(LEFT)) >= 5
         miss_right = len(missing_keypoints.intersection(RIGHT)) >= 5
         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_torso and miss_middle
+        needs_to_move_left = miss_left
+        needs_to_move_right = miss_right
+        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}.")
         return "finished"
diff --git a/skills/src/lasr_skills/describe_people.py b/skills/src/lasr_skills/describe_people.py
@@ -56,7 +56,7 @@ def __init__(self):
                     object_name="person",
                     crop_method=crop_method,
                     rgb_topic=rgb_topic,
-                    use_mask=False,
+                    use_mask=True,
                 ),
                 transitions=transitions,
             )
@@ -85,8 +85,8 @@ def __init__(self):
                     ],
                 ),
                 transitions={
-                    "succeeded": "CONVERT_IMAGE",
-                    "failed": "ADJUST_CAMERA",
+                    "succeeded": "GET_IMAGE",
+                    "failed": "GET_IMAGE",
                 },
             )