Added validate keypoints into the state.

LASR-at-Home · Jun 27, 2024 · 0c1cb36 · 0c1cb36
1 parent 118b461
commit 0c1cb36
Show file tree

Hide file tree

Showing 3 changed files with 66 additions and 14 deletions.
diff --git a/skills/src/lasr_skills/__init__.py b/skills/src/lasr_skills/__init__.py
@@ -25,3 +25,4 @@
 from .wait import Wait
 from .look_to_given_point import LookToGivenPoint
 from .find_gesture_person import FindGesturePerson
+from .adjust_camera import AdjustCamera
diff --git a/skills/src/lasr_skills/adjust_camera.py b/skills/src/lasr_skills/adjust_camera.py
@@ -0,0 +1,17 @@
+import smach
+import rospy
+
+
+class AdjustCamera(smach.State):
+
+    def __init__(self,):
+        smach.State.__init__(
+            self,
+            outcomes=["finished"],
+            input_keys=["missing_keypoints"],
+            output_keys=[],
+        )
+
+    def execute(self, userdata):
+        missing_keypoints = userdata.missing_keypoints
+        print(missing_keypoints)
diff --git a/skills/src/lasr_skills/describe_people.py b/skills/src/lasr_skills/describe_people.py
@@ -18,6 +18,7 @@
 from .vision import GetCroppedImage, ImageMsgToCv2
 import numpy as np
 from lasr_skills.validate_keypoints import ValidateKeypoints
+from lasr_skills.adjust_camera import AdjustCamera
 
 
 class DescribePeople(smach.StateMachine):
@@ -41,13 +42,13 @@ def __init__(self):
             )
             if "tiago" in os.environ["ROS_MASTER_URI"]:
                 transitions = {
-                    "succeeded": "CONVERT_IMAGE",
+                    "succeeded": "CHECK_KEYPOINTS",
                     "failed": "SAY_GET_IMAGE_AGAIN",
                 }
             else:
                 transitions={
-                    "succeeded": "CONVERT_IMAGE",
-                    "failed": "CONVERT_IMAGE",
+                    "succeeded": "CHECK_KEYPOINTS",
+                    "failed": "GET_IMAGE_AGAIN",
                 }
             smach.StateMachine.add(
                 "GET_IMAGE",
@@ -60,6 +61,49 @@ def __init__(self):
                 transitions=transitions,
             )
 
+            smach.StateMachine.add(
+                "CHECK_KEYPOINTS",
+                ValidateKeypoints(
+                    keypoints_to_detect=[
+                        'nose',
+                        'leftEye',
+                        'rightEye',
+                        'leftEar',
+                        'rightEar',
+                        'leftShoulder',
+                        'rightShoulder',
+                        'leftElbow',
+                        'rightElbow',
+                        'leftWrist',
+                        'rightWrist',
+                        'leftHip',
+                        'rightHip',
+                        'leftKnee',
+                        'rightKnee',
+                        'leftAnkle',
+                        'rightAnkle'
+                    ],
+                ),
+                transitions={
+                    "succeeded": "CONVERT_IMAGE",
+                    "failed": "ADJUST_CAMERA",
+                },
+            )
+
+            if "tiago" in os.environ["ROS_MASTER_URI"]:
+                transitions = {
+                    "finished": "SAY_GET_IMAGE_AGAIN",
+                }
+            else:
+                transitions={
+                    "finished": "GET_IMAGE_AGAIN",
+                }
+            smach.StateMachine.add(
+                "ADJUST_CAMERA",
+                AdjustCamera(),
+                transitions=transitions,
+            )
+
             if "tiago" in os.environ["ROS_MASTER_URI"]:
                 smach.StateMachine.add(
                     "SAY_GET_IMAGE_AGAIN",
@@ -73,16 +117,6 @@ def __init__(self):
                     },
                 )
 
-            if "tiago" in os.environ["ROS_MASTER_URI"]:
-                transitions = {
-                    "succeeded": "CONVERT_IMAGE",
-                    "failed": "SAY_CONTINUE",
-                }
-            else:
-                transitions={
-                    "succeeded": "CONVERT_IMAGE",
-                    "failed": "CONVERT_IMAGE",
-                }
             smach.StateMachine.add(
                 "GET_IMAGE_AGAIN",
                 GetCroppedImage(
@@ -91,7 +125,7 @@ def __init__(self):
                     rgb_topic=rgb_topic,
                     use_mask=False,
                 ),
-                transitions=transitions,
+                transitions={"succeeded": "CONVERT_IMAGE", "failed": "failed"},
             )
 
             if "tiago" in os.environ["ROS_MASTER_URI"]: