Updated missing body part detection.

MBTMBTMBT · Jun 27, 2024 · 049ceab · 049ceab
1 parent 0c1cb36
commit 049ceab
Show file tree

Hide file tree

Showing 3 changed files with 77 additions and 9 deletions.
diff --git a/skills/src/lasr_skills/adjust_camera.py b/skills/src/lasr_skills/adjust_camera.py
@@ -2,6 +2,69 @@
 import rospy
 
 
+ALL_KEYS = {
+    'nose',
+    'leftEye',
+    'rightEye',
+    'leftEar',
+    'rightEar',
+    'leftShoulder',
+    'rightShoulder',
+    # 'leftElbow',
+    # 'rightElbow',
+    'leftWrist',
+    'rightWrist',
+    'leftHip',
+    'rightHip',
+    # 'leftKnee',
+    # 'rightKnee',
+    # 'leftAnkle',
+    # 'rightAnkle'
+}
+
+LEFT = {
+    'leftEye',
+    'leftEar',
+    'leftShoulder',
+    'leftElbow',
+    'leftWrist',
+    'leftHip',
+    'leftKnee',
+    'leftAnkle',
+}
+
+RIGHT = {
+    'rightEye',
+    'rightEar',
+    'rightShoulder',
+    'rightElbow',
+    'rightWrist',
+    'rightHip',
+    'rightKnee',
+    'rightAnkle',
+}
+
+HEAD = {
+    'nose',
+    'leftEye',
+    'rightEye',
+    'leftEar',
+    'rightEar',
+}
+
+MIDDLE = {
+    'leftShoulder',
+    'rightShoulder',
+}
+
+TORSO = {
+    'leftWrist',
+    'rightWrist',
+    'leftHip',
+    'rightHip',
+}
+
+
 class AdjustCamera(smach.State):
 
     def __init__(self,):
@@ -13,5 +76,11 @@ def __init__(self,):
         )
 
     def execute(self, userdata):
-        missing_keypoints = userdata.missing_keypoints
-        print(missing_keypoints)
+        missing_keypoints = {key for key in userdata.missing_keypoints}
+        miss_head = len(missing_keypoints.intersection(HEAD)) >= 3
+        miss_middle = len(missing_keypoints.intersection(MIDDLE)) >= 2
+        miss_torso = len(missing_keypoints.intersection(TORSO)) >= 3
+        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}.")
+        return "finished"
diff --git a/skills/src/lasr_skills/describe_people.py b/skills/src/lasr_skills/describe_people.py
@@ -72,16 +72,16 @@ def __init__(self):
                         'rightEar',
                         'leftShoulder',
                         'rightShoulder',
-                        'leftElbow',
-                        'rightElbow',
+                        # 'leftElbow',
+                        # 'rightElbow',
                         'leftWrist',
                         'rightWrist',
                         'leftHip',
                         'rightHip',
-                        'leftKnee',
-                        'rightKnee',
-                        'leftAnkle',
-                        'rightAnkle'
+                        # 'leftKnee',
+                        # 'rightKnee',
+                        # 'leftAnkle',
+                        # 'rightAnkle'
                     ],
                 ),
                 transitions={

diff --git a/skills/src/lasr_skills/validate_keypoints.py b/skills/src/lasr_skills/validate_keypoints.py
@@ -28,7 +28,6 @@ def __init__(
 
             bodypix_confidence (float, optional): The confidence threshold for bodypix. Defaults to 0.7.
 
-
         """
         smach.State.__init__(
             self,
-Original file line number
+Diff line change
@@ Expand Up / @@ -28,7 +28,6 @@ def __init__( @@
                 bodypix_confidence (float, optional): The confidence threshold for bodypix. Defaults to 0.7.
             """
             smach.State.__init__(
                 self,
@@ Expand Down @@