From ac0657da19422886c74ee0fb0ccea0761a1c1172 Mon Sep 17 00:00:00 2001
From: Haiwei Luo <137087599+haiwei-luo@users.noreply.github.com>
Date: Tue, 9 Jul 2024 20:46:45 +0100
Subject: [PATCH] Receptionist slowness fix  (#254)

Co-authored-by: Jared Swift <jared.swift@kcl.ac.uk>
---
 .../src/lasr_vision_yolov8/yolo.py            |  8 +--
 tasks/receptionist/config/bh_arcade.yaml      | 57 +++++++++++++++++++
 tasks/receptionist/launch/setup.launch        |  1 -
 .../states/introduce_and_seat_guest.py        | 21 ++++---
 4 files changed, 75 insertions(+), 12 deletions(-)
 create mode 100644 tasks/receptionist/config/bh_arcade.yaml

diff --git a/common/vision/lasr_vision_yolov8/src/lasr_vision_yolov8/yolo.py b/common/vision/lasr_vision_yolov8/src/lasr_vision_yolov8/yolo.py
index 6140b8639..e0517901f 100644
--- a/common/vision/lasr_vision_yolov8/src/lasr_vision_yolov8/yolo.py
+++ b/common/vision/lasr_vision_yolov8/src/lasr_vision_yolov8/yolo.py
@@ -152,10 +152,10 @@ def detect_3d(
                 f"Detected point: {detection.point} of object {detection.name}"
             )
 
-        markers.create_and_publish_marker(
-            debug_point_publisher,
-            PointStamped(point=detection.point, header=pcl_map.header),
-        )
+        # markers.create_and_publish_marker(
+        #     debug_point_publisher,
+        #     PointStamped(point=detection.point, header=pcl_map.header),
+        # )
 
         detected_objects.append(detection)
 
diff --git a/tasks/receptionist/config/bh_arcade.yaml b/tasks/receptionist/config/bh_arcade.yaml
new file mode 100644
index 000000000..ce87ffd5a
--- /dev/null
+++ b/tasks/receptionist/config/bh_arcade.yaml
@@ -0,0 +1,57 @@
+priors:
+  names:
+    - Adel
+    - Angel
+    - Axel
+    - Charlie
+    - Jane
+    - Jules
+    - Morgan
+    - Paris
+    - Robin
+    - Simone
+  drinks:
+    - cola
+    - iced tea
+    - juice pack
+    - milk
+    - orange juice
+    - red wine
+    - tropical juice
+  
+# WAIT POSE KITCHEN:
+wait_pose: 
+  position: 
+    x: 8.245934441303595
+    y: 24.285935396834816
+    z: 0.0
+  orientation: 
+    x: 0.0
+    y: 0.0
+    z: 0.08719394681831685
+    w: 0.9961913549304895
+
+# WAIT AREA KITCHEN:
+wait_area: [[9.16, 25.3], [10.6, 25.7], [11, 24.1], [9.4, 23.7]]
+
+seat_pose:
+  position: 
+    x: 7.439730848846352
+    y: 22.667057212175145
+    z: 0.0
+  orientation: 
+    x: 0.0
+    y: 0.0
+    z: -0.6336387580418182
+    w: 0.7736290611832146
+
+search_motions: ["look_left", "look_right"]
+sofa_point:
+  x: 7.78
+  y: 20.1
+  z: 0.5
+seat_area: [[10.8, 20.2], [5.2, 18.9], [4.41, 21.6], [10.1, 23.1]]
+max_people_on_sofa: 2
+sofa_area: [[8.52, 20.1], [7.1, 19.8], [6.71, 21.1], [8.06, 21.5]]
+
+sweep: true
\ No newline at end of file
diff --git a/tasks/receptionist/launch/setup.launch b/tasks/receptionist/launch/setup.launch
index bb5b3b957..95a4c899f 100644
--- a/tasks/receptionist/launch/setup.launch
+++ b/tasks/receptionist/launch/setup.launch
@@ -15,7 +15,6 @@
     <rosparam command="load" file="$(find receptionist)/config/motions.yaml"/>
 
     <!--      PERCEPTION     -->
-    <include file="$(find lasr_vision_yolov8)/launch/service.launch"/>
     <include file="$(find lasr_vision_cropped_detection)/launch/cropped_detection.launch"/>
     <node pkg="lasr_vision_feature_extraction" type="service" name="torch_service" output="screen"/>
     <include file="$(find lasr_vision_bodypix)/launch/bodypix.launch">
diff --git a/tasks/receptionist/src/receptionist/states/introduce_and_seat_guest.py b/tasks/receptionist/src/receptionist/states/introduce_and_seat_guest.py
index ba783d003..8579e637b 100644
--- a/tasks/receptionist/src/receptionist/states/introduce_and_seat_guest.py
+++ b/tasks/receptionist/src/receptionist/states/introduce_and_seat_guest.py
@@ -699,7 +699,7 @@ def execute(self, userdata):
                 # Say to sit on the sofa
                 smach.StateMachine.add(
                     "SAY_SOFA",
-                    Say(text="Please sit on the sofa"),
+                    Say(text="Please sit on the sofa that I am looking at"),
                     transitions={
                         "succeeded": "LOOK_AT_SEAT",
                         "preempted": "LOOK_AT_SEAT",
@@ -723,9 +723,9 @@ def execute(self, userdata):
                     "SAY_ANY",
                     Say(text="Please sit on any empty seat"),
                     transitions={
-                        "succeeded": "succeeded",
-                        "preempted": "succeeded",
-                        "aborted": "succeeded",
+                        "succeeded": "WAIT_SEAT",
+                        "preempted": "WAIT_SEAT",
+                        "aborted": "WAIT_SEAT",
                     },
                 )
 
@@ -734,12 +734,19 @@ def execute(self, userdata):
                     "LOOK_AT_SEAT",
                     LookToPoint(),
                     transitions={
-                        "succeeded": "succeeded",
-                        "aborted": "succeeded",
-                        "timed_out": "succeeded",
+                        "succeeded": "WAIT_SEAT",
+                        "aborted": "WAIT_SEAT",
+                        "timed_out": "WAIT_SEAT",
                     },
                     remapping={"pointstamped": "seat_position"},
                 )
+
+                smach.StateMachine.add(
+                    "WAIT_SEAT",
+                    Wait(3),
+                    transitions={"succeeded": "succeeded", "failed": "failed"},
+                )
+
             else:
 
                 class RecognisePeople(smach.State):