Receptinist 21/06

Co-authored-by: Jared Swift <j.w.swift@outlook.com>
Co-authored-by: m-barker <mattbarker322@gmail.com>
Co-authored-by: Zoe <ezoe013@gmail.com>
Co-authored-by: fireblonde <nicollehchevska@gmail.com>

common/vision/lasr_vision_clip/src/lasr_vision_clip/clip_utils.py

@@ -41,6 +41,7 @@ def run_clip(
     txt = model.encode(labels)
     img = model.encode(img)
     with torch.no_grad():
+        torch
         cos_scores = util.cos_sim(img, txt)
     return cos_scores
 

common/vision/lasr_vision_deepface/src/lasr_vision_deepface/deepface.py

@@ -62,7 +62,6 @@ def _extract_face(cv_im: Mat) -> Union[Mat, None]:
     try:
         faces = DeepFace.extract_faces(
             cv_im,
-            target_size=(224, 244),
             detector_backend="mtcnn",
             enforce_detection=True,
         )

common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/image_with_masks_and_attributes.py

@@ -279,7 +279,6 @@ def describe(self) -> dict:
                 max_attribute = "sleeveless top"
             result["attributes"][max_attribute] = True
 
-
         if outwear:
             max_prob = 0.0
             max_attribute = "short sleeve outwear"

skills/src/lasr_skills/__init__.py

@@ -22,4 +22,6 @@
 from .detect_gesture import DetectGesture
 from .learn_face import LearnFace
 from .look_at_person import LookAtPerson
+from .wait import Wait
+from .look_to_given_point import LookToGivenPoint
 from .find_gesture_person import FindGesturePerson

skills/src/lasr_skills/check_known_people.py

@@ -0,0 +1,35 @@
+import smach
+import rospy
+import os
+import rospkg
+
+DATASET_ROOT = os.path.join(
+    rospkg.RosPack().get_path("lasr_vision_deepface"), "datasets"
+)
+
+
+class CheckKnownPeople(smach.State):
+    def __init__(self):
+        smach.State.__init__(
+            self,
+            outcomes=["succeeded", "failed"],
+            input_keys=["task_name"],
+            output_keys=["known_people"],
+        )
+
+    def execute(self, userdata):
+        try:
+            dataset_path = os.path.join(DATASET_ROOT, userdata.task_name)
+            print(dataset_path)
+            known_people_names = [
+                f
+                for f in os.listdir(dataset_path)
+                if os.path.isdir(os.path.join(dataset_path, f))
+            ]
+            rospy.set_param("/known_people", known_people_names)
+            userdata.known_people = known_people_names
+            return "succeeded"
+        except Exception as e:
+            print(f"Face detection using dataset {dataset_path}")
+            rospy.logerr(f"Failed to get known people: {str(e)}")
+            return "failed"

skills/src/lasr_skills/describe_people.py

@@ -5,6 +5,7 @@
 import smach
 import cv2_img
 import numpy as np
+from lasr_skills import Say
 from lasr_vision_msgs.msg import BodyPixMaskRequest
 from lasr_vision_msgs.srv import (
     YoloDetection,
@@ -37,11 +38,39 @@ def __init__(self):
             )
             smach.StateMachine.add(
                 "GET_IMAGE",
-                GetCroppedImage(
-                    object_name="person", crop_method=crop_method, rgb_topic=rgb_topic
+                GetCroppedImage(object_name="person", crop_method="closest"),
+                transitions={
+                    "succeeded": "CONVERT_IMAGE",
+                    "failed": "SAY_GET_IMAGE_AGAIN",
+                },
+            )
+            smach.StateMachine.add(
+                "SAY_GET_IMAGE_AGAIN",
+                Say(
+                    text="Make sure you're looking into my eyes, I can't seem to see you."
                 ),
-                transitions={"succeeded": "CONVERT_IMAGE"},
+                transitions={
+                    "succeeded": "GET_IMAGE_AGAIN",
+                    "preempted": "GET_IMAGE_AGAIN",
+                    "aborted": "GET_IMAGE_AGAIN",
+                },
+            )
+            smach.StateMachine.add(
+                "GET_IMAGE_AGAIN",
+                GetCroppedImage(object_name="person", crop_method="closest"),
+                transitions={"succeeded": "CONVERT_IMAGE", "failed": "SAY_CONTINUE"},
             )
+
+            smach.StateMachine.add(
+                "SAY_CONTINUE",
+                Say(text="I can't see anyone, I will continue"),
+                transitions={
+                    "succeeded": "failed",
+                    "preempted": "failed",
+                    "aborted": "failed",
+                },
+            )
+
             smach.StateMachine.add(
                 "CONVERT_IMAGE", ImageMsgToCv2(), transitions={"succeeded": "SEGMENT"}
             )

skills/src/lasr_skills/detect_3d_in_area.py

@@ -29,6 +29,7 @@ def __init__(
         def execute(self, userdata):
             detected_objects = userdata["detections_3d"].detected_objects
             # publish polygon for debugging
+
             polygon_msg = Polygon()
             polygon_msg.points = [
                 Point32(x=point[0], y=point[1], z=0.0)
@@ -50,7 +51,7 @@ def execute(self, userdata):
 
     def __init__(
         self,
-        area_polygon: Polygon,
+        area_polygon: ShapelyPolygon,
         depth_topic: str = "/xtion/depth_registered/points",
         model: str = "yolov8x-seg.pt",
         filter: Union[List[str], None] = None,

skills/src/lasr_skills/look_at_person.py

@@ -15,7 +15,6 @@
 import ros_numpy as rnp
 import rosservice
 from smach import CBState
-from std_msgs.msg import String
 import actionlib
 from control_msgs.msg import PointHeadAction, PointHeadGoal
 from geometry_msgs.msg import Point
@@ -64,6 +63,18 @@ def execute(self, userdata):
                 len(userdata.bbox_eyes) < 1 and len(userdata.detections.detections) < 1
             ):
                 return "no_detection"
+            print("THE DEEPFACE")
+            print(userdata.deepface_detection)
+
+            if self._filter:
+                if userdata.deepface_detection:
+                    deepface = userdata.deepface_detection[0]
+                    for bbox in userdata.bbox_eyes:
+                        if bbox["bbox"] == deepface:
+                            userdata.bbox_eyes = [bbox]
+                            break
+                else:
+                    return "failed"
 
             if self._filter:
                 if userdata.deepface_detection:
@@ -92,7 +103,15 @@ def execute(self, userdata):
                 pcl_xyz = rnp.point_cloud2.pointcloud2_to_xyz_array(
                     userdata.pcl_msg, remove_nans=False
                 )
-                eye_point_pcl = pcl_xyz[int(eye_point[1]), int(eye_point[0])]
+                try:
+                    eye_point_pcl = pcl_xyz[int(eye_point[1]), int(eye_point[0])]
+                except IndexError:
+                    eye_point_pcl = pcl_xyz[
+                        int(eye_point[1]) - 1, int(eye_point[0]) - 1
+                    ]
+                except Exception as e:
+                    rospy.logerr(f"Unexpected Exception: {e}")
+                    return "failed"
                 if any([True for i in eye_point_pcl if i != i]):
                     eye_point_pcl = pcl_xyz[int(right_eye[1]), int(right_eye[0])]
 
@@ -130,6 +149,8 @@ def execute(self, userdata):
                 )
                 self.look_at_pub.send_goal(goal)
 
+                print(self.look_at_pub.get_state())
+
                 return "succeeded"
 
             return "failed"
@@ -270,7 +291,7 @@ def __init__(self, filter=False):
                 transitions={
                     "succeeded": "LOOP",
                     "aborted": "failed",
-                    "preempted": "failed",
+                    "timed_out": "LOOP",
                 },
                 remapping={"pointstamped": "pointstamped"},
             )

skills/src/lasr_skills/look_to_given_point.py

@@ -0,0 +1,58 @@
+"""Look to a specific point passed in as a parameter
+Similar to look_to_point but the input key is replaced with a passed argument for the point to look at
+"""
+
+import smach_ros
+import smach
+import actionlib
+import rospy
+from control_msgs.msg import PointHeadGoal, PointHeadAction
+from geometry_msgs.msg import Point, PointStamped
+from std_msgs.msg import Header
+from actionlib_msgs.msg import GoalStatus
+from typing import List
+
+
+class LookToGivenPoint(smach.State):
+    def __init__(self, look_position: List[float]):
+        """
+        Args:
+            look_position (List[float]): Position to look to
+        """
+        smach.State.__init__(
+            self,
+            outcomes=["succeeded", "aborted", "timed_out"],
+        )
+        self.goal_pointstamped = PointStamped(
+            point=Point(x=look_position[0], y=look_position[1], z=1.0)
+        )
+        self.client = actionlib.SimpleActionClient(
+            "/head_controller/point_head_action", PointHeadAction
+        )
+        self.client.wait_for_server()
+
+    def execute(self, userdata):
+        goal = PointHeadGoal(
+            pointing_frame="head_2_link",
+            pointing_axis=Point(1.0, 0.0, 0.0),
+            max_velocity=1.0,
+            target=PointStamped(
+                header=Header(frame_id="map"),
+                point=self.goal_pointstamped.point,
+            ),
+        )
+        self.client.send_goal(goal)
+
+        # Wait for the result with a timeout of 7 seconds
+        finished_within_time = self.client.wait_for_result(rospy.Duration(7.0))
+
+        if finished_within_time:
+            state = self.client.get_state()
+            if state == GoalStatus.SUCCEEDED:
+                rospy.sleep(1)
+                return "succeeded"
+            else:
+                return "aborted"
+        else:
+            self.client.cancel_goal()
+            return "timed_out"

skills/src/lasr_skills/look_to_point.py

@@ -1,28 +1,49 @@
+import smach_ros
 import smach
-import rospy
 import actionlib
-from control_msgs.msg import PointHeadAction, PointHeadGoal
-from geometry_msgs.msg import Point
+import rospy
+from control_msgs.msg import PointHeadGoal, PointHeadAction
+from geometry_msgs.msg import Point, PointStamped
+from std_msgs.msg import Header
+from actionlib_msgs.msg import GoalStatus
 
 
 class LookToPoint(smach.State):
     def __init__(self):
         smach.State.__init__(
             self,
-            outcomes=["succeeded", "preempted", "aborted"],
+            outcomes=["succeeded", "aborted", "timed_out"],
             input_keys=["pointstamped"],
         )
-        self.look_at_pub = actionlib.SimpleActionClient(
+        self.client = actionlib.SimpleActionClient(
             "/head_controller/point_head_action", PointHeadAction
         )
+        self.client.wait_for_server()
 
     def execute(self, userdata):
-        goal = PointHeadGoal()
-        goal.pointing_frame = "head_2_link"
-        goal.pointing_axis = Point(1.0, 0.0, 0.0)
-        goal.max_velocity = 1.0
-        goal.target = userdata.pointstamped
-        self.look_at_pub.send_goal(goal)
-        self.look_at_pub.wait_for_result()
-        rospy.sleep(3.0)
-        return "succeeded"
+        # Define the goal
+        goal = PointHeadGoal(
+            pointing_frame="head_2_link",
+            pointing_axis=Point(1.0, 0.0, 0.0),
+            max_velocity=1.0,
+            target=PointStamped(
+                header=Header(frame_id="map"),
+                point=userdata.pointstamped.point,
+            ),
+        )
+
+        # Send the goal
+        self.client.send_goal(goal)
+
+        # Wait for the result with a timeout of 7 seconds
+        finished_within_time = self.client.wait_for_result(rospy.Duration(7.0))
+
+        if finished_within_time:
+            state = self.client.get_state()
+            if state == GoalStatus.SUCCEEDED:
+                return "succeeded"
+            else:
+                return "aborted"
+        else:
+            self.client.cancel_goal()
+            return "timed_out"

skills/src/lasr_skills/wait.py

@@ -0,0 +1,23 @@
+"""Generic wait state for waiting a desired number of seconds"""
+
+import smach
+import rospy
+
+
+class Wait(smach.State):
+    def __init__(self, wait_time: int):
+        """
+        Args:
+            wait_time (int): Number of seconds to wait for and remain idle
+        """
+        smach.State.__init__(self, outcomes=["succeeded", "failed"])
+        self._wait_time = wait_time
+
+    def execute(self, userdata) -> str:
+        try:
+            print(f"Waiting for {self._wait_time} seconds.")
+            rospy.sleep(self._wait_time)
+            return "succeeded"
+        except:
+            print("Waiting failed")
+            return "failed"

tasks/receptionist/config/lab.yaml

@@ -18,17 +18,32 @@ priors:
     - orange juice
     - red wine
     - tropical juice
+# wait_pose: 
+#   position: 
+#     x: 2.4307581363168773
+#     y: -1.661594410669659
+#     z: 0.0
+#   orientation: 
+#     x: 0.0
+#     y: 0.0
+#     z: 0.012769969339563213
+#     w: 0.9999184606171978
+
 wait_pose: 
   position: 
-    x: 2.4307581363168773
-    y: -1.661594410669659
+    x: 4.637585370589175
+    y: 6.715591164127531
     z: 0.0
   orientation: 
     x: 0.0
     y: 0.0
-    z: 0.012769969339563213
-    w: 0.9999184606171978
-wait_area: [[2.65, -0.61], [4.21, -0.33], [4.58, -2.27], [2.67, -2.66]]
+    z: 0.478893309417269
+    w: 0.4
+#0.478893309417269
+#0.8778731105321406
+
+# wait_area: [[2.65, -0.61], [4.21, -0.33], [4.58, -2.27], [2.67, -2.66]]
+wait_area: [[3.60, 8.56], [4.69, 8.90], [4.98, 7.59], [3.95, 7.34]]
 seat_pose:
   position: 
     x: 1.1034954065916212

tasks/receptionist/doc/PREREQUISITES.md

@@ -1 +1,8 @@
 If you would like to view the documentation in the browser, ensure you have at [Node.js v16.x](https://nodejs.org/en) installed.
+
+
+Please make sure the following models are installed in order to run this code: 
+
+- lasr_vision_yolov8 requires yolov8n-seg.pt in lasr_vision_yolov8/models/
+- lasr_vision_feature_extraction requires model.pth in lasr_vision_feature_extraction/models/ 
+- lasr_vision_clip requires a model from hugging face, this can be downloaded by running 'rosrun lasr_vision_clip vqa'