Receptionist update with deepface

common/speech/lasr_rasa/assistants/receptionist/data/nlu.yml

@@ -25,6 +25,8 @@ nlu:
     - [Axel](name)
     - [Charlie](name)
     - [Jane](name)
+    - [Jayne](name)
+    - [Jay](name)
     - [Jules](name)
     - [Morgan](name)
     - [Paris](name)

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

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

skills/src/lasr_skills/__init__.py

@@ -19,6 +19,7 @@
 from .clip_vqa import QueryImage
 from .detect_faces import DetectFaces
 from .recognise import Recognise
-from .detect_pointing import DetectPointingDirection
+
+# from .detect_pointing import DetectPointingDirection
 from .detect_gesture import DetectGesture
 from .find_gesture_person import FindGesturePerson

skills/src/lasr_skills/look_to_point.py

@@ -1,19 +1,66 @@
 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
 
 
-class LookToPoint(smach_ros.SimpleActionState):
+# class LookToPoint(smach_ros.SimpleActionState):
+#     def __init__(self):
+#         super(LookToPoint, self).__init__(
+#             "/head_controller/point_head_action",
+#             PointHeadAction,
+#             goal_cb=lambda ud, _: 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=ud.point,
+#                 ),
+#             ),
+#             input_keys=["point"],
+#         )
+
+
+class LookToPoint(smach.State):
     def __init__(self):
-        super(LookToPoint, self).__init__(
-            "/head_controller/point_head_action",
-            PointHeadAction,
-            goal_cb=lambda ud, _: PointHeadGoal(
-                pointing_frame="head_2_link",
-                pointing_axis=Point(1.0, 0.0, 0.0),
-                max_velocity=1.0,
-                target=ud.pointstamped,
+        smach.State.__init__(
+            self,
+            outcomes=["succeeded", "aborted", "preempted", "timed_out"],
+            input_keys=["point"],
+        )
+        self.client = actionlib.SimpleActionClient(
+            "/head_controller/point_head_action", PointHeadAction
+        )
+        self.client.wait_for_server()
+
+    def execute(self, userdata):
+        # 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.point,
             ),
-            input_keys=["pointstamped"],
         )
+
+        # 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:
+            result = self.client.get_result()
+            if result:
+                return "succeeded"
+            else:
+                return "aborted"
+        else:
+            self.client.cancel_goal()
+            return "timed_out"

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' 

tasks/receptionist/launch/setup.launch

@@ -8,15 +8,21 @@
     <node pkg="lasr_speech_recognition_whisper" type="transcribe_microphone_server" name="transcribe_speech" output="screen" args="--mic_device $(arg whisper_device_param)"/>
 
     <!--     STATIC POINTS     -->
-    <rosparam command="load" file="$(find receptionist)/config/$(arg config).yaml" />
+    <!--<rosparam command="load" file="$(find receptionist)/config/$(arg config).yaml" />-->
+    <rosparam command="load" file="$(find receptionist)/config/lab.yaml"/>
 
     <!--     MOTIONS     -->
     <rosparam command="load" file="$(find lasr_skills)/config/motions.yaml"/>
     <rosparam command="load" file="$(find receptionist)/config/motions.yaml"/>
 
     <!--      PERCEPTION     -->
-    <include file="$(find lasr_vision_yolov8)/launch/service.launch" />
+    <include file="$(find lasr_vision_yolov8)/launch/service.launch">
+        <param name="debug" value="true" />
+    </include>
+
     <node pkg="lasr_vision_feature_extraction" type="service" name="torch_service" output="screen"/>
     <node pkg="lasr_vision_bodypix" type="service" name="bodypix_service" output="screen"/>
 
+    <node pkg="lasr_vision_deepface" type="service" name="deepface_service" output="screen"/>
+
 </launch>

tasks/receptionist/scripts/main.py

@@ -1,28 +1,32 @@
 #!/usr/bin/env python3
 import rospy
 from receptionist.state_machine import Receptionist
+import smach
+import smach_ros
 
 from geometry_msgs.msg import Pose, Point, Quaternion
 from shapely.geometry import Polygon
 
 if __name__ == "__main__":
     rospy.init_node("receptionist_robocup")
-    wait_pose_param = rospy.get_param("/receptionist/wait_pose")
+    # wait_pose_param = rospy.get_param("/receptionist/wait_pose")
+    wait_pose_param = rospy.get_param("/wait_pose/")
+
     wait_pose = Pose(
         position=Point(**wait_pose_param["position"]),
         orientation=Quaternion(**wait_pose_param["orientation"]),
     )
 
-    wait_area_param = rospy.get_param("/receptionist/wait_area")
+    wait_area_param = rospy.get_param("/wait_area")
     wait_area = Polygon(wait_area_param)
 
-    seat_pose_param = rospy.get_param("/receptionist/seat_pose")
+    seat_pose_param = rospy.get_param("/seat_pose")
     seat_pose = Pose(
         position=Point(**seat_pose_param["position"]),
         orientation=Quaternion(**seat_pose_param["orientation"]),
     )
 
-    seat_area_param = rospy.get_param("/receptionist/seat_area")
+    seat_area_param = rospy.get_param("/seat_area")
     seat_area = Polygon(seat_area_param)
 
     receptionist = Receptionist(
@@ -35,6 +39,11 @@
             "drink": "beer",
         },
     )
+
+    sis = smach_ros.IntrospectionServer("smach_server", receptionist, "/SM_ROOT")
+    sis.start()
     outcome = receptionist.execute()
+
+    sis.stop()
     rospy.loginfo(f"Receptionist finished with outcome: {outcome}")
     rospy.spin()