Receptionist recognise and look at

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

@@ -37,6 +37,7 @@ def run_clip(
     Returns:
         List[float]: the cosine similarity scores between the image and label embeddings.
     """
+
     txt = model.encode(labels)
     img = model.encode(img)
     with torch.no_grad():

common/vision/lasr_vision_deepface/nodes/service

@@ -16,7 +16,6 @@ from lasr_vision_msgs.srv import (
     DetectFacesResponse,
 )
 
-
 rospy.init_node("recognise_service")
 
 # Determine variables

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

@@ -24,7 +24,6 @@
     rospkg.RosPack().get_path("lasr_vision_deepface"), "datasets"
 )
 
-
 Mat = np.ndarray
 
 
@@ -100,8 +99,8 @@ def create_dataset(
         face_cropped_cv_im = _extract_face(cv_im)
         if face_cropped_cv_im is None:
             continue
-        cv2.imwrite(os.path.join(dataset_path, f"{name}_{i+1}.png"), face_cropped_cv_im)  # type: ignore
-        rospy.loginfo(f"Took picture {i+1}")
+        cv2.imwrite(os.path.join(dataset_path, f"{name}_{i + 1}.png"), face_cropped_cv_im)  # type: ignore
+        rospy.loginfo(f"Took picture {i + 1}")
         images.append(face_cropped_cv_im)
         if debug_publisher is not None:
             debug_publisher.publish(
@@ -132,6 +131,8 @@ def recognise(
 
     # Run inference
     rospy.loginfo("Running inference")
+    import smach
+
     try:
         result = DeepFace.find(
             cv_im,
@@ -197,7 +198,6 @@ def detect_faces(
     request: DetectFacesRequest,
     debug_publisher: Union[rospy.Publisher, None],
 ) -> DetectFacesResponse:
-
     cv_im = cv2_img.msg_to_cv2_img(request.image_raw)
 
     response = DetectFacesResponse()

common/vision/lasr_vision_feature_extraction/requirements.in

@@ -1 +0,0 @@
-torchvision==0.16.0

common/vision/lasr_vision_feature_extraction/requirements.txt

@@ -1,30 +1 @@
-certifi==2023.11.17       # via requests
-charset-normalizer==3.3.2  # via requests
-filelock==3.13.1          # via torch, triton
-fsspec==2023.10.0         # via torch
-idna==3.4                 # via requests
-jinja2==3.1.2             # via torch
-markupsafe==2.1.3         # via jinja2
-mpmath==1.3.0             # via sympy
-networkx==3.2.1           # via torch
-numpy==1.26.2             # via torchvision
-nvidia-cublas-cu12==12.1.3.1  # via nvidia-cudnn-cu12, nvidia-cusolver-cu12, torch
-nvidia-cuda-cupti-cu12==12.1.105  # via torch
-nvidia-cuda-nvrtc-cu12==12.1.105  # via torch
-nvidia-cuda-runtime-cu12==12.1.105  # via torch
-nvidia-cudnn-cu12==8.9.2.26  # via torch
-nvidia-cufft-cu12==11.0.2.54  # via torch
-nvidia-curand-cu12==10.3.2.106  # via torch
-nvidia-cusolver-cu12==11.4.5.107  # via torch
-nvidia-cusparse-cu12==12.1.0.106  # via nvidia-cusolver-cu12, torch
-nvidia-nccl-cu12==2.18.1  # via torch
-nvidia-nvjitlink-cu12==12.3.101  # via nvidia-cusolver-cu12, nvidia-cusparse-cu12
-nvidia-nvtx-cu12==12.1.105  # via torch
-pillow==10.1.0            # via torchvision
-requests==2.31.0          # via torchvision
-sympy==1.12               # via torch
-torch==2.1.0              # via torchvision
-torchvision==0.16.0       # via -r requirements.in
-triton==2.1.0             # via torch
-typing-extensions==4.8.0  # via torch
-urllib3==2.1.0            # via requests
+

common/vision/lasr_vision_msgs/srv/LearnFace.srv

@@ -1,9 +1,9 @@
-# Name to associate
-string name
-
 # Dataset to add face to
 string dataset
 
+# Name to associate
+string name
+
 # Number of images to take
 int32 n_images
 

skills/scripts/test_learn_face.py

@@ -0,0 +1,22 @@
+#!/usr/bin/env python3
+
+import rospy
+import smach
+from lasr_skills import LearnFace
+
+if __name__ == "__main__":
+    rospy.init_node("learn_face")
+    # make segmentation instead for create dataset
+
+    s = smach.StateMachine(outcomes=["succeeded", "failed"])
+    with s:
+        smach.StateMachine.add(
+            "LEARN_FACE",
+            LearnFace(dataset="receptionist", name="nicole", n_images=10),
+            transitions={
+                "succeeded": "succeeded",
+                "failed": "failed",
+            },
+        )
+
+    s.execute()

skills/scripts/test_look_at_person.py

@@ -0,0 +1,31 @@
+#!/usr/bin/env python3
+
+import rospy
+import smach
+import smach_ros
+from lasr_skills.vision import GetPointCloud
+from lasr_skills import LookAtPerson
+
+
+if __name__ == "__main__":
+    rospy.init_node("look_at_person")
+    sm = smach.StateMachine(outcomes=["succeeded", "failed"])
+    with sm:
+        smach.StateMachine.add(
+            "GET_POINTCLOUD",
+            GetPointCloud("/xtion/depth_registered/points"),
+            transitions={"succeeded": "succeeded"},
+            remapping={"pcl_msg": "pcl_msg"},
+        )
+
+    sis = smach_ros.IntrospectionServer("pointcloud_server", sm, "/LOOK_AT_PERSON")
+    sis.start()
+    sm.execute()
+    pcl = sm.userdata.pcl_msg
+    sis.stop()
+
+    sm = LookAtPerson(filter=False)
+    sm.userdata.pcl_msg = pcl
+    sm.userdata.deepface_detection = []
+    outcome = sm.execute()
+    print(outcome)

skills/src/lasr_skills/__init__.py

@@ -20,4 +20,6 @@
 from .detect_faces import DetectFaces
 from .recognise import Recognise
 from .detect_gesture import DetectGesture
+from .learn_face import LearnFace
+from .look_at_person import LookAtPerson
 from .find_gesture_person import FindGesturePerson

skills/src/lasr_skills/check_known_people.py

@@ -0,0 +1,34 @@
+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:
+            rospy.logerr(f"Failed to get known people: {str(e)}")
+            return "failed"

skills/src/lasr_skills/find_named_person.py

@@ -6,7 +6,6 @@
 
 from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, Point, Quaternion
 
-
 from typing import List, Union
 
 

skills/src/lasr_skills/learn_face.py

@@ -0,0 +1,26 @@
+import rospy
+import smach
+from lasr_vision_msgs.srv import LearnFace as LearnFaceSrv
+
+
+class LearnFace(smach.State):
+    def __init__(
+        self,
+        dataset: str,
+        name: str,
+        n_images: int,
+    ):
+        smach.State.__init__(self, outcomes=["succeeded", "failed"])
+
+        self._dataset = dataset
+        self._name = name
+        self._n_images = n_images
+        self._learn_face = rospy.ServiceProxy("/learn_face", LearnFaceSrv)
+
+    def execute(self, userdata):
+        try:
+            result = self._learn_face(self._dataset, self._name, self._n_images)
+            return "succeeded"
+        except rospy.ServiceException as e:
+            rospy.logwarn(f"Unable to learn face. ({str(e)})")
+            return "failed"

skills/src/lasr_skills/look_at_person.py

@@ -1,7 +1,7 @@
 import smach_ros
 from geometry_msgs.msg import PointStamped
 import smach
-from vision import GetPointCloud
+from .vision import GetPointCloud
 from lasr_vision_msgs.srv import BodyPixDetection, BodyPixDetectionRequest
 from lasr_vision_msgs.msg import BodyPixMaskRequest
 from lasr_skills import LookToPoint, DetectFaces
@@ -35,11 +35,17 @@
 
 class LookAtPerson(smach.StateMachine):
     class CheckEyes(smach.State):
-        def __init__(self, debug=True):
+        def __init__(self, debug=True, _filter=False):
             smach.State.__init__(
                 self,
                 outcomes=["succeeded", "failed", "no_detection"],
-                input_keys=["bbox_eyes", "pcl_msg", "masks", "detections"],
+                input_keys=[
+                    "bbox_eyes",
+                    "pcl_msg",
+                    "masks",
+                    "detections",
+                    "deepface_detection",
+                ],
                 output_keys=["pointstamped"],
             )
             self.DEBUG = debug
@@ -48,6 +54,7 @@ def __init__(self, debug=True):
             self.look_at_pub = actionlib.SimpleActionClient(
                 "/head_controller/point_head_action", PointHeadAction
             )
+            self._filter = _filter
 
         def execute(self, userdata):
             rospy.sleep(1)
@@ -58,6 +65,16 @@ def execute(self, userdata):
             ):
                 return "no_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"
+
             for det in userdata.bbox_eyes:
                 left_eye = det["left_eye"]
                 right_eye = det["right_eye"]
@@ -92,11 +109,25 @@ def execute(self, userdata):
                 userdata.pointstamped = look_at
 
                 self.look_at_pub.wait_for_server()
+                if any(
+                    [
+                        True
+                        for i in [look_at.point.x, look_at.point.y, look_at.point.z]
+                        if i != i
+                    ]
+                ):
+                    look_at.point.x = 0.0
+                    look_at.point.y = 0.0
+                    look_at.point.z = 0.0
+
                 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 = look_at
+                rospy.loginfo(
+                    f"LOOKING AT POINT {look_at.point.x}, {look_at.point.y}, {look_at.point.z}"
+                )
                 self.look_at_pub.send_goal(goal)
 
                 return "succeeded"
@@ -140,12 +171,14 @@ def match_poses_and_detections(ud):
 
         return "succeeded"
 
-    def __init__(self):
-        super(LookAtPerson, self).__init__(
-            outcomes=["succeeded", "failed"],
-            input_keys=[],
-            output_keys=["masks", "poses", "pointstamped"],
+    def __init__(self, filter=False):
+        smach.StateMachine.__init__(
+            self,
+            outcomes=["succeeded", "failed", "no_detection"],
+            input_keys=["pcl_msg", "deepface_detection"],
+            output_keys=[],
         )
+
         self.DEBUG = rospy.get_param("/debug", True)
         IS_SIMULATION = (
             "/pal_startup_control/start" not in rosservice.get_service_list()
@@ -166,19 +199,11 @@ def __init__(self):
                             request=StartupStopRequest("head_manager"),
                         ),
                         transitions={
-                            "succeeded": "GET_IMAGE",
+                            "succeeded": "SEGMENT_PERSON",
                             "aborted": "failed",
                             "preempted": "failed",
                         },
                     )
-            smach.StateMachine.add(
-                "GET_IMAGE",
-                GetPointCloud("/xtion/depth_registered/points"),
-                transitions={
-                    "succeeded": "SEGMENT_PERSON",
-                },
-                remapping={"pcl_msg": "pcl_msg"},
-            )
 
             eyes = BodyPixMaskRequest()
             eyes.parts = ["left_eye", "right_eye"]
@@ -225,16 +250,17 @@ def __init__(self):
             )
             smach.StateMachine.add(
                 "CHECK_EYES",
-                self.CheckEyes(self.DEBUG),
+                self.CheckEyes(self.DEBUG, _filter=filter),
                 transitions={
-                    "succeeded": "LOOK_TO_POINT",
+                    "succeeded": "LOOP",
                     "failed": "failed",
-                    "no_detection": "succeeded",
+                    "no_detection": "no_detection",
                 },
                 remapping={
                     "pcl_msg": "pcl_msg",
                     "bbox_eyes": "bbox_eyes",
                     "pointstamped": "pointstamped",
+                    "deepface_detection": "deepface_detection",
                 },
             )
 
@@ -258,25 +284,6 @@ def __init__(self):
                 ),
                 transitions={
                     "succeeded": "CHECK_EYES",
-                    "finish": "ENABLE_HEAD_MANAGER",
+                    "finish": "succeeded",
                 },
             )
-            if not IS_SIMULATION:
-                if PUBLIC_CONTAINER:
-                    rospy.logwarn(
-                        "You are using a public container. The head manager will not be start following navigation."
-                    )
-                else:
-                    smach.StateMachine.add(
-                        "ENABLE_HEAD_MANAGER",
-                        smach_ros.ServiceState(
-                            "/pal_startup_control/start",
-                            StartupStart,
-                            request=StartupStartRequest("head_manager", ""),
-                        ),
-                        transitions={
-                            "succeeded": "succeeded",
-                            "preempted": "failed",
-                            "aborted": "failed",
-                        },
-                    )