Receptionist recognise and look at (#220)

Co-authored-by: Jared Swift <j.w.swift@outlook.com>

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/examples/check_known_people_test

@@ -0,0 +1,12 @@
+#!/usr/bin/env python3
+
+import rospy
+from lasr_vision_msgs.srv import CheckKnownPeopleRequest, CheckKnownPeopleResponse, CheckKnownPeople
+
+if __name__ == "__main__":
+
+    # get the known people from the service
+    rospy.wait_for_service("/check_known_people")
+    check_known_people = rospy.ServiceProxy("/check_known_people", CheckKnownPeople)
+    response = check_known_people("receptionist")
+    print(response.known_people)

common/vision/lasr_vision_deepface/nodes/check_known_people_service

@@ -0,0 +1,26 @@
+#!/usr/bin/env python3
+
+import rospy
+import os
+import rospkg
+from lasr_vision_msgs.srv import CheckKnownPeopleRequest, CheckKnownPeopleResponse, CheckKnownPeople
+
+DATASET_ROOT = os.path.join(
+    rospkg.RosPack().get_path("lasr_vision_deepface"), "datasets"
+)
+
+rospy.init_node("check_known_people_service")
+
+def check_known_people(request : CheckKnownPeopleRequest) -> CheckKnownPeopleResponse:
+    dataset_path = os.path.join(DATASET_ROOT, request.task_name)
+    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)
+    return CheckKnownPeopleResponse(known_people_names)
+
+rospy.Service("/check_known_people", CheckKnownPeople, check_known_people)
+rospy.loginfo("Check known people service started")
+rospy.spin()

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,7 @@ def recognise(
 
     # Run inference
     rospy.loginfo("Running inference")
+
     try:
         result = DeepFace.find(
             cv_im,
@@ -197,7 +197,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/CMakeLists.txt

@@ -65,6 +65,7 @@ add_service_files(
   PointingDirection.srv
   TorchFaceFeatureDetectionDescription.srv
   DetectFaces.srv
+  CheckKnownPeople.srv
 )
 
 # Generate actions in the 'action' folder

common/vision/lasr_vision_msgs/srv/CheckKnownPeople.srv

@@ -0,0 +1,5 @@
+# Task name - receptionist, carry my luggage, etc
+string task_name
+---
+# Known people names
+string[] known_people

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/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"