Receptionist 07-07 (#246)

* fix: never return early.

* feat: look back to centre before introducing.

* refactor: remove redundant comment

* fix: only look to the centre if we've sweeped.

* feat: publish the waiting area.

tasks/receptionist/scripts/main.py

@@ -11,7 +11,10 @@
 
 if __name__ == "__main__":
     rospy.init_node("receptionist_robocup")
-    wait_pose_param = rospy.get_param("/receptionist/wait_pose")
+
+    wait_area_publisher = rospy.Publisher(
+        "/receptionist/wait_area", PolygonStamped, queue_size=1, latch=True
+    )
 
     seat_area_publisher = rospy.Publisher(
         "/receptionist/seat_area", PolygonStamped, queue_size=1, latch=True
@@ -21,6 +24,8 @@
         "/receptionist/sofa_area", PolygonStamped, queue_size=1, latch=True
     )
 
+    wait_pose_param = rospy.get_param("/receptionist/wait_pose")
+
     wait_pose = Pose(
         position=Point(**wait_pose_param["position"]),
         orientation=Quaternion(**wait_pose_param["orientation"]),
@@ -58,6 +63,15 @@
 
     sweep = rospy.get_param("/receptionist/sweep")
 
+    wait_area_publisher.publish(
+        PolygonStamped(
+            polygon=Polygon(
+                points=[Point(x=x, y=y, z=0.0) for (x, y) in wait_area.exterior.coords]
+            ),
+            header=Header(frame_id="map"),
+        )
+    )
+
     seat_area_publisher.publish(
         PolygonStamped(
             polygon=Polygon(

tasks/receptionist/src/receptionist/state_machine.py

@@ -47,7 +47,7 @@ def __init__(
         self.wait_area = wait_area
         self.seat_pose = seat_pose
         self.seat_area = seat_area
-        # self.sweep_points = sweep_points
+
         with self:
             self.userdata.guest_data = {
                 "host": host_data,

tasks/receptionist/src/receptionist/states/introduce_and_seat_guest.py

@@ -329,8 +329,9 @@ def identify(img_msg):
                                         self._expected_detections[0]
                                     )
                                 else:
-                                    rospy.logwarn("Failed to find expected guest")
-                                    return "failed"
+                                    rospy.logwarn(
+                                        f"Failed to find expected guest {self._expected_detections[0]}"
+                                    )
 
                         print("+" * 50)
                         print(([(d.name, d.point) for d in filtered_face_detections]))
@@ -350,24 +351,26 @@ def identify(img_msg):
                                         if detection.name
                                         == self._expected_detections[1]
                                     )
-                                    # TODO: handle this being empty
+
                                     other_detections = [
                                         detection
                                         for detection in filtered_face_detections
                                         if detection.name == "unknown"
                                     ]
-                                    if not other_detections:
-                                        return "failed"
-                                    furthest_unknown = max(
-                                        other_detections,
-                                        key=lambda x: _euclidian_distance(
-                                            x.point, other.point
-                                        ),
-                                    )
-                                    furthest_unknown.name = self._expected_detections[0]
+                                    if other_detections:
+                                        furthest_unknown = max(
+                                            other_detections,
+                                            key=lambda x: _euclidian_distance(
+                                                x.point, other.point
+                                            ),
+                                        )
+                                        furthest_unknown.name = (
+                                            self._expected_detections[0]
+                                        )
                                 else:
-                                    rospy.logwarn("Failed to find expected guest")
-                                    return "failed"
+                                    rospy.logwarn(
+                                        f"Failed to find expected guest {self._expected_detections[1]}"
+                                    )
                             elif self._expected_detections[1] not in [
                                 detection.name for detection in filtered_face_detections
                             ] and self._expected_detections[0] in [
@@ -380,21 +383,25 @@ def identify(img_msg):
                                         if detection.name
                                         == self._expected_detections[0]
                                     )
-
-                                    furthest_unknown = max(
-                                        [
-                                            detection
-                                            for detection in filtered_face_detections
-                                            if detection.name == "unknown"
-                                        ],
-                                        key=lambda x: _euclidian_distance(
-                                            x.point, other.point
-                                        ),
-                                    )
-                                    furthest_unknown.name = self._expected_detections[1]
+                                    other_detections = [
+                                        detection
+                                        for detection in filtered_face_detections
+                                        if detection.name == "unknown"
+                                    ]
+                                    if other_detections:
+                                        furthest_unknown = max(
+                                            other_detections,
+                                            key=lambda x: _euclidian_distance(
+                                                x.point, other.point
+                                            ),
+                                        )
+                                        furthest_unknown.name = (
+                                            self._expected_detections[1]
+                                        )
                                 else:
-                                    rospy.logwarn("Failed to find expected guest")
-                                    return "failed"
+                                    rospy.logwarn(
+                                        f"Failed to find expected guest {self._expected_detections[0]}"
+                                    )
 
                         print("-" * 50)
                         print(([(d.name, d.point) for d in filtered_face_detections]))
@@ -588,7 +595,17 @@ def execute(self, userdata):
                 smach.StateMachine.add(
                     "DETECT_PEOPLE_AND_SEATS",
                     DetectPeopleAndSeats(seating_area, motions),
-                    transitions={"succeeded": "HANDLE_RESPONSES", "failed": "failed"},
+                    transitions={"succeeded": "LOOK_CENTRE", "failed": "failed"},
+                )
+
+                smach.StateMachine.add(
+                    "LOOK_CENTRE",
+                    PlayMotion(motion_name="look_centre"),
+                    transitions={
+                        "succeeded": "HANDLE_RESPONSES",
+                        "aborted": "HANDLE_RESPONSES",
+                        "preempted": "HANDLE_RESPONSES",
+                    },
                 )
 
                 # Handle the responses from the detections