feat: working raising hand gesture detection

Author: m-barker

common/vision/lasr_vision_bodypix/src/lasr_vision_bodypix/bodypix.py

@@ -58,6 +58,8 @@ def detect(
     Run BodyPix inference on given detection request
     """
 
+    debug_publisher = rospy.Publisher("/bodypix/debug", SensorImage, queue_size=1)
+
     # decode the image
     rospy.loginfo("Decoding")
     img = cv2_img.msg_to_cv2_img(request.image_raw)
@@ -69,6 +71,7 @@ def detect(
     # run inference
     rospy.loginfo("Running inference")
     result = model.predict_single(img)
+
     mask = result.get_mask(threshold=request.confidence)
     rospy.loginfo("Inference complete")
 
@@ -110,6 +113,7 @@ def detect(
         keypoints_msg = []
 
         for i, keypoint in pose.keypoints.items():
+
             if camel_to_snake(keypoint.part) in request.masks[0].parts:
                 keypoint_msg = BodyPixKeypoint()
                 keypoint_msg.xy = [int(keypoint.position.x), int(keypoint.position.y)]
@@ -118,7 +122,6 @@ def detect(
                 keypoints_msg.append(keypoint_msg)
 
         pose_msg.keypoints = keypoints_msg
-        pose_msg.score = pose.score
         output_poses.append(pose_msg)
 
     response = BodyPixDetectionResponse()

common/vision/lasr_vision_msgs/msg/BodyPixPose.msg

@@ -1,7 +1,2 @@
 # keypoints
-BodyPixKeypoint[] keypoints
-
-# score
-float32 score
-# x and y coordinates
-float32[] coord
+BodyPixKeypoint[] keypoints

skills/launch/gesture_with_camera.launch

@@ -0,0 +1,14 @@
+<launch>
+    <description>Perform gesture detection using the camera</description>
+    <usage doc="Run the demo"></usage>
+
+    
+    <!-- show debug topic -->
+    <node name="image_view" pkg="rqt_image_view" type="rqt_image_view" respawn="false" output="screen" args="/recognise/debug" />
+
+
+    <!-- launch video stream -->
+   <include file="$(find video_stream_opencv)/launch/camera.launch">
+        <arg name="visualize" value="true" />
+   </include>
+</launch>

skills/src/lasr_skills/__init__.py

@@ -17,3 +17,4 @@
 from .handover_object import HandoverObject
 from .ask_and_listen import AskAndListen
 from .clip_vqa import QueryImage
+from .detect_gesture import DetectGesture

skills/src/lasr_skills/detect_gesture.py

@@ -0,0 +1,103 @@
+#!/usr/bin/env python3
+import smach
+import rospy
+from lasr_skills.vision import GetImage
+from lasr_vision_msgs.srv import BodyPixDetection, BodyPixDetectionRequest
+from lasr_vision_msgs.msg import BodyPixMaskRequest
+
+
+class DetectGesture(smach.State):
+    """
+    State for detecting gestures.
+    """
+
+    def __init__(self, gesture_to_detect: str = "raising_left_hand"):
+        self.gesture_to_detect = gesture_to_detect
+        smach.State.__init__(
+            self,
+            outcomes=["succeeded", "failed"],
+            input_keys=["img_msg"],
+            output_keys=["gesture_detected"],
+        )
+
+        self.body_pix_client = rospy.ServiceProxy("/bodypix/detect", BodyPixDetection)
+
+    def execute(self, userdata):
+
+        body_pix_masks = BodyPixMaskRequest()
+        body_pix_masks.parts = [
+            "left_shoulder",
+            "right_shoulder",
+            "left_wrist",
+            "right_wrist",
+        ]
+        masks = [body_pix_masks]
+
+        req = BodyPixDetectionRequest()
+        req.image_raw = userdata.img_msg
+        req.masks = masks
+        req.dataset = "resnet50"
+        req.confidence = 0.7
+
+        try:
+            res = self.body_pix_client(req)
+        except Exception as e:
+            print(e)
+            return "failed"
+
+        part_info = {}
+        poses = res.poses
+        for pose in poses:
+            for keypoint in pose.keypoints:
+                part_info[keypoint.part] = {
+                    "x": keypoint.xy[0],
+                    "y": keypoint.xy[1],
+                    "score": keypoint.score,
+                }
+
+        if self.gesture_to_detect == "raising_left_hand":
+            if part_info["leftWrist"]["y"] < part_info["leftShoulder"]["y"]:
+                userdata.gesture_detected = True
+            else:
+                userdata.gesture_detected = False
+        elif self.gesture_to_detect == "raising_right_hand":
+            if part_info["rightWrist"]["y"] < part_info["rightShoulder"]["y"]:
+                userdata.gesture_detected = True
+            else:
+                userdata.gesture_detected = False
+        else:
+            raise ValueError("Invalid gesture to detect")
+
+        return "succeeded"
+
+
+class GestureDetectionSM(smach.StateMachine):
+    """
+    State machine for detecting gestures.
+    """
+
+    def __init__(self, gesture_to_detect: str = "raising_left_hand"):
+        smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"])
+        self.gesture_to_detect = gesture_to_detect
+        with self:
+            smach.StateMachine.add(
+                "GET_IMAGE",
+                GetImage(),
+                transitions={"succeeded": "BODY_PIX_DETECTION", "failed": "failed"},
+            )
+
+            smach.StateMachine.add(
+                "BODY_PIX_DETECTION",
+                DetectGesture(gesture_to_detect=self.gesture_to_detect),
+                transitions={"succeeded": "succeeded", "failed": "failed"},
+            )
+
+
+if __name__ == "__main__":
+    rospy.init_node("gesture_detection_sm")
+    while not rospy.is_shutdown():
+        sm = GestureDetectionSM()
+        sm.execute()
+        print("Raising Left Hand Detected:", sm.userdata.gesture_detected)
+
+    rospy.spin()

skills/src/lasr_skills/vision/get_image.py

@@ -18,7 +18,7 @@ def __init__(self, topic: str = None):
             self.topic = (
                 "/xtion/rgb/image_raw"
                 if "tiago" in os.environ["ROS_MASTER_URI"]
-                else "/camera/image_raw"
+                else "/usb_cam/image_raw"
             )
         else:
             self.topic = topic