From 71f7359f1f601ece5d19a409f25204f2d2cba22e Mon Sep 17 00:00:00 2001 From: Jared Swift Date: Thu, 4 Jul 2024 16:54:51 +0100 Subject: [PATCH 01/17] more shit --- tasks/receptionist/config/lab.yaml | 21 ++++--- tasks/receptionist/scripts/main.py | 7 ++- .../src/receptionist/state_machine.py | 12 ++-- .../src/receptionist/states/check_sofa.py | 55 ++++++++++--------- .../receptionist/states/pointcloud_sweep.py | 1 + .../states/run_and_process_detections.py | 47 +++++++--------- 6 files changed, 77 insertions(+), 66 deletions(-) diff --git a/tasks/receptionist/config/lab.yaml b/tasks/receptionist/config/lab.yaml index 1336cedc1..c649d42af 100644 --- a/tasks/receptionist/config/lab.yaml +++ b/tasks/receptionist/config/lab.yaml @@ -56,15 +56,20 @@ seat_pose: w: 0.577141314753899 sweep_points: - - x : 1.0 - y: 1.68 + - x : -0.55 + y: 2.0 z: 1.0 - - x: 0.38 - y: 1.72 + - x: 0.91 + y: 2.11 z: 1.0 - - x: -1.53 - y: 1.94 + - x : 2.69 + y: 1.86 z: 1.0 -seat_area: [[1.94, 2.79], [2.36,1.18], [-0.94, 0.44], [-1.34, 2.05]] +sofa_point: + x: 0.91 + y: 2.11 + z: 1.0 +seat_area: [[2.75, 1.24], [2.69, 2.95], [-0.31, 2.57], [-0.32, 0.58]] max_people_on_sofa: 1 -sofa_area: [[0.5, 0.87], [0.5, 2.18], [1.26, 2.64], [1.54, 1.26]] +sofa_area: [[1.35, 2.66], [0.18, 2.46], [0.29, 1.91], [1.68, 2.0]] + diff --git a/tasks/receptionist/scripts/main.py b/tasks/receptionist/scripts/main.py index c6dee42b4..d49910168 100755 --- a/tasks/receptionist/scripts/main.py +++ b/tasks/receptionist/scripts/main.py @@ -32,14 +32,18 @@ sofa_area_param = rospy.get_param("/receptionist/sofa_area") + sofa_point_param = rospy.get_param("/receptionist/sofa_point") + max_people_on_sofa = rospy.get_param("/receptionist/max_people_on_sofa") seat_area = Polygon(seat_area_param) sofa_area = Polygon(sofa_area_param) + sofa_point = Point(**sofa_point_param) + # exclude the sofa area from the seat area - seat_area = seat_area.difference(sofa_area) + # seat_area = seat_area.difference(sofa_area) receptionist = Receptionist( wait_pose, @@ -48,6 +52,7 @@ sweep_points, seat_area, sofa_area, + sofa_point, { "name": "charlie", "drink": "wine", diff --git a/tasks/receptionist/src/receptionist/state_machine.py b/tasks/receptionist/src/receptionist/state_machine.py index 82d8d8688..a5cc1238a 100755 --- a/tasks/receptionist/src/receptionist/state_machine.py +++ b/tasks/receptionist/src/receptionist/state_machine.py @@ -39,9 +39,13 @@ def __init__( host_data: dict, max_people_on_sofa: int = 3, face_detection_confidence: float = 0.2, - known_host: bool = False, + known_host: bool = True, + learn_guest_1: bool = True, ): smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"]) + + assert known_host or learn_guest_1, "Must learn at least one guest" + self.wait_pose = wait_pose self.wait_area = wait_area self.seat_pose = seat_pose @@ -91,7 +95,7 @@ def __init__( smach.StateMachine.add( "HANDLE_GUEST_1", - HandleGuest("guest1", not known_host), + HandleGuest("guest1", learn_guest_1), transitions={ "succeeded": "SAY_FOLLOW_GUEST_1", "failed": "SAY_FOLLOW_GUEST_1", @@ -302,8 +306,8 @@ def __init__( smach.StateMachine.add( "FIND_AND_LOOK_AT_GUEST_1", FindAndLookAt( - guest_id="guest1" if not known_host else None, - mask=["host"] if known_host else None, + guest_id="guest1" if learn_guest_1 else None, + mask=["host"] if not learn_guest_1 else None, ), transitions={ "succeeded": "INTRODUCE_GUEST_2_TO_GUEST_1", diff --git a/tasks/receptionist/src/receptionist/states/check_sofa.py b/tasks/receptionist/src/receptionist/states/check_sofa.py index 8a1c377a5..e5c1967af 100644 --- a/tasks/receptionist/src/receptionist/states/check_sofa.py +++ b/tasks/receptionist/src/receptionist/states/check_sofa.py @@ -10,12 +10,12 @@ CroppedDetection, ) +from lasr_vision_msgs.msg import CDRequest + class CheckSofa(smach.StateMachine): - def __init__( - self,, sofa_area: ShapelyPolygon, max_people_on_sofa: int - ): + def __init__(self, sofa_area: ShapelyPolygon, max_people_on_sofa: int): smach.StateMachine.__init__( self, @@ -33,38 +33,43 @@ def __init__( "vision/cropped_detection", CroppedDetection, request=CroppedDetectionRequest( - method="closest", - use_mask=True, - yolo_model="yolov8x-seg.pt", - yolo_model_confidence=0.5, - yolo_nms_threshold=0.3, - return_sensor_reading=False, - polygons=[ - Polygon( - points=[ - Point( - x=point[0], - y=point[1], - z=0.0, + requests=[ + CDRequest( + method="closest", + use_mask=True, + yolo_model="yolov8x-seg.pt", + yolo_model_confidence=0.5, + yolo_nms_threshold=0.3, + return_sensor_reading=False, + polygons=[ + Polygon( + points=[ + Point( + x=point[0], + y=point[1], + z=0.0, + ) + for point in sofa_area.exterior.coords + ] ) - for point in sofa_area.exterior.coords - ] + ], ) - ], + ] ), + response_cb=self.detections_cb, ), transitions={ - "has_free_space": "has_free_space", - "no_free_space": "no_free_space", + "succeeded": "has_free_space", + "aborted": "no_free_space", + "preempted": "no_free_space", }, - response_cb=self.detections_cb, ) def detections_cb(self, userdata, response): if len(response.detections) == 0: - return "no_free_space" + return "aborted" if len(response.detections) >= self.max_people_on_sofa: - return "no_free_space" + return "aborted" - return "has_free_space" + return "succeeded" diff --git a/tasks/receptionist/src/receptionist/states/pointcloud_sweep.py b/tasks/receptionist/src/receptionist/states/pointcloud_sweep.py index e5a1b97c7..a401ce9ed 100755 --- a/tasks/receptionist/src/receptionist/states/pointcloud_sweep.py +++ b/tasks/receptionist/src/receptionist/states/pointcloud_sweep.py @@ -86,6 +86,7 @@ def __init__(self, depth_topic: str = "/xtion/depth_registered/points"): def execute(self, userdata): try: + rospy.sleep(1.0) pcl = rospy.wait_for_message(self.depth_topic, PointCloud2) # transform pcl to map frame trans = tf_buffer.lookup_transform( diff --git a/tasks/receptionist/src/receptionist/states/run_and_process_detections.py b/tasks/receptionist/src/receptionist/states/run_and_process_detections.py index 87e94cf72..4040715fb 100755 --- a/tasks/receptionist/src/receptionist/states/run_and_process_detections.py +++ b/tasks/receptionist/src/receptionist/states/run_and_process_detections.py @@ -21,6 +21,12 @@ ) +def _euclidian_distance(point1: Point, point2: Point): + return ((point1.x - point2.x) ** 2 + (point1.y - point2.y) ** 2) + ( + (point1.z - point2.z) ** 2 + ) ** 0.5 + + class RunAndProcessDetections(smach.StateMachine): def __init__( self, @@ -148,7 +154,9 @@ def execute(self, userdata): return "succeeded" class RunFaceDetections(smach.State): - def __init__(self): + def __init__( + self, + ): smach.State.__init__( self, outcomes=["succeeded", "failed"], @@ -160,9 +168,6 @@ def __init__(self): self._recognise = rospy.ServiceProxy("/recognise", Recognise) self._recognise.wait_for_service() - self._detect_faces = rospy.ServiceProxy("/detect_faces", DetectFaces) - self._detect_faces.wait_for_service() - def execute(self, userdata): try: face_detections = [] @@ -171,25 +176,16 @@ def execute(self, userdata): f"Running face detection on {person_detection[0].name}" ) img_msg = person_detection[1] - face_detection_result = self._detect_faces(img_msg) - if len(face_detection_result.detections) > 0: + recognise_result = self._recognise(img_msg, self._dataset, 0.2) + if len(recognise_result.detections) > 0: - recognise_result = self._recognise(img_msg, self._dataset, 0.2) - if len(recognise_result.detections) > 0: + face_detection = person_detection + face_detection[0].name = recognise_result.detections[0].name - face_detection = person_detection - face_detection[0].name = recognise_result.detections[0].name - face_detections.append(face_detection[0]) - rospy.loginfo( - f"Recognised face as {recognise_result.detections[0].name}" - ) - else: - face_detection = person_detection - face_detection[0].name = "unknown" - face_detections.append(face_detection[0]) - rospy.loginfo("Detected face but could not recognise it.") - else: - rospy.loginfo("No face detected.") + rospy.loginfo( + f"Recognised face as {recognise_result.detections[0].name}" + ) + face_detections.append(face_detection[0]) userdata.matched_face_detections = face_detections except rospy.ServiceException as e: @@ -213,11 +209,6 @@ def __init__( self.closesness_distance = closesness_distance self.overlap_threshold = overlap_threshold - def _euclidian_distance(self, point1: Point, point2: Point): - return ((point1.x - point2.x) ** 2 + (point1.y - point2.y) ** 2) + ( - (point1.z - point2.z) ** 2 - ) ** 0.5 - def execute(self, userdata): try: seat_detections = [] @@ -237,7 +228,7 @@ def execute(self, userdata): if detection.name == "chair": for added_chair in seat_detections: if ( - self._euclidian_distance( + _euclidian_distance( added_chair[0].point, detection.point, ) @@ -252,7 +243,7 @@ def execute(self, userdata): elif detection.name == "person": # for added_person in people_detections: # if ( - # self._euclidian_distance( + # _euclidian_distance( # added_person[0].point, # detection.point, # ) From 1a0f743d99c3aaec64f54af107581130c52f771c Mon Sep 17 00:00:00 2001 From: Jared Swift Date: Thu, 4 Jul 2024 20:05:05 +0100 Subject: [PATCH 02/17] less shit? --- tasks/receptionist/config/lab.yaml | 66 +++++++++++++------ tasks/receptionist/scripts/main.py | 2 + .../src/receptionist/state_machine.py | 4 +- .../src/receptionist/states/check_sofa.py | 4 +- .../states/receptionist_learn_face.py | 2 +- .../states/run_and_process_detections.py | 42 ++++++++---- 6 files changed, 82 insertions(+), 38 deletions(-) diff --git a/tasks/receptionist/config/lab.yaml b/tasks/receptionist/config/lab.yaml index c649d42af..691ea63f3 100644 --- a/tasks/receptionist/config/lab.yaml +++ b/tasks/receptionist/config/lab.yaml @@ -31,45 +31,69 @@ priors: wait_pose: position: - x: 4.637585370589175 - y: 6.715591164127531 + x: 8.245934441303595 + y: 24.285935396834816 z: 0.0 orientation: x: 0.0 y: 0.0 - z: 0.7827525387329473 - w: 0.6223330805180823 + z: 0.08719394681831685 + w: 0.9961913549304895 + #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]] +wait_area: [[9.16, 25.3], [10.6, 25.7], [11, 24.1], [9.4, 23.7]] seat_pose: position: - x: 1.1034954065916212 - y: 0.17802904565746552 + x: 7.352258327332648 + y: 21.865999986066072 z: 0.0 orientation: x: 0.0 y: 0.0 - z: 0.816644293927375 - w: 0.577141314753899 + z: -0.6596152487198429 + w: 0.7516034351014236 + sweep_points: - - x : -0.55 - y: 2.0 + - x: 7.78 + y: 20.1 + z: 0.5 + - x: 7.78 + y: 20.1 + z: 1.0 + - x: 9.34 + y: 21.2 + z: 0.5 + - x: 9.34 + y: 21.2 z: 1.0 - - x: 0.91 - y: 2.11 + - x: 8.56 + y: 20.6 + z: 0.5 + - x: 8.56 + y: 20.6 z: 1.0 - - x : 2.69 - y: 1.86 + - x: 6.74 + y: 20.1 + z: 0.5 + - x: 6.74 + y: 20.1 z: 1.0 + - x: 5.76 + y: 21.5 + z: 0.5 + - x: 5.76 + y: 21.5 + z: 1.0 + sofa_point: - x: 0.91 - y: 2.11 - z: 1.0 -seat_area: [[2.75, 1.24], [2.69, 2.95], [-0.31, 2.57], [-0.32, 0.58]] -max_people_on_sofa: 1 -sofa_area: [[1.35, 2.66], [0.18, 2.46], [0.29, 1.91], [1.68, 2.0]] + 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.21, 21.7], [8.76, 19.6], [6.98, 19.2], [6.51, 21.3]] diff --git a/tasks/receptionist/scripts/main.py b/tasks/receptionist/scripts/main.py index d49910168..e2ed89dd4 100755 --- a/tasks/receptionist/scripts/main.py +++ b/tasks/receptionist/scripts/main.py @@ -37,8 +37,10 @@ max_people_on_sofa = rospy.get_param("/receptionist/max_people_on_sofa") seat_area = Polygon(seat_area_param) + assert seat_area.is_valid sofa_area = Polygon(sofa_area_param) + assert sofa_area.is_valid sofa_point = Point(**sofa_point_param) diff --git a/tasks/receptionist/src/receptionist/state_machine.py b/tasks/receptionist/src/receptionist/state_machine.py index a5cc1238a..996ebd3cc 100755 --- a/tasks/receptionist/src/receptionist/state_machine.py +++ b/tasks/receptionist/src/receptionist/state_machine.py @@ -117,7 +117,7 @@ def __init__( smach.StateMachine.add( "RUN_AND_PROCESS_DETECTIONS_GUEST_1", - RunAndProcessDetections(seat_area), + RunAndProcessDetections(seat_area, sofa_area), transitions={ "succeeded": "FIND_AND_LOOK_AT_HOST_1", "failed": "FIND_AND_LOOK_AT_HOST_1", @@ -255,7 +255,7 @@ def __init__( smach.StateMachine.add( "RUN_AND_PROCESS_DETECTIONS_GUEST_2", - RunAndProcessDetections(seat_area), + RunAndProcessDetections(seat_area, sofa_area), transitions={ "succeeded": "FIND_AND_LOOK_AT_HOST_2", "failed": "FIND_AND_LOOK_AT_HOST_2", diff --git a/tasks/receptionist/src/receptionist/states/check_sofa.py b/tasks/receptionist/src/receptionist/states/check_sofa.py index e5c1967af..d42d11c87 100644 --- a/tasks/receptionist/src/receptionist/states/check_sofa.py +++ b/tasks/receptionist/src/receptionist/states/check_sofa.py @@ -66,10 +66,10 @@ def __init__(self, sofa_area: ShapelyPolygon, max_people_on_sofa: int): ) def detections_cb(self, userdata, response): - if len(response.detections) == 0: + if len(response.responses[0].detections) == 0: return "aborted" - if len(response.detections) >= self.max_people_on_sofa: + if len(response.responses[0].detections) >= self.max_people_on_sofa: return "aborted" return "succeeded" diff --git a/tasks/receptionist/src/receptionist/states/receptionist_learn_face.py b/tasks/receptionist/src/receptionist/states/receptionist_learn_face.py index e29fcb80d..df87a2a68 100644 --- a/tasks/receptionist/src/receptionist/states/receptionist_learn_face.py +++ b/tasks/receptionist/src/receptionist/states/receptionist_learn_face.py @@ -25,7 +25,7 @@ class ReceptionistLearnFaces(smach.State): _dataset_size: int _learn_face: rospy.ServiceProxy - def __init__(self, guest_id: str, dataset_size: int = 10): + def __init__(self, guest_id: str, dataset_size: int = 20): smach.State.__init__( self, outcomes=["succeeded", "failed"], input_keys=["guest_data"] ) diff --git a/tasks/receptionist/src/receptionist/states/run_and_process_detections.py b/tasks/receptionist/src/receptionist/states/run_and_process_detections.py index 4040715fb..69be98834 100755 --- a/tasks/receptionist/src/receptionist/states/run_and_process_detections.py +++ b/tasks/receptionist/src/receptionist/states/run_and_process_detections.py @@ -31,6 +31,7 @@ class RunAndProcessDetections(smach.StateMachine): def __init__( self, seating_area: ShapelyPolygon, + sofa_area: ShapelyPolygon, detection_service: str = "/vision/cropped_detection", ): smach.StateMachine.__init__( @@ -48,6 +49,7 @@ def __init__( self._detection_service, CroppedDetection ) self.seating_area = seating_area + self.sofa_area = sofa_area with self: smach.StateMachine.add( @@ -55,6 +57,7 @@ def __init__( self.RunDetections( detection_client=self._detection_client, seating_area=self.seating_area, + sofa_area=self.sofa_area, method="closest", use_mask=True, yolo_model="yolov8x-seg.pt", @@ -90,6 +93,7 @@ def __init__( self, detection_client: rospy.ServiceProxy, seating_area: ShapelyPolygon, + sofa_area: ShapelyPolygon, method: str = "closest", use_mask: bool = True, yolo_model: str = "yolov8x-seg.pt", @@ -107,6 +111,7 @@ def __init__( self.detector = detection_client self.seating_area = seating_area + self.sofa_area = sofa_area self.method = method self.use_mask = use_mask self.yolo_model = yolo_model @@ -140,7 +145,17 @@ def execute(self, userdata): ) for point in self.seating_area.exterior.coords ] - ) + ), + Polygon( + points=[ + Point( + x=point[0], + y=point[1], + z=0.0, + ) + for point in self.sofa_area.exterior.coords + ] + ), ], pointcloud=pointcloud, object_names=self.object_names, @@ -166,9 +181,9 @@ def __init__( self._dataset = "receptionist" self._recognise = rospy.ServiceProxy("/recognise", Recognise) - self._recognise.wait_for_service() def execute(self, userdata): + self._recognise.wait_for_service() try: face_detections = [] for person_detection in userdata.people_detections: @@ -225,7 +240,10 @@ def execute(self, userdata): rospy.loginfo(f"Processing detection: {detection.name}") add_person = True add_chair = True - if detection.name == "chair": + if ( + detection.name == "chair" + and detection_set.polygon_ids[index] == 0 + ): for added_chair in seat_detections: if ( _euclidian_distance( @@ -241,15 +259,15 @@ def execute(self, userdata): ) rospy.loginfo(f"Found chair at: {detection.point}") elif detection.name == "person": - # for added_person in people_detections: - # if ( - # _euclidian_distance( - # added_person[0].point, - # detection.point, - # ) - # < self.closesness_distance - # ): - # add_person = False + for added_person in people_detections: + if ( + _euclidian_distance( + added_person[0].point, + detection.point, + ) + < self.closesness_distance + ): + add_person = False if add_person: people_detections.append( (detection, detection_set.cropped_imgs[index]) From 9ec546936b2293b6d50e6a4fcbdab57850a542df Mon Sep 17 00:00:00 2001 From: Jared Swift Date: Thu, 4 Jul 2024 20:09:47 +0100 Subject: [PATCH 03/17] fix: don't sleep. --- tasks/receptionist/src/receptionist/states/pointcloud_sweep.py | 1 - 1 file changed, 1 deletion(-) diff --git a/tasks/receptionist/src/receptionist/states/pointcloud_sweep.py b/tasks/receptionist/src/receptionist/states/pointcloud_sweep.py index a401ce9ed..e5a1b97c7 100755 --- a/tasks/receptionist/src/receptionist/states/pointcloud_sweep.py +++ b/tasks/receptionist/src/receptionist/states/pointcloud_sweep.py @@ -86,7 +86,6 @@ def __init__(self, depth_topic: str = "/xtion/depth_registered/points"): def execute(self, userdata): try: - rospy.sleep(1.0) pcl = rospy.wait_for_message(self.depth_topic, PointCloud2) # transform pcl to map frame trans = tf_buffer.lookup_transform( From f536bf811c3ec611cea4f4311c74ed9297a09a75 Mon Sep 17 00:00:00 2001 From: Matt Date: Thu, 4 Jul 2024 23:04:59 +0100 Subject: [PATCH 04/17] feat: sadly not working face detections :( --- common/vision/lasr_vision_clip/CMakeLists.txt | 1 + .../examples/test_person_detector.py | 107 +++++++++++++++++ .../lasr_vision_clip/nodes/learn_face.py | 8 ++ .../vision/lasr_vision_clip/requirements.in | 4 +- .../vision/lasr_vision_clip/requirements.txt | 17 +-- .../src/lasr_vision_clip/__init__.py | 3 +- .../src/lasr_vision_clip/clip_utils.py | 26 +++- .../src/lasr_vision_clip/learn_face.py | 112 ++++++++++++++++++ .../cropped_detection.py | 52 ++++---- common/vision/lasr_vision_msgs/CMakeLists.txt | 2 + .../lasr_vision_msgs/msg/CDResponse.msg | 2 +- .../lasr_vision_msgs/srv/ClipLearnFace.srv | 7 ++ .../srv/ClipRecogniseFace.srv | 13 ++ 13 files changed, 314 insertions(+), 40 deletions(-) create mode 100755 common/vision/lasr_vision_clip/examples/test_person_detector.py create mode 100644 common/vision/lasr_vision_clip/nodes/learn_face.py create mode 100644 common/vision/lasr_vision_clip/src/lasr_vision_clip/learn_face.py create mode 100644 common/vision/lasr_vision_msgs/srv/ClipLearnFace.srv create mode 100644 common/vision/lasr_vision_msgs/srv/ClipRecogniseFace.srv diff --git a/common/vision/lasr_vision_clip/CMakeLists.txt b/common/vision/lasr_vision_clip/CMakeLists.txt index 5084b3576..1b6de6c4a 100644 --- a/common/vision/lasr_vision_clip/CMakeLists.txt +++ b/common/vision/lasr_vision_clip/CMakeLists.txt @@ -158,6 +158,7 @@ include_directories( catkin_install_python(PROGRAMS nodes/vqa nodes/img_encoder.py + nodes/learn_face.py examples/encode_image_example.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) diff --git a/common/vision/lasr_vision_clip/examples/test_person_detector.py b/common/vision/lasr_vision_clip/examples/test_person_detector.py new file mode 100755 index 000000000..a9d2e00d9 --- /dev/null +++ b/common/vision/lasr_vision_clip/examples/test_person_detector.py @@ -0,0 +1,107 @@ +#!/usr/bin/env python3 +from lasr_vision_msgs.srv import ( + ClipLearnFaceRequest, + ClipLearnFace, + ClipLearnFaceResponse, + CroppedDetection, + CroppedDetectionRequest, + CroppedDetectionResponse, + ClipRecogniseFaceRequest, + ClipRecogniseFace, + ClipRecogniseFaceResponse, +) +from lasr_vision_msgs.msg import CDRequest +from sensor_msgs.msg import Image +import cv2 +from cv2_img import msg_to_cv2_img, cv2_img_to_msg +import rospy +from typing import List +import numpy as np + + +if __name__ == "__main__": + rospy.init_node("clip_encoder_test") + cropped_detector = rospy.ServiceProxy("/vision/cropped_detection", CroppedDetection) + learn_face_service = rospy.ServiceProxy("/vision/learn_face", ClipLearnFace) + detect_face_service = rospy.ServiceProxy( + "/vision/face_detection", ClipRecogniseFace + ) + debug_pub = rospy.Publisher("/clip/recognise/debug", Image, queue_size=1) + input_str = "" + while True: + input_str = input("Please enter your name and hit enter to learn your face: ") + if input_str == "done": + break + person_1_imgs = [] + for i in range(10): + cropped_response = cropped_detector( + CroppedDetectionRequest( + [ + CDRequest( + method="centered", + use_mask=True, + object_names=["person"], + yolo_model="yolov8x-seg.pt", + yolo_model_confidence=0.8, + yolo_nms_threshold=0.4, + ) + ] + ) + ) + rospy.sleep(0.1) + try: + person_1_imgs.append(cropped_response.responses[0].cropped_imgs[0]) + except: + continue + + learn_face_service(ClipLearnFaceRequest(raw_imgs=person_1_imgs, name=input_str)) + + # Run inference + while not rospy.is_shutdown(): + cropped_response = cropped_detector( + CroppedDetectionRequest( + [ + CDRequest( + method="centered", + use_mask=True, + object_names=["person"], + yolo_model="yolov8x-seg.pt", + yolo_model_confidence=0.8, + yolo_nms_threshold=0.4, + ) + ] + ) + ) + + try: + names = [] + xywhs = [] + for cropped_img in cropped_response.responses[0].cropped_imgs: + response = detect_face_service( + ClipRecogniseFaceRequest(image_raw=cropped_img) + ) + names.append(response.name) + xywhs.append(response.xywh) + rospy.loginfo(f"Recognised face: {response.name}") + + # Add names to image + cv2_img = msg_to_cv2_img(cropped_response.responses[0].masked_img) + for name, xywh in zip(names, xywhs): + x, y, w, h = xywh[0], xywh[1], xywh[2], xywh[3] + cv2.rectangle(cv2_img, (x, y), (x + w, y + h), (0, 255, 0), 2) + cv2.putText( + cv2_img, + name, + (x, y), + cv2.FONT_HERSHEY_SIMPLEX, + 0.5, + (0, 255, 0), + 2, + cv2.LINE_AA, + ) + debug_pub.publish(cv2_img_to_msg(cv2_img)) + except Exception as e: + rospy.loginfo(e) + continue + + rospy.spin() diff --git a/common/vision/lasr_vision_clip/nodes/learn_face.py b/common/vision/lasr_vision_clip/nodes/learn_face.py new file mode 100644 index 000000000..2d9185e46 --- /dev/null +++ b/common/vision/lasr_vision_clip/nodes/learn_face.py @@ -0,0 +1,8 @@ +import rospy +from lasr_vision_clip import FaceService + + +if __name__ == "__main__": + rospy.init_node("clip_vqa_service") + face_service = FaceService() + rospy.spin() diff --git a/common/vision/lasr_vision_clip/requirements.in b/common/vision/lasr_vision_clip/requirements.in index b03f1e5cc..7ee0832cf 100644 --- a/common/vision/lasr_vision_clip/requirements.in +++ b/common/vision/lasr_vision_clip/requirements.in @@ -1,2 +1,4 @@ +facenet-pytorch sentence-transformers -opencv-python \ No newline at end of file +opencv-python +opencv-contrib-python \ No newline at end of file diff --git a/common/vision/lasr_vision_clip/requirements.txt b/common/vision/lasr_vision_clip/requirements.txt index 1345d36ea..264d9aefb 100644 --- a/common/vision/lasr_vision_clip/requirements.txt +++ b/common/vision/lasr_vision_clip/requirements.txt @@ -1,5 +1,6 @@ certifi==2024.7.4 # via requests charset-normalizer==3.3.2 # via requests +facenet-pytorch==2.6.0 # via -r requirements.in filelock==3.15.4 # via huggingface-hub, torch, transformers, triton fsspec==2024.6.1 # via huggingface-hub, torch huggingface-hub==0.23.4 # via sentence-transformers, tokenizers, transformers @@ -9,7 +10,7 @@ joblib==1.4.2 # via scikit-learn markupsafe==2.1.5 # via jinja2 mpmath==1.3.0 # via sympy networkx==3.2.1 # via torch -numpy==1.26.4 # via opencv-python, scikit-learn, scipy, sentence-transformers, transformers +numpy==1.26.4 # via facenet-pytorch, opencv-contrib-python, opencv-python, scikit-learn, scipy, sentence-transformers, torchvision, transformers 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 @@ -19,15 +20,16 @@ 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.20.5 # via torch +nvidia-nccl-cu12==2.19.3 # via torch nvidia-nvjitlink-cu12==12.5.82 # via nvidia-cusolver-cu12, nvidia-cusparse-cu12 nvidia-nvtx-cu12==12.1.105 # via torch +opencv-contrib-python==4.10.0.84 # via -r requirements.in opencv-python==4.10.0.84 # via -r requirements.in packaging==24.1 # via huggingface-hub, transformers -pillow==10.4.0 # via sentence-transformers +pillow==10.2.0 # via facenet-pytorch, sentence-transformers, torchvision pyyaml==6.0.1 # via huggingface-hub, transformers regex==2024.5.15 # via transformers -requests==2.32.3 # via huggingface-hub, transformers +requests==2.32.3 # via facenet-pytorch, huggingface-hub, transformers safetensors==0.4.3 # via transformers scikit-learn==1.5.1 # via sentence-transformers scipy==1.13.1 # via scikit-learn, sentence-transformers @@ -35,9 +37,10 @@ sentence-transformers==3.0.1 # via -r requirements.in sympy==1.12.1 # via torch threadpoolctl==3.5.0 # via scikit-learn tokenizers==0.19.1 # via transformers -torch==2.3.1 # via sentence-transformers -tqdm==4.66.4 # via huggingface-hub, sentence-transformers, transformers +torch==2.2.2 # via facenet-pytorch, sentence-transformers, torchvision +torchvision==0.17.2 # via facenet-pytorch +tqdm==4.66.4 # via facenet-pytorch, huggingface-hub, sentence-transformers, transformers transformers==4.42.3 # via sentence-transformers -triton==2.3.1 # via torch +triton==2.2.0 # via torch typing-extensions==4.12.2 # via huggingface-hub, torch urllib3==2.2.2 # via requests diff --git a/common/vision/lasr_vision_clip/src/lasr_vision_clip/__init__.py b/common/vision/lasr_vision_clip/src/lasr_vision_clip/__init__.py index 8b1378917..b783e5137 100644 --- a/common/vision/lasr_vision_clip/src/lasr_vision_clip/__init__.py +++ b/common/vision/lasr_vision_clip/src/lasr_vision_clip/__init__.py @@ -1 +1,2 @@ - +from .clip_utils import load_model, encode_img, load_face_model, infer +from .learn_face import FaceService diff --git a/common/vision/lasr_vision_clip/src/lasr_vision_clip/clip_utils.py b/common/vision/lasr_vision_clip/src/lasr_vision_clip/clip_utils.py index 46cdddae6..ddf9ffd74 100644 --- a/common/vision/lasr_vision_clip/src/lasr_vision_clip/clip_utils.py +++ b/common/vision/lasr_vision_clip/src/lasr_vision_clip/clip_utils.py @@ -6,7 +6,6 @@ import numpy as np from copy import deepcopy from sentence_transformers import SentenceTransformer, util - from sensor_msgs.msg import Image @@ -41,12 +40,29 @@ def run_clip( txt = model.encode(labels) img = model.encode(img) with torch.no_grad(): - torch cos_scores = util.cos_sim(img, txt) return cos_scores -def encode_img(model: SentenceTransformer, img_msg: Image) -> np.ndarray: +def load_face_model(): + from transformers import AutoImageProcessor, AutoModel + + processor = AutoImageProcessor.from_pretrained("google/vit-base-patch16-224") + model = AutoModel.from_pretrained("google/vit-base-patch16-224").to("cuda") + + return processor, model + + +def infer(image, processor, model): + image = cv2_img.msg_to_cv2_img(image) + inputs = processor(image, return_tensors="pt").to("cuda") + outputs = model(**inputs) + # squeeze and flatten + outputs.pooler_output = outputs.pooler_output.squeeze(0).flatten() + return outputs.pooler_output.detach().cpu().numpy() + + +def encode_img(model, img_msg: Image) -> np.ndarray: """Run the CLIP model. Args: @@ -56,8 +72,8 @@ def encode_img(model: SentenceTransformer, img_msg: Image) -> np.ndarray: Returns: np.ndarray: the image embedding """ - - return model.encode(cv2_img.msg_to_pillow_img(img_msg)) + img = cv2_img.msg_to_cv2_img(img_msg) + return model(img.unsqueeze(0)).detach().numpy() def query_image_stream( diff --git a/common/vision/lasr_vision_clip/src/lasr_vision_clip/learn_face.py b/common/vision/lasr_vision_clip/src/lasr_vision_clip/learn_face.py new file mode 100644 index 000000000..90945fb53 --- /dev/null +++ b/common/vision/lasr_vision_clip/src/lasr_vision_clip/learn_face.py @@ -0,0 +1,112 @@ +#!/usr/bin/env python3 +import os +import cv2 +import rospy +from typing import Dict +import numpy as np +import rospkg +from lasr_vision_msgs.srv import ( + ClipRecogniseFaceRequest, + ClipRecogniseFaceResponse, + ClipLearnFace, + ClipRecogniseFace, + ClipLearnFaceRequest, + ClipLearnFaceResponse, +) +from sensor_msgs.msg import Image +from cv2_img import msg_to_cv2_img, cv2_img_to_msg +from lasr_vision_clip import load_face_model, encode_img, infer + + +class FaceService: + def __init__(self, similarity_threshold: float = 6.0) -> None: + self._face_classifier = cv2.CascadeClassifier( + os.path.join( + rospkg.RosPack().get_path("lasr_vision_clip"), + "data", + "haarcascade_frontalface_default.xml", + ) + ) + self.learned_faces: Dict[str, np.ndarray] = {} + self._similarity_threshold = similarity_threshold + self.processor, self.model = load_face_model() + self._face_pub = rospy.Publisher("/clip/face_detection", Image, queue_size=1) + + rospy.Service("/vision/face_detection", ClipRecogniseFace, self.face_detection) + rospy.Service("/vision/learn_face", ClipLearnFace, self.learn_face) + + rospy.loginfo("Face detector service started") + + def _detect_faces(self, img: np.ndarray): + faces = self._face_classifier.detectMultiScale( + img, 1.1, minNeighbors=5, minSize=(10, 10) + ) + return faces + + def face_detection( + self, req: ClipRecogniseFaceRequest + ) -> ClipRecogniseFaceResponse: + img = req.image_raw + cv2_img = msg_to_cv2_img(img) + # cv2_img = cv2.cvtColor(cv2_img, cv2.COLOR_BGR2GRAY) + try: + faces = self._detect_faces(cv2_img) + + # Assume only one face in image + encoded_face = None + closest_name = "Unknown" + min_dist = float("inf") + min_xywh = None + for x, y, w, h in faces: + cv2_face = cv2_img[y : y + h, x : x + w] + # cv2_face = cv2.cvtColor(cv2_face, cv2.COLOR_GRAY2BGR) + face_msg = cv2_img_to_msg(cv2_face) + self._face_pub.publish(face_msg) + encoded_face = infer( + cv2_img_to_msg(cv2_img), self.processor, self.model + ) + encoded_face = encoded_face.flatten() + for name, face in self.learned_faces.items(): + distance = np.linalg.norm(encoded_face - face) + rospy.loginfo(f"Distance to {name} : {distance}") + if distance < min_dist: + min_dist = distance + min_xywh = [x, y, w, h] + closest_name = name + return ClipRecogniseFaceResponse( + name=closest_name, distance=min_dist, xywh=min_xywh + ) + except Exception as e: + rospy.loginfo(e) + return ClipRecogniseFaceResponse(name="Unknown", distance=None, xywh=None) + + def learn_face(self, request: ClipLearnFaceRequest) -> ClipLearnFaceResponse: + imgs = request.raw_imgs + + embedding_vectors = [] + for img in imgs: + cv2_img = msg_to_cv2_img(img) + # cv2_img = cv2.cvtColor(cv2_img, cv2.COLOR_BGR2GRAY) + rospy.loginfo(f"Image shape: {cv2_img.shape}") + try: + faces = self._detect_faces(cv2_img) + except Exception as e: # No face detected + rospy.loginfo(e) + continue + for x, y, w, h in faces: + cv2_face = cv2_img[y : y + h, x : x + w] + # cv2_face = cv2.cvtColor(cv2_face, cv2.COLOR_GRAY2BGR) + face_msg = cv2_img_to_msg(cv2_face) + self._face_pub.publish(face_msg) + encoded_face = infer( + cv2_img_to_msg(cv2_img), self.processor, self.model + ) + encoded_face = encoded_face.flatten() + embedding_vectors.append(encoded_face) + + embedding_vectors = np.array(embedding_vectors) + embedding_vector = np.mean(embedding_vectors, axis=0) + self.learned_faces[request.name] = embedding_vector + rospy.loginfo(f"Learned {request.name}") + + return ClipLearnFaceResponse() diff --git a/common/vision/lasr_vision_cropped_detection/src/lasr_vision_cropped_detection/cropped_detection.py b/common/vision/lasr_vision_cropped_detection/src/lasr_vision_cropped_detection/cropped_detection.py index 2bbc8f0ec..5fd1a7692 100644 --- a/common/vision/lasr_vision_cropped_detection/src/lasr_vision_cropped_detection/cropped_detection.py +++ b/common/vision/lasr_vision_cropped_detection/src/lasr_vision_cropped_detection/cropped_detection.py @@ -410,16 +410,17 @@ def process_single_detection_request( if combined_mask is not None: # Add distances to the image for i, (dist, detect) in enumerate(zip(distances, detections)): - cv2.putText( - combined_mask, - f"Dist: {round(dist, 2)}", - (detect.xywh[0], detect.xywh[1]), - cv2.FONT_HERSHEY_SIMPLEX, - 1, - (0, 255, 0), - 2, - cv2.LINE_AA, - ) + continue + # cv2.putText( + # combined_mask, + # f"Dist: {round(dist, 2)}", + # (detect.xywh[0], detect.xywh[1]), + # cv2.FONT_HERSHEY_SIMPLEX, + # 1, + # (0, 255, 0), + # 2, + # cv2.LINE_AA, + # ) combined_mask_debug_publisher.publish(cv2_img_to_msg(combined_mask)) response.masked_img = cv2_img_to_msg(combined_mask) @@ -429,21 +430,22 @@ def process_single_detection_request( rospy.logwarn("No detections found") response.distances = distances try: - debug_image = np.hstack(cropped_images) - # Add distances to the image - for i, dist in enumerate(distances): - cv2.putText( - debug_image, - f"Dist: {round(dist, 2)}", - (i * cropped_images[0].shape[0] + 150, 50), - cv2.FONT_HERSHEY_SIMPLEX, - 1, - (0, 255, 0), - 2, - cv2.LINE_AA, - ) - - debug_publisher.publish(cv2_img_to_msg(debug_image)) + print("...") + # debug_image = np.hstack(cropped_images) + # # Add distances to the image + # for i, dist in enumerate(distances): + # cv2.putText( + # debug_image, + # f"Dist: {round(dist, 2)}", + # (i * cropped_images[0].shape[0] + 150, 50), + # cv2.FONT_HERSHEY_SIMPLEX, + # 1, + # (0, 255, 0), + # 2, + # cv2.LINE_AA, + # ) + + # debug_publisher.publish(cv2_img_to_msg(debug_image)) except ValueError: rospy.logwarn("No detections found") diff --git a/common/vision/lasr_vision_msgs/CMakeLists.txt b/common/vision/lasr_vision_msgs/CMakeLists.txt index 595155b73..4b71ef21d 100644 --- a/common/vision/lasr_vision_msgs/CMakeLists.txt +++ b/common/vision/lasr_vision_msgs/CMakeLists.txt @@ -70,6 +70,8 @@ add_service_files( CheckKnownPeople.srv CroppedDetection.srv ClipImageEncoder.srv + ClipLearnFace.srv + ClipRecogniseFace.srv ) # Generate actions in the 'action' folder diff --git a/common/vision/lasr_vision_msgs/msg/CDResponse.msg b/common/vision/lasr_vision_msgs/msg/CDResponse.msg index 03091fcfa..95911c281 100644 --- a/common/vision/lasr_vision_msgs/msg/CDResponse.msg +++ b/common/vision/lasr_vision_msgs/msg/CDResponse.msg @@ -22,4 +22,4 @@ uint8[] polygon_ids sensor_msgs/Image rgb_image # The pointcloud used for the 3D crop -sensor_msgs/PointCloud2 pointcloud \ No newline at end of file +sensor_msgs/PointCloud2 pointcloud \ No newline at end of file diff --git a/common/vision/lasr_vision_msgs/srv/ClipLearnFace.srv b/common/vision/lasr_vision_msgs/srv/ClipLearnFace.srv new file mode 100644 index 000000000..a41a904c2 --- /dev/null +++ b/common/vision/lasr_vision_msgs/srv/ClipLearnFace.srv @@ -0,0 +1,7 @@ +# Images to use to learn face +sensor_msgs/Image[] raw_imgs + +# Name of person to be learned +string name + +--- \ No newline at end of file diff --git a/common/vision/lasr_vision_msgs/srv/ClipRecogniseFace.srv b/common/vision/lasr_vision_msgs/srv/ClipRecogniseFace.srv new file mode 100644 index 000000000..a191f2dc3 --- /dev/null +++ b/common/vision/lasr_vision_msgs/srv/ClipRecogniseFace.srv @@ -0,0 +1,13 @@ +# Raw image to run inference on +sensor_msgs/Image image_raw + +--- + +# Name of face +string name + +# Distance from detected face +float32 distance + +# Face bounding box +int32[] xywh \ No newline at end of file From 7c07e43558feef7b69c87b6da10cfbde645f5048 Mon Sep 17 00:00:00 2001 From: Jared Swift Date: Fri, 5 Jul 2024 03:04:46 +0100 Subject: [PATCH 05/17] something --- .../cropped_detection.py | 2 + tasks/receptionist/config/lab.yaml | 12 +- tasks/receptionist/scripts/main.py | 41 +- .../src/receptionist/state_machine.py | 271 +----------- .../src/receptionist/states/__init__.py | 1 + .../src/receptionist/states/introduce.py | 2 +- .../states/introduce_and_seat_guest.py | 402 ++++++++++++++++++ .../states/receptionist_learn_face.py | 2 +- 8 files changed, 469 insertions(+), 264 deletions(-) create mode 100644 tasks/receptionist/src/receptionist/states/introduce_and_seat_guest.py diff --git a/common/vision/lasr_vision_cropped_detection/src/lasr_vision_cropped_detection/cropped_detection.py b/common/vision/lasr_vision_cropped_detection/src/lasr_vision_cropped_detection/cropped_detection.py index 5fd1a7692..e9360cc57 100644 --- a/common/vision/lasr_vision_cropped_detection/src/lasr_vision_cropped_detection/cropped_detection.py +++ b/common/vision/lasr_vision_cropped_detection/src/lasr_vision_cropped_detection/cropped_detection.py @@ -287,6 +287,8 @@ def filter_detections_by_polygon( print(f"Detection {detection} is within polygon {index}") detection_polygon_ids.append(index) filtered_detections.append(detection) + else: + print(f"Detection {detection} is not within polygon {index}") return filtered_detections, detection_polygon_ids diff --git a/tasks/receptionist/config/lab.yaml b/tasks/receptionist/config/lab.yaml index 691ea63f3..63b3ef4fa 100644 --- a/tasks/receptionist/config/lab.yaml +++ b/tasks/receptionist/config/lab.yaml @@ -47,14 +47,16 @@ wait_pose: wait_area: [[9.16, 25.3], [10.6, 25.7], [11, 24.1], [9.4, 23.7]] seat_pose: position: - x: 7.352258327332648 - y: 21.865999986066072 + x: 7.146951994681816 + y: 23.821753048065705 z: 0.0 orientation: x: 0.0 y: 0.0 - z: -0.6596152487198429 - w: 0.7516034351014236 + z: -0.6423412634039911 + w: 0.7664187506373813 + + sweep_points: @@ -95,5 +97,5 @@ sofa_point: 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.21, 21.7], [8.76, 19.6], [6.98, 19.2], [6.51, 21.3]] +sofa_area: [[8.52, 20.1], [7.1, 19.8], [6.71, 21.1], [8.06, 21.5]] diff --git a/tasks/receptionist/scripts/main.py b/tasks/receptionist/scripts/main.py index e2ed89dd4..431f68e89 100755 --- a/tasks/receptionist/scripts/main.py +++ b/tasks/receptionist/scripts/main.py @@ -4,20 +4,30 @@ import smach import smach_ros -from geometry_msgs.msg import Pose, Point, Quaternion -from shapely.geometry import Polygon +from geometry_msgs.msg import Pose, Point, Quaternion, Polygon, PolygonStamped +from shapely.geometry import Polygon as ShapelyPolygon + +from std_msgs.msg import Header if __name__ == "__main__": rospy.init_node("receptionist_robocup") wait_pose_param = rospy.get_param("/receptionist/wait_pose") + seat_area_publisher = rospy.Publisher( + "/receptionist/seat_area", PolygonStamped, queue_size=1, latch=True + ) + + sofa_area_publisher = rospy.Publisher( + "/receptionist/sofa_area", PolygonStamped, queue_size=1, latch=True + ) + 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 = Polygon(wait_area_param) + wait_area = ShapelyPolygon(wait_area_param) seat_pose_param = rospy.get_param("/receptionist/seat_pose") seat_pose = Pose( @@ -36,16 +46,35 @@ max_people_on_sofa = rospy.get_param("/receptionist/max_people_on_sofa") - seat_area = Polygon(seat_area_param) + seat_area = ShapelyPolygon(seat_area_param) assert seat_area.is_valid - sofa_area = Polygon(sofa_area_param) + sofa_area = ShapelyPolygon(sofa_area_param) assert sofa_area.is_valid sofa_point = Point(**sofa_point_param) # exclude the sofa area from the seat area - # seat_area = seat_area.difference(sofa_area) + seat_area = seat_area.difference(sofa_area) + + seat_area_publisher.publish( + PolygonStamped( + polygon=Polygon( + points=[Point(x=x, y=y, z=0.0) for (x, y) in seat_area.exterior.coords] + ), + header=Header(frame_id="map"), + ) + ) + sofa_area_publisher.publish( + PolygonStamped( + polygon=Polygon( + points=[Point(x=x, y=y, z=0.0) for (x, y) in sofa_area.exterior.coords] + ), + header=Header(frame_id="map"), + ) + ) + + assert seat_area.is_valid receptionist = Receptionist( wait_pose, diff --git a/tasks/receptionist/src/receptionist/state_machine.py b/tasks/receptionist/src/receptionist/state_machine.py index 996ebd3cc..78ed090ba 100755 --- a/tasks/receptionist/src/receptionist/state_machine.py +++ b/tasks/receptionist/src/receptionist/state_machine.py @@ -23,6 +23,7 @@ RunAndProcessDetections, RecognisePeople, CheckSofa, + IntroduceAndSeatGuest, ) @@ -105,113 +106,18 @@ def __init__( self._guide_guest(guest_id=1) smach.StateMachine.add( - "SWEEP_GUEST_1", - PointCloudSweep( - sweep_points=sweep_points, + "INTRODUCE_AND_SEAT_GUEST_1", + IntroduceAndSeatGuest( + "guest1", + ["host"], + seat_area, + sofa_area, + sofa_point, + max_people_on_sofa, ), - transitions={ - "succeeded": "RUN_AND_PROCESS_DETECTIONS_GUEST_1", - "failed": "failed", - }, - ) - - smach.StateMachine.add( - "RUN_AND_PROCESS_DETECTIONS_GUEST_1", - RunAndProcessDetections(seat_area, sofa_area), - transitions={ - "succeeded": "FIND_AND_LOOK_AT_HOST_1", - "failed": "FIND_AND_LOOK_AT_HOST_1", - }, - remapping={"empty_seat_point": "seat_position"}, - ) - - # Look at host - smach.StateMachine.add( - "FIND_AND_LOOK_AT_HOST_1", - FindAndLookAt(guest_id=None), # assume host is the only person there? - transitions={ - "succeeded": "INTRODUCE_GUEST_1_TO_HOST", - "failed": "INTRODUCE_GUEST_1_TO_HOST", - }, - ) - - smach.StateMachine.add( - "INTRODUCE_GUEST_1_TO_HOST", - Introduce(guest_to_introduce="guest1", guest_to_introduce_to="host"), - transitions={ - "succeeded": "LOOK_AT_WAITING_GUEST_1_1", - "failed": "LOOK_AT_WAITING_GUEST_1_1", - }, - ) - - smach.StateMachine.add( - "LOOK_AT_WAITING_GUEST_1_1", - PlayMotion(motion_name="look_very_left"), - transitions={ - "succeeded": "INTRODUCE_HOST_TO_GUEST_1", - "aborted": "INTRODUCE_HOST_TO_GUEST_1", - "preempted": "INTRODUCE_HOST_TO_GUEST_1", - }, - ) - - smach.StateMachine.add( - "INTRODUCE_HOST_TO_GUEST_1", - Introduce(guest_to_introduce="host", guest_to_introduce_to="guest1"), - transitions={ - "succeeded": "CHECK_SOFA_1", - "failed": "CHECK_SOFA_1", - }, - ) - - smach.StateMachine.add( - "CHECK_SOFA_1", - CheckSofa(sofa_area, max_people_on_sofa), - transitions={ - "has_free_space": "LOOK_AT_SOFA_1", - "no_free_space": "SEAT_GUEST_1_CHAIR", - }, - ) - - smach.StateMachine.add( - "LOOK_AT_SOFA_1", - LookToPoint( - pointstamped=PointStamped( - point=sofa_point, header=Header(frame_id="map") - ) - ), - transitions={ - "succeeded": "SAY_GUEST_1_SIT_SOFA", - "aborted": "SAY_GUEST_1_SIT_SOFA", - "timed_out": "SAY_GUEST_1_SIT_SOFA", - }, - ) - - smach.StateMachine.add( - "SAY_GUEST_1_SIT_SOFA", - Say(text="Please sit on the sofa."), - transitions={ - "succeeded": "SAY_RETURN_WAITING_AREA", - "aborted": "SAY_RETURN_WAITING_AREA", - "preempted": "SAY_RETURN_WAITING_AREA", - }, - ) - - smach.StateMachine.add( - "SEAT_GUEST_1_CHAIR", - SeatGuest(), - transitions={ - "succeeded": "SAY_RETURN_WAITING_AREA", - "failed": "SAY_SEAT_GUEST_1_FAILED", - }, - ) - - smach.StateMachine.add( - "SAY_SEAT_GUEST_1_FAILED", - Say(text="Please sit in an empty seat."), transitions={ "succeeded": "SAY_RETURN_WAITING_AREA", - "aborted": "SAY_RETURN_WAITING_AREA", - "preempted": "SAY_RETURN_WAITING_AREA", + "failed": "SAY_RETURN_WAITING_AREA", }, ) @@ -243,155 +149,18 @@ def __init__( self._guide_guest(guest_id=2) smach.StateMachine.add( - "SWEEP_GUEST_2", - PointCloudSweep( - sweep_points=sweep_points, - ), - transitions={ - "succeeded": "RUN_AND_PROCESS_DETECTIONS_GUEST_2", - "failed": "failed", - }, - ) - - smach.StateMachine.add( - "RUN_AND_PROCESS_DETECTIONS_GUEST_2", - RunAndProcessDetections(seat_area, sofa_area), - transitions={ - "succeeded": "FIND_AND_LOOK_AT_HOST_2", - "failed": "FIND_AND_LOOK_AT_HOST_2", - }, - remapping={"empty_seat_point": "seat_position"}, - ) - - smach.StateMachine.add( - "FIND_AND_LOOK_AT_HOST_2", - FindAndLookAt( - guest_id="host" if known_host else None, - mask=["guest1"] if not known_host else None, + "INTRODUCE_AND_SEAT_GUEST_2", + IntroduceAndSeatGuest( + "guest2", + ["host", "guest1"], + seat_area, + sofa_area, + sofa_point, + max_people_on_sofa, ), - transitions={ - "succeeded": "INTRODUCE_GUEST_2_TO_HOST", - "failed": "INTRODUCE_GUEST_2_TO_HOST", - }, - ) - - smach.StateMachine.add( - "INTRODUCE_GUEST_2_TO_HOST", - Introduce(guest_to_introduce="guest2", guest_to_introduce_to="host"), - transitions={ - "succeeded": "LOOK_AT_WAITING_GUEST_2_1", - "failed": "LOOK_AT_WAITING_GUEST_2_1", - }, - ) - - smach.StateMachine.add( - "LOOK_AT_WAITING_GUEST_2_1", - PlayMotion(motion_name="look_very_left"), - transitions={ - "succeeded": "INTRODUCE_HOST_TO_GUEST_2", - "aborted": "INTRODUCE_HOST_TO_GUEST_2", - "preempted": "INTRODUCE_HOST_TO_GUEST_2", - }, - ) - - smach.StateMachine.add( - "INTRODUCE_HOST_TO_GUEST_2", - Introduce(guest_to_introduce="host", guest_to_introduce_to="guest2"), - transitions={ - "succeeded": "FIND_AND_LOOK_AT_GUEST_1", - "failed": "FIND_AND_LOOK_AT_GUEST_1", - }, - ) - - smach.StateMachine.add( - "FIND_AND_LOOK_AT_GUEST_1", - FindAndLookAt( - guest_id="guest1" if learn_guest_1 else None, - mask=["host"] if not learn_guest_1 else None, - ), - transitions={ - "succeeded": "INTRODUCE_GUEST_2_TO_GUEST_1", - "failed": "INTRODUCE_GUEST_2_TO_GUEST_1", - }, - ) - - smach.StateMachine.add( - "INTRODUCE_GUEST_2_TO_GUEST_1", - Introduce(guest_to_introduce="guest2", guest_to_introduce_to="guest1"), - transitions={ - "succeeded": "LOOK_AT_WAITING_GUEST_2_2", - "failed": "LOOK_AT_WAITING_GUEST_2_2", - }, - ) - - smach.StateMachine.add( - "LOOK_AT_WAITING_GUEST_2_2", - PlayMotion(motion_name="look_very_left"), - transitions={ - "succeeded": "INTRODUCE_GUEST_1_TO_GUEST_2", - "aborted": "INTRODUCE_GUEST_1_TO_GUEST_2", - "preempted": "INTRODUCE_GUEST_1_TO_GUEST_2", - }, - ) - - smach.StateMachine.add( - "INTRODUCE_GUEST_1_TO_GUEST_2", - Introduce(guest_to_introduce="guest1", guest_to_introduce_to="guest2"), - transitions={ - "succeeded": "CHECK_SOFA_2", - "failed": "CHECK_SOFA_2", - }, - ) - - smach.StateMachine.add( - "CHECK_SOFA_2", - CheckSofa(sofa_area, max_people_on_sofa), - transitions={ - "has_free_space": "LOOK_AT_SOFA_2", - "no_free_space": "SEAT_GUEST_2_CHAIR", - }, - ) - - smach.StateMachine.add( - "LOOK_AT_SOFA_2", - LookToPoint( - pointstamped=PointStamped( - point=sofa_point, header=Header(frame_id="map") - ) - ), - transitions={ - "succeeded": "SAY_GUEST_2_SIT_SOFA", - "aborted": "SAY_GUEST_2_SIT_SOFA", - "timed_out": "SAY_GUEST_2_SIT_SOFA", - }, - ) - - smach.StateMachine.add( - "SAY_GUEST_2_SIT_SOFA", - Say(text="Please sit on the sofa."), - transitions={ - "succeeded": "SAY_RETURN_WAITING_AREA", - "aborted": "SAY_RETURN_WAITING_AREA", - "preempted": "SAY_RETURN_WAITING_AREA", - }, - ) - - smach.StateMachine.add( - "SEAT_GUEST_2_CHAIR", - SeatGuest(), - transitions={ - "succeeded": "SAY_GOODBYE", - "failed": "SAY_SEAT_GUEST_2_FAILED", - }, - ) - - smach.StateMachine.add( - "SAY_SEAT_GUEST_2_FAILED", - Say(text="Please sit in an empty seat."), transitions={ "succeeded": "SAY_GOODBYE", - "aborted": "SAY_GOODBYE", - "preempted": "SAY_GOODBYE", + "failed": "SAY_GOODBYE", }, ) @@ -506,7 +275,7 @@ def _guide_guest(self, guest_id: int) -> None: f"SAY_WAIT_GUEST_{guest_id}", Say(text="Please wait here on my left."), transitions={ - "succeeded": f"SWEEP_GUEST_{guest_id}", + "succeeded": f"INTRODUCE_AND_SEAT_GUEST_{guest_id}", "preempted": "failed", "aborted": "failed", }, diff --git a/tasks/receptionist/src/receptionist/states/__init__.py b/tasks/receptionist/src/receptionist/states/__init__.py index 3ecdea3ab..eaedf1b1b 100644 --- a/tasks/receptionist/src/receptionist/states/__init__.py +++ b/tasks/receptionist/src/receptionist/states/__init__.py @@ -11,3 +11,4 @@ from .recognise_people import RecognisePeople from .seat_guest import SeatGuest from .check_sofa import CheckSofa +from .introduce_and_seat_guest import IntroduceAndSeatGuest diff --git a/tasks/receptionist/src/receptionist/states/introduce.py b/tasks/receptionist/src/receptionist/states/introduce.py index c20951445..2f14012ea 100644 --- a/tasks/receptionist/src/receptionist/states/introduce.py +++ b/tasks/receptionist/src/receptionist/states/introduce.py @@ -7,7 +7,7 @@ import rospy import smach from smach import UserData -from lasr_skills import Say +from lasr_skills import Say, LookToPoint from typing import Dict, List, Any, Optional diff --git a/tasks/receptionist/src/receptionist/states/introduce_and_seat_guest.py b/tasks/receptionist/src/receptionist/states/introduce_and_seat_guest.py new file mode 100644 index 000000000..3ae2b8a7b --- /dev/null +++ b/tasks/receptionist/src/receptionist/states/introduce_and_seat_guest.py @@ -0,0 +1,402 @@ +import smach +import smach_ros + +from typing import List + +from shapely.geometry import Polygon as ShapelyPolygon +from geometry_msgs.msg import Point, Polygon, PointStamped +from std_msgs.msg import Header + + +from lasr_skills import LookToPoint, Say + +from lasr_vision_msgs.msg import CDRequest, CDResponse + +from lasr_vision_msgs.srv import ( + CroppedDetectionRequest, + CroppedDetectionResponse, + CroppedDetection, + Recognise, +) +import rospy + +from receptionist.states import Introduce + +import numpy as np + + +def _euclidian_distance(point1: Point, point2: Point): + return ((point1.x - point2.x) ** 2 + (point1.y - point2.y) ** 2) + ( + (point1.z - point2.z) ** 2 + ) ** 0.5 + + +class IntroduceAndSeatGuest(smach.StateMachine): + + class RecognisePeople(smach.State): + + def __init__(self): + smach.State.__init__( + self, + outcomes=["succeeded", "failed"], + input_keys=["people_detections"], + output_keys=["matched_face_detections"], + ) + self._recognise = rospy.ServiceProxy("/recognise", Recognise) + self._recognise.wait_for_service() + self._dataset = "receptionist" + + def execute(self, userdata): + try: + face_detections = [] + for person_detection in userdata.people_detections: + rospy.loginfo( + f"Running face detection on {person_detection[0].name}" + ) + img_msg = person_detection[1] + recognise_result = self._recognise(img_msg, self._dataset, 0.2) + if len(recognise_result.detections) > 0: + + face_detection = person_detection + face_detection[0].name = recognise_result.detections[0].name + + rospy.loginfo( + f"Recognised face as {recognise_result.detections[0].name}" + ) + face_detections.append(face_detection[0]) + + userdata.matched_face_detections = face_detections + except rospy.ServiceException as e: + rospy.logwarn(f"Unable to perform face detection. ({str(e)})") + return "failed" + return "succeeded" + + class HandleResponse(smach.State): + + def __init__(self): + + smach.State.__init__( + self, + outcomes=["succeeded", "failed"], + input_keys=["responses"], + output_keys=["people_detections", "empty_seat_detections"], + ) + self.closesness_distance = 0.5 + self.overlap_threshold = 0.8 + + def execute(self, userdata): + try: + seat_detections = [] + people_detections = [] + response = userdata.responses[0] + rospy.loginfo(f"Processing {len(response.detections_3d)} detections.") + for index, detection in enumerate(response.detections_3d): + if ( + np.isnan(detection.point.x) + or np.isnan(detection.point.y) + or np.isnan(detection.point.z) + ): + continue + rospy.loginfo(f"Processing detection: {detection.name}") + + if ( + detection.name == "chair" + and response.polygon_ids[index] + == 0 # ignore chairs inside the sofa area + ): + seat_detections.append( + ( + detection, + response.cropped_imgs[index], + response.polygon_ids[index], + ) + ) + rospy.loginfo(f"Found chair at: {detection.point}") + elif detection.name == "person": + people_detections.append( + ( + detection, + response.cropped_imgs[index], + response.polygon_ids[index], + ) + ) + rospy.loginfo(f"Found person at: {detection.point}") + + # Filter out people detections that are on seats + filtered_seats = [] + for seat_det in seat_detections: + seat = seat_det[0] + seat_removed = False + seat_seg = np.array([seat.xyseg]).reshape(-1, 2) + seat_polygon = ShapelyPolygon(seat_seg) + for person in people_detections: + person_seg = np.array([person[0].xyseg]).reshape(-1, 2) + person_polygon = ShapelyPolygon(person_seg) + if ( + person_polygon.intersection(seat_polygon).area + / person_polygon.area + >= self.overlap_threshold + ): + rospy.loginfo( + f"Person detected on seat. Removing seat: {seat.point}" + ) + seat_removed = True + break + if not seat_removed: + filtered_seats.append(seat_det) + + # Each is a list of (Detection, Image, PolygonIndex) pairs. + userdata.empty_seat_detections = filtered_seats + userdata.people_detections = people_detections + rospy.loginfo( + f"Found {len(filtered_seats)} empty seats and {len(people_detections)} people." + ) + + except Exception as e: + rospy.logerr(f"Failed to process detections: {str(e)}") + return "failed" + return "succeeded" + + class GetLookPoint(smach.State): + + def __init__(self, guest_id: str): + smach.State.__init__( + self, + outcomes=["succeeded"], + input_keys=["matched_face_detections"], + output_keys=["look_point"], + ) + self._guest_id = guest_id + + def execute(self, userdata): + if len(userdata.matched_face_detections) == 0: + userdata.look_point = PointStamped() + return "succeeded" + + for detection in userdata.matched_face_detections: + if detection.name == self._guest_id: + look_point = PointStamped( + point=detection.point, header=Header(frame_id="map") + ) + userdata.look_point = look_point + return "succeeded" + userdata.look_point = PointStamped() + return "succeeded" + + class SelectSeat(smach.State): + + def __init__(self, sofa_position: Point, max_people_on_sofa: int): + smach.State.__init__( + self, + outcomes=["succeeded_sofa", "succeeded_chair", "failed"], + input_keys=["empty_seat_detections", "people_detections"], + output_keys=["seat_position"], + ) + self._sofa_position = sofa_position + self._max_people_on_sofa = max_people_on_sofa + + def execute(self, userdata): + + num_people_on_sofa = 0 + + for detection in userdata.people_detections: + + if detection[2] == 1: # is inside the sofa area + num_people_on_sofa += 1 + + if num_people_on_sofa < self._max_people_on_sofa: + + userdata.seat_position = PointStamped( + point=self._sofa_position, header=Header(frame_id="map") + ) + return "succeeded_sofa" + + if len(userdata.empty_seat_detections) > 0: + userdata.seat_position = PointStamped( + point=userdata.empty_seat_detections[0][0].point, + header=Header(frame_id="map"), + ) + return "succeeded_chair" + + return "failed" + + def __init__( + self, + guest_id: str, + guests_to_introduce_to: List[str], + seating_area: ShapelyPolygon, + sofa_area: ShapelyPolygon, + sofa_position: Point, + max_people_on_sofa: int, + ): + + smach.StateMachine.__init__( + self, outcomes=["succeeded", "failed"], input_keys=["guest_data"] + ) + + """ + 1. Detect all people and seats + 2. Introduce the guest to the other guests + 3. Seat the guest + + """ + + rospy.sleep(5.0) + + with self: + + # Detect people and seats + smach.StateMachine.add( + "DETECT_PEOPLE_AND_SEATS", + smach_ros.ServiceState( + "/vision/cropped_detection", + CroppedDetection, + request=CroppedDetectionRequest( + requests=[ + CDRequest( + method="closest", + use_mask=True, + yolo_model="yolov8x-seg.pt", + yolo_model_confidence=0.5, + yolo_nms_threshold=0.3, + return_sensor_reading=False, + object_names=["person", "chair"], + polygons=[ + Polygon( + points=[ + Point( + x=point[0], + y=point[1], + z=0.0, + ) + for point in seating_area.exterior.coords + ] + ), + Polygon( + points=[ + Point( + x=point[0], + y=point[1], + z=0.0, + ) + for point in sofa_area.exterior.coords + ] + ), + ], + ) + ] + ), + output_keys=["response"], + response_slots=["responses"], + ), + transitions={ + "succeeded": "HANDLE_RESPONSE", + "aborted": "failed", + "preempted": "failed", + }, + remapping={"response": "detections"}, + ) + + smach.StateMachine.add( + "HANDLE_RESPONSE", + self.HandleResponse(), + transitions={"succeeded": "RECOGNISE_PEOPLE", "failed": "failed"}, + ) + + smach.StateMachine.add( + "RECOGNISE_PEOPLE", + self.RecognisePeople(), + transitions={ + "succeeded": ( + f"GET_LOOK_POINT_{guests_to_introduce_to[0]}" + if len(guests_to_introduce_to) > 0 + else "succeeded" + ), + "failed": "failed", + }, + ) + + for i, guest_to_introduce_to in enumerate(guests_to_introduce_to): + + smach.StateMachine.add( + f"GET_LOOK_POINT_{guest_to_introduce_to}", + self.GetLookPoint(guest_to_introduce_to), + transitions={"succeeded": f"LOOK_AT_{guest_to_introduce_to}"}, + ) + + smach.StateMachine.add( + f"LOOK_AT_{guest_to_introduce_to}", + LookToPoint(), + transitions={ + "succeeded": f"INTRODUCE_{guest_id}_TO_{guest_to_introduce_to}", + "aborted": f"INTRODUCE_{guest_id}_TO_{guest_to_introduce_to}", + "timed_out": f"INTRODUCE_{guest_id}_TO_{guest_to_introduce_to}", + }, + remapping={"pointstamped": "look_point"}, + ) + + smach.StateMachine.add( + f"INTRODUCE_{guest_id}_TO_{guest_to_introduce_to}", + Introduce( + guest_to_introduce=guest_id, + guest_to_introduce_to=guest_to_introduce_to, + ), + transitions={ + "succeeded": ( + "SELECT_SEAT" + if i == len(guests_to_introduce_to) - 1 + else f"GET_LOOK_POINT_{guests_to_introduce_to[i+1]}" + ) + }, + ) + + smach.StateMachine.add( + "SELECT_SEAT", + self.SelectSeat(sofa_position, max_people_on_sofa), + transitions={ + "succeeded_sofa": "SAY_SOFA", + "succeeded_chair": "SAY_CHAIR", + "failed": "SAY_ANY", + }, + ) + + smach.StateMachine.add( + "SAY_SOFA", + Say(text="Please sit on the sofa"), + transitions={ + "succeeded": "LOOK_AT_SEAT", + "preempted": "LOOK_AT_SEAT", + "aborted": "LOOK_AT_SEAT", + }, + ) + + smach.StateMachine.add( + "SAY_CHAIR", + Say(text="Please sit on the chair that I am looking at"), + transitions={ + "succeeded": "LOOK_AT_SEAT", + "preempted": "LOOK_AT_SEAT", + "aborted": "LOOK_AT_SEAT", + }, + ) + + smach.StateMachine.add( + "SAY_ANY", + Say(text="Please sit on any empty seat"), + transitions={ + "succeeded": "succeeded", + "preempted": "succeeded", + "aborted": "succeeded", + }, + ) + + smach.StateMachine.add( + "LOOK_AT_SEAT", + LookToPoint(), + transitions={ + "succeeded": "succeeded", + "aborted": "succeeded", + "timed_out": "succeeded", + }, + remapping={"pointstamped": "seat_position"}, + ) diff --git a/tasks/receptionist/src/receptionist/states/receptionist_learn_face.py b/tasks/receptionist/src/receptionist/states/receptionist_learn_face.py index df87a2a68..e29fcb80d 100644 --- a/tasks/receptionist/src/receptionist/states/receptionist_learn_face.py +++ b/tasks/receptionist/src/receptionist/states/receptionist_learn_face.py @@ -25,7 +25,7 @@ class ReceptionistLearnFaces(smach.State): _dataset_size: int _learn_face: rospy.ServiceProxy - def __init__(self, guest_id: str, dataset_size: int = 20): + def __init__(self, guest_id: str, dataset_size: int = 10): smach.State.__init__( self, outcomes=["succeeded", "failed"], input_keys=["guest_data"] ) From e6e083ab60bc2060b241794a5a936a26d945c8e4 Mon Sep 17 00:00:00 2001 From: Haiwei L Date: Fri, 5 Jul 2024 03:05:19 +0100 Subject: [PATCH 06/17] make state machine for getting name and drink --- .../src/receptionist/states/__init__.py | 4 +- .../receptionist/states/get_name_and_drink.py | 213 +++++++++++------- .../receptionist/states/get_name_or_drink.py | 145 +++++++----- .../src/receptionist/states/handle_guest.py | 16 +- 4 files changed, 231 insertions(+), 147 deletions(-) diff --git a/tasks/receptionist/src/receptionist/states/__init__.py b/tasks/receptionist/src/receptionist/states/__init__.py index 3ecdea3ab..deab34032 100644 --- a/tasks/receptionist/src/receptionist/states/__init__.py +++ b/tasks/receptionist/src/receptionist/states/__init__.py @@ -1,10 +1,10 @@ -from .get_name_and_drink import ParseNameAndDrink +from .get_name_and_drink import GetNameAndDrink from .get_attributes import GetGuestAttributes from .introduce import Introduce from .find_and_look_at import FindAndLookAt from .receptionist_learn_face import ReceptionistLearnFaces from .detect_faces import DetectFaces -from .get_name_or_drink import ParseTranscribedInfo +from .get_name_or_drink import GetNameOrDrink from .handle_guest import HandleGuest from .pointcloud_sweep import PointCloudSweep from .run_and_process_detections import RunAndProcessDetections diff --git a/tasks/receptionist/src/receptionist/states/get_name_and_drink.py b/tasks/receptionist/src/receptionist/states/get_name_and_drink.py index 1ef8c91a4..d4f8b2e57 100644 --- a/tasks/receptionist/src/receptionist/states/get_name_and_drink.py +++ b/tasks/receptionist/src/receptionist/states/get_name_and_drink.py @@ -8,94 +8,143 @@ from smach import UserData from typing import List, Dict, Any - -class ParseNameAndDrink(smach.State): - def __init__( - self, - guest_id: str, - param_key: str = "/receptionist/priors", - ): - """Parses the transcription of the guests' name and favourite drink. - - Args: - param_key (str, optional): Name of the parameter that contains the list of - possible . Defaults to "/receptionist/priors". - """ - smach.State.__init__( +class GetNameAndDrink(smach.StateMachine): + class ParseNameAndDrink(smach.State): + def __init__( self, - outcomes=["succeeded", "failed", "failed_name", "failed_drink"], - input_keys=["guest_transcription", "guest_data"], - output_keys=["guest data", "guest_transcription"], - ) - self._guest_id = guest_id - prior_data: Dict[str, List[str]] = rospy.get_param(param_key) - self._possible_names = [name.lower() for name in prior_data["names"]] - self._possible_drinks = [drink.lower() for drink in prior_data["drinks"]] - - def execute(self, userdata: UserData) -> str: - """Parses the transcription of the guests' name and favourite drink. - - Args: - userdata (UserData): State machine userdata assumed to contain a key - called "guest transcription" with the transcription of the guest's name and - favourite drink. - - Returns: - str: state outcome. Updates the userdata with the parsed name and drink, under - the parameter "guest data". - """ - outcome = "succeeded" - name_found = False - drink_found = False - transcription = userdata.guest_transcription.lower() - - transcription = userdata["guest_transcription"].lower() - - for name in self._possible_names: - if name in transcription: - userdata.guest_data[self._guest_id]["name"] = name - rospy.loginfo(f"Guest Name identified as: {name}") - name_found = True - break - - for drink in self._possible_drinks: - if drink in transcription: - userdata.guest_data[self._guest_id]["drink"] = drink - rospy.loginfo(f"Guest Drink identified as: {drink}") - drink_found = True - break - - if not name_found: - rospy.loginfo("Name not found in transcription") - userdata.guest_data[self._guest_id]["name"] = "unknown" - outcome = "failed" - if not drink_found: - rospy.loginfo("Drink not found in transcription") - userdata.guest_data[self._guest_id]["drink"] = "unknown" - outcome = "failed" - - { - "name": name, - "drink": drink, - }, - - if outcome == "failed": + guest_id: str, + param_key: str = "/receptionist/priors", + ): + """Parses the transcription of the guests' name and favourite drink. + + Args: + param_key (str, optional): Name of the parameter that contains the list of + possible . Defaults to "/receptionist/priors". + """ + smach.State.__init__( + self, + outcomes=["succeeded", "failed", "failed_name", "failed_drink"], + input_keys=["guest_transcription", "guest_data"], + output_keys=["guest_data", "guest_transcription"], + ) + self._guest_id = guest_id + prior_data: Dict[str, List[str]] = rospy.get_param(param_key) + self._possible_names = [name.lower() for name in prior_data["names"]] + self._possible_drinks = [drink.lower() for drink in prior_data["drinks"]] + + def execute(self, userdata: UserData) -> str: + """Parses the transcription of the guests' name and favourite drink. + + Args: + userdata (UserData): State machine userdata assumed to contain a key + called "guest transcription" with the transcription of the guest's name and + favourite drink. + + Returns: + str: state outcome. Updates the userdata with the parsed name and drink, under + the parameter "guest_data". + """ + outcome = "succeeded" + name_found = False + drink_found = False + transcription = userdata.guest_transcription.lower() + + transcription = userdata["guest_transcription"].lower() + + for name in self._possible_names: + if name in transcription: + userdata.guest_data[self._guest_id]["name"] = name + rospy.loginfo(f"Guest Name identified as: {name}") + name_found = True + break + + for drink in self._possible_drinks: + if drink in transcription: + userdata.guest_data[self._guest_id]["drink"] = drink + rospy.loginfo(f"Guest Drink identified as: {drink}") + drink_found = True + break + + if not name_found: + rospy.loginfo("Name not found in transcription") + userdata.guest_data[self._guest_id]["name"] = "unknown" + outcome = "failed" + if not drink_found: + rospy.loginfo("Drink not found in transcription") + userdata.guest_data[self._guest_id]["drink"] = "unknown" + outcome = "failed" + + return outcome + + class PostRecoveryDecision(smach.State): + def __init__( + self, + guest_id: str, + param_key: str = "/receptionist/priors", + ): + + smach.State.__init__( + self, + outcomes=["succeeded", "failed", "failed_name", "failed_drink"], + input_keys=["guest_transcription", "guest_data"], + output_keys=["guest_data", "guest_transcription"], + ) + self._guest_id = guest_id + prior_data: Dict[str, List[str]] = rospy.get_param(param_key) + self._possible_names = [name.lower() for name in prior_data["names"]] + self._possible_drinks = [drink.lower() for drink in prior_data["drinks"]] + + def execute(self, userdata: UserData) -> str: if not self._recovery_name_and_drink_required(userdata): if userdata.guest_data[self._guest_id]["name"] == "unknown": outcome = "failed_name" else: outcome = "failed_drink" + else: + outcome = "failed" + return outcome + + def _recovery_name_and_drink_required(self, userdata: UserData) -> bool: + """Determine whether both the name and drink requires recovery. + + Returns: + bool: True if both attributes require recovery. + """ + if userdata.guest_data[self._guest_id]["name"] == "unknown": + if userdata.guest_data[self._guest_id]["drink"] == "unknown": + return True + else: + return False - return outcome + def __init__( + self, + guest_id: str, + param_key: str = "/receptionist/priors", + ): - def _recovery_name_and_drink_required(self, userdata: UserData) -> bool: - """Determine whether both the name and drink requires recovery. + self._guest_id = guest_id + self._param_key = param_key - Returns: - bool: True if both attributes require recovery. - """ - if userdata.guest_data[self._guest_id]["name"] == "unknown": - if userdata.guest_data[self._guest_id]["drink"] == "unknown": - return True - else: - return False + smach.StateMachine.__init__( + self, + outcomes=["succeeded", "failed", "failed_name", "failed_drink"], + input_keys=["guest_transcription", "guest_data"], + output_keys=["guest_data", "guest_transcription"], + + ) + with self: + + smach.StateMachine.add( + "PARSE_NAME_AND_DRINK", + self.ParseNameAndDrink(guest_id=self._guest_id, param_key=self._param_key), + transitions={"succeeded": "succeeded", "failed": "POST_RECOVERY_DECISION"}, + ) + smach.StateMachine.add( + "POST_RECOVERY_DECISION", + self.PostRecoveryDecision(guest_id=self._guest_id, param_key=self._param_key), + transitions={ + "failed": "failed", + "failed_name": "failed_name", + "failed_drink": "failed_drink", + }, + ) \ No newline at end of file diff --git a/tasks/receptionist/src/receptionist/states/get_name_or_drink.py b/tasks/receptionist/src/receptionist/states/get_name_or_drink.py index 2c425e832..9db23ff96 100644 --- a/tasks/receptionist/src/receptionist/states/get_name_or_drink.py +++ b/tasks/receptionist/src/receptionist/states/get_name_or_drink.py @@ -8,66 +8,101 @@ from smach import UserData from typing import List, Dict, Any +class GetNameOrDrink(smach.StateMachine): + class ParseTranscribedInfo(smach.State): + def __init__( + self, + guest_id: str, + info_type: str, + param_key: str = "/receptionist/priors", + ): + """Parses the transcription of the guests' information. -class ParseTranscribedInfo(smach.State): - def __init__( - self, - guest_id: str, - info_type: str, - param_key: str = "/receptionist/priors", - ): - """Parses the transcription of the guests' information. + Args: + guest_id (str): ID of the guest (identifying the guest) + info_type (str): The type of information to try and extract useful information + (drink or name) + param_key (str, optional): Name of the parameter that contains the list of + possible . Defaults to "receptionist/priors". + """ + smach.State.__init__( + self, + outcomes=["succeeded", "failed"], + input_keys=["guest_transcription", "guest_data"], + output_keys=["guest_data", "guest_transcription"], + ) + self._guest_id = guest_id + self._type = info_type + prior_data: Dict[str, List[str]] = rospy.get_param(param_key) + possible_drinks = [drink.lower() for drink in prior_data["drinks"]] + possible_names = [name.lower() for name in prior_data["names"]] + self._possible_information = {"drink": possible_drinks, "name": possible_names}[ + self._type + ] - Args: - guest_id (str): ID of the guest (identifying the guest) - info_type (str): The type of information to try and extract useful information - (drink or name) - param_key (str, optional): Name of the parameter that contains the list of - possible . Defaults to "receptionist/priors". - """ - smach.State.__init__( - self, - outcomes=["succeeded", "failed"], - input_keys=["guest_transcription", "guest_data"], - output_keys=["guest data", "guest_transcription"], - ) - self._guest_id = guest_id - self._type = info_type - prior_data: Dict[str, List[str]] = rospy.get_param(param_key) - possible_drinks = [drink.lower() for drink in prior_data["drinks"]] - possible_names = [name.lower() for name in prior_data["names"]] - self._possible_information = {"drink": possible_drinks, "name": possible_names}[ - self._type - ] + def execute(self, userdata: UserData) -> str: + """Parse the guest's information. - def execute(self, userdata: UserData) -> str: - """Parse the guest's information. + Args: + userdata (UserData): State machine userdata assumed to contain a key + called "guest transcription" with the transcription of the guest's name or + favourite drink or both. - Args: - userdata (UserData): State machine userdata assumed to contain a key - called "guest transcription" with the transcription of the guest's name or - favourite drink or both. + Returns: + str: state outcome. Updates the userdata with the parsed information (drink or name), under + the parameter "guest data". + """ + outcome = "succeeded" + information_found = False + transcription = userdata.guest_transcription.lower() - Returns: - str: state outcome. Updates the userdata with the parsed information (drink or name), under - the parameter "guest data". - """ - outcome = "succeeded" - information_found = False - transcription = userdata.guest_transcription.lower() + transcription = userdata["guest_transcription"].lower() - transcription = userdata["guest_transcription"].lower() + for key_phrase in self._possible_information: + print(self._possible_information) + if key_phrase in transcription: + userdata.guest_data[self._guest_id][self._type] = key_phrase + rospy.loginfo(f"Guest {self._type} identified as: {key_phrase}") + information_found = True + break + if not information_found: + rospy.loginfo(f"{self._type} not found in transcription") + userdata.guest_data[self._guest_id][self._type] = "unknown" + outcome = "failed" - for key_phrase in self._possible_information: - print(self._possible_information) - if key_phrase in transcription: - userdata.guest_data[self._guest_id][self._type] = key_phrase - rospy.loginfo(f"Guest {self._type} identified as: {key_phrase}") - information_found = True - break - if not information_found: - rospy.loginfo(f"{self._type} not found in transcription") - userdata.guest_data[self._guest_id][self._type] = "unknown" - outcome = "failed" + return outcome + + def __init__( + self, + guest_id: str, + info_type: str, + param_key: str = "/receptionist/priors", + ): + + self._guest_id = guest_id + self._info_type = info_type + self._param_key = param_key + + smach.StateMachine.__init__( + self, + outcomes=["succeeded", "failed"], + input_keys=["guest_transcription", "guest_data"], + output_keys=["guest_data", "guest_transcription"], + + ) + with self: - return outcome + smach.StateMachine.add( + "PARSE_NAME_OR_DRINK", + self.ParseTranscribedInfo(guest_id=self._guest_id, info_type=self._info_type, param_key=self._param_key), + transitions={"succeeded": "succeeded", "failed": "failed"}, + ) + # smach.StateMachine.add( + # "POST_RECOVERY_DECISION", + # self.PostRecoveryDecision(guest_id=self._guest_id, param_key=self._param_key), + # transitions={ + # "failed": "failed", + # "failed_name": "failed_name", + # "failed_drink": "failed_drink", + # } + # ) diff --git a/tasks/receptionist/src/receptionist/states/handle_guest.py b/tasks/receptionist/src/receptionist/states/handle_guest.py index 0d75d7440..71a2dafbc 100644 --- a/tasks/receptionist/src/receptionist/states/handle_guest.py +++ b/tasks/receptionist/src/receptionist/states/handle_guest.py @@ -2,8 +2,8 @@ from lasr_skills import AskAndListen, Say, AdjustCamera from receptionist.states import ( - ParseNameAndDrink, - ParseTranscribedInfo, + GetNameAndDrink, + GetNameOrDrink, ReceptionistLearnFaces, GetGuestAttributes, ) @@ -11,7 +11,7 @@ class HandleGuest(smach.StateMachine): - class GetNameAndDrink(smach.StateMachine): + class HandleNameAndDrink(smach.StateMachine): def __init__(self, guest_id: str): super().__init__( outcomes=["succeeded", "failed"], @@ -31,7 +31,7 @@ def __init__(self, guest_id: str): smach.StateMachine.add( f"PARSE_NAME_AND_DRINK_GUEST_{guest_id}", - ParseNameAndDrink(guest_id), + GetNameAndDrink(guest_id), transitions={ "succeeded": "succeeded", "failed": f"REPEAT_GET_NAME_AND_DRINK_GUEST_{guest_id}", @@ -54,7 +54,7 @@ def __init__(self, guest_id: str): smach.StateMachine.add( f"REPEAT_PARSE_NAME_AND_DRINK_GUEST_{guest_id}", - ParseNameAndDrink(guest_id), + GetNameAndDrink(guest_id), transitions={ "succeeded": "succeeded", "failed": "succeeded", @@ -79,7 +79,7 @@ def __init__(self, guest_id: str): smach.StateMachine.add( f"REPEAT_PARSE_NAME_GUEST_{guest_id}", - ParseTranscribedInfo(guest_id, "name"), + GetNameOrDrink(guest_id, "name"), transitions={ "succeeded": "succeeded", "failed": "succeeded", @@ -104,7 +104,7 @@ def __init__(self, guest_id: str): smach.StateMachine.add( f"REPEAT_PARSE_DRINK_GUEST_{guest_id}", - ParseTranscribedInfo(guest_id, "drink"), + GetNameOrDrink(guest_id, "drink"), transitions={ "succeeded": "succeeded", "failed": "succeeded", @@ -249,7 +249,7 @@ def __init__(self, guest_id: str, learn_face: bool): with sm_con: smach.Concurrence.add( "GET_NAME_AND_DRINK", - self.GetNameAndDrink(guest_id), + self.HandleNameAndDrink(guest_id), ) smach.Concurrence.add( From 7a085cc1e6bf8af55eaaca16fb19b4a86b491f2d Mon Sep 17 00:00:00 2001 From: Jared Swift Date: Fri, 5 Jul 2024 03:05:56 +0100 Subject: [PATCH 07/17] refactor: black --- .../scripts/test_microphones.py | 2 +- .../src/lasr_vision_bodypix/bodypix.py | 12 +- skills/src/lasr_skills/adjust_camera.py | 252 ++++++++++++------ skills/src/lasr_skills/describe_people.py | 1 - skills/src/lasr_skills/detect_3d_in_area.py | 5 +- .../lasr_skills/vision/get_cropped_image.py | 15 +- tasks/receptionist/scripts/test_seat_guest.py | 2 - .../receptionist/states/find_and_look_at.py | 7 +- 8 files changed, 194 insertions(+), 102 deletions(-) diff --git a/common/speech/lasr_speech_recognition_whisper/scripts/test_microphones.py b/common/speech/lasr_speech_recognition_whisper/scripts/test_microphones.py index c67f3a177..921097691 100644 --- a/common/speech/lasr_speech_recognition_whisper/scripts/test_microphones.py +++ b/common/speech/lasr_speech_recognition_whisper/scripts/test_microphones.py @@ -34,7 +34,7 @@ def main(args: dict) -> None: output_dir = args["output_dir"] r = sr.Recognizer() - with sr.Microphone(device_index=13,sample_rate=16000) as source: + with sr.Microphone(device_index=13, sample_rate=16000) as source: print("Say something!") audio = r.listen(source, timeout=5, phrase_time_limit=5) print("Finished listening") diff --git a/common/vision/lasr_vision_bodypix/src/lasr_vision_bodypix/bodypix.py b/common/vision/lasr_vision_bodypix/src/lasr_vision_bodypix/bodypix.py index f6f0bc785..1e34fc738 100644 --- a/common/vision/lasr_vision_bodypix/src/lasr_vision_bodypix/bodypix.py +++ b/common/vision/lasr_vision_bodypix/src/lasr_vision_bodypix/bodypix.py @@ -23,7 +23,7 @@ import rospkg # model cache -# preload resnet 50 model so that it won't waste the time +# preload resnet 50 model so that it won't waste the time # doing that in the middle of the task. loaded_models = { "resnet50": load_model(download_model(BodyPixModelPaths.RESNET50_FLOAT_STRIDE_16)) @@ -171,7 +171,11 @@ def detect_keypoints( BodyPixKeypoint(keypoint_name=keypoint.part, x=x, y=y) ) detected_keypoints_normalized.append( - BodyPixKeypointNormalized(keypoint_name=keypoint.part, x=float(x)/mask.shape[1], y=float(y)/mask.shape[0]) + BodyPixKeypointNormalized( + keypoint_name=keypoint.part, + x=float(x) / mask.shape[1], + y=float(y) / mask.shape[0], + ) ) # publish to debug topic @@ -201,4 +205,6 @@ def detect_keypoints( ) debug_publisher.publish(cv2_img.cv2_img_to_msg(coloured_mask)) - return BodyPixKeypointDetectionResponse(keypoints=detected_keypoints, normalized_keypoints=detected_keypoints_normalized) + return BodyPixKeypointDetectionResponse( + keypoints=detected_keypoints, normalized_keypoints=detected_keypoints_normalized + ) diff --git a/skills/src/lasr_skills/adjust_camera.py b/skills/src/lasr_skills/adjust_camera.py index 1895841af..c885398ae 100644 --- a/skills/src/lasr_skills/adjust_camera.py +++ b/skills/src/lasr_skills/adjust_camera.py @@ -12,62 +12,75 @@ LEFT = { - 'leftEye', + "leftEye", # 'leftEar', - 'leftShoulder', + "leftShoulder", } RIGHT = { - 'rightEye', + "rightEye", # 'rightEar', - 'rightShoulder', + "rightShoulder", } HEAD = { # 'nose', - 'leftEye', - 'rightEye', + "leftEye", + "rightEye", # 'leftEar', # 'rightEar', } MIDDLE = { - 'leftShoulder', - 'rightShoulder', + "leftShoulder", + "rightShoulder", } TORSO = { - 'leftWrist', - 'rightWrist', - 'leftHip', - 'rightHip', + "leftWrist", + "rightWrist", + "leftHip", + "rightHip", } ALL_KEYS_WITHOUT_TORSO = LEFT.union(RIGHT).union(HEAD).union(MIDDLE) ALL_KEYS = ALL_KEYS_WITHOUT_TORSO.union(TORSO) -positions = ['u3l', 'u3m', 'u3r', 'u2l', 'u2m', 'u2r', 'u1l', 'u1m', 'u1r', 'ml', 'mm', 'mr',] +positions = [ + "u3l", + "u3m", + "u3r", + "u2l", + "u2m", + "u2r", + "u1l", + "u1m", + "u1r", + "ml", + "mm", + "mr", +] position_dict = { - (3, -1): 'u3l', - (3, 0): 'u3m', - (3, 1): 'u3r', - (2, -1): 'u2l', - (2, 0): 'u2m', - (2, 1): 'u2r', - (1, -1): 'u1l', - (1, 0): 'u1m', - (1, 1): 'u1r', - (0, -1): 'ml', - (0, 0): 'mm', - (0, 1): 'mr', + (3, -1): "u3l", + (3, 0): "u3m", + (3, 1): "u3r", + (2, -1): "u2l", + (2, 0): "u2m", + (2, 1): "u2r", + (1, -1): "u1l", + (1, 0): "u1m", + (1, 1): "u1r", + (0, -1): "ml", + (0, 0): "mm", + (0, 1): "mr", } class AdjustCamera(smach.StateMachine): def __init__( - self, + self, bodypix_model: str = "resnet50", bodypix_confidence: float = 0.7, max_attempts=1000, @@ -75,7 +88,11 @@ def __init__( ): smach.StateMachine.__init__( self, - outcomes=["finished", "failed", "truncated",], + outcomes=[ + "finished", + "failed", + "truncated", + ], input_keys=[], output_keys=[], ) @@ -88,8 +105,8 @@ def __init__( with self: smach.StateMachine.add( - 'init_u2m', - PlayMotion(motion_name='u2m'), + "init_u2m", + PlayMotion(motion_name="u2m"), transitions={ "succeeded": "GET_IMAGE", "aborted": "GET_IMAGE", @@ -122,16 +139,16 @@ def __init__( if debug: _transitions = { - "finished": "GET_IMAGE", - "failed": "GET_IMAGE", - "truncated": "GET_IMAGE", - } + "finished": "GET_IMAGE", + "failed": "GET_IMAGE", + "truncated": "GET_IMAGE", + } else: _transitions = { - "finished": "finished", - "failed": "failed", - "truncated": "truncated", - } + "finished": "finished", + "failed": "failed", + "truncated": "truncated", + } for position in positions: _transitions[position] = position @@ -158,7 +175,7 @@ def __init__( class DecideAdjustCamera(smach.State): def __init__( - self, + self, # keypoints_to_detect: List[str] = ALL_KEYS, bodypix_model: str = "resnet50", bodypix_confidence: float = 0.7, @@ -166,8 +183,15 @@ def __init__( ): smach.State.__init__( self, - outcomes=["finished", "failed", 'truncated',] + positions, - input_keys=["img_msg",], + outcomes=[ + "finished", + "failed", + "truncated", + ] + + positions, + input_keys=[ + "img_msg", + ], output_keys=[], ) self.max_attempts = max_attempts @@ -180,9 +204,9 @@ def __init__( self.position = [2, 0] self.counter = 0 - + def execute(self, userdata): - + req = BodyPixKeypointDetectionRequest() req.image_raw = userdata.img_msg req.dataset = self._bodypix_model @@ -194,7 +218,7 @@ def execute(self, userdata): except Exception as e: print(e) return "failed" - + detected_keypoints = res.normalized_keypoints keypoint_names = [keypoint.keypoint_name for keypoint in detected_keypoints] @@ -204,121 +228,181 @@ def execute(self, userdata): for keypoint in detected_keypoints } missing_keypoints = { - keypoint - for keypoint in ALL_KEYS - if keypoint not in keypoint_names + keypoint for keypoint in ALL_KEYS if keypoint not in keypoint_names } has_both_shoulders = len(missing_keypoints.intersection(MIDDLE)) == 0 has_both_eyes = len(missing_keypoints.intersection(HEAD)) == 0 - has_more_than_one_shoulder = len(missing_keypoints.intersection(MIDDLE)) <= 1 + has_more_than_one_shoulder = ( + len(missing_keypoints.intersection(MIDDLE)) <= 1 + ) has_more_than_one_one_eye = len(missing_keypoints.intersection(HEAD)) <= 1 rospy.logwarn(f"missing keypoints: {missing_keypoints}") - rospy.logwarn(f"missing shoulders: {missing_keypoints.intersection(MIDDLE)}, missing eyes: {missing_keypoints.intersection(HEAD)}") + rospy.logwarn( + f"missing shoulders: {missing_keypoints.intersection(MIDDLE)}, missing eyes: {missing_keypoints.intersection(HEAD)}" + ) # has_torso = len(missing_keypoints.intersection(TORSO)) <= 1 - - if not has_more_than_one_shoulder and not has_more_than_one_one_eye: + + if not has_more_than_one_shoulder and not has_more_than_one_one_eye: # 'Try recovery behaviour or give up, need a bit polish miss_head = len(missing_keypoints.intersection(HEAD)) >= 2 miss_middle = len(missing_keypoints.intersection(MIDDLE)) >= 2 miss_torso = len(missing_keypoints.intersection(TORSO)) >= 4 miss_left = len(missing_keypoints.intersection(LEFT)) >= 1 miss_right = len(missing_keypoints.intersection(RIGHT)) >= 1 - rospy.logwarn(f"Missing head: {miss_head}, middle: {miss_middle}, torso: {miss_torso}, left: {miss_left}, right: {miss_right}.") + rospy.logwarn( + f"Missing head: {miss_head}, middle: {miss_middle}, torso: {miss_torso}, left: {miss_left}, right: {miss_right}." + ) needs_to_move_up = miss_head and (not miss_torso or not miss_middle) needs_to_move_down = not miss_head and miss_middle and miss_torso needs_to_move_left = miss_right needs_to_move_right = miss_left - rospy.logwarn(f"Needs to move up: {needs_to_move_up}, down: {needs_to_move_down}, left: {needs_to_move_left}, right: {needs_to_move_right}.") - + rospy.logwarn( + f"Needs to move up: {needs_to_move_up}, down: {needs_to_move_down}, left: {needs_to_move_left}, right: {needs_to_move_right}." + ) + # if counter > maxmum, check if head is in, if not, move up to get head, otherwise return finished. if self.counter > self.max_attempts: if not miss_head or self.counter > self.max_attempts + 2: return "truncated" - + # self.counter += 1 if not (needs_to_move_left and needs_to_move_right): # return "failed" if needs_to_move_left: - self.position = (self.position[0], self.position[1] - 1 if self.position[1] > -1 else self.position[1]) + self.position = ( + self.position[0], + ( + self.position[1] - 1 + if self.position[1] > -1 + else self.position[1] + ), + ) return position_dict[self.position] if needs_to_move_right: - self.position = (self.position[0], self.position[1] + 1 if self.position[1] < 1 else self.position[1]) + self.position = ( + self.position[0], + ( + self.position[1] + 1 + if self.position[1] < 1 + else self.position[1] + ), + ) return position_dict[self.position] if needs_to_move_up and needs_to_move_down: return "failed" if needs_to_move_up: - self.position = (self.position[0] + 1 if self.position[0] < 3 else self.position[0], self.position[1]) + self.position = ( + ( + self.position[0] + 1 + if self.position[0] < 3 + else self.position[0] + ), + self.position[1], + ) return position_dict[userdata.position] if needs_to_move_down: - self.position = (self.position[0] - 1 if self.position[0] > 0 else self.position[0], self.position[1]) + self.position = ( + ( + self.position[0] - 1 + if self.position[0] > 0 + else self.position[0] + ), + self.position[1], + ) return position_dict[userdata.position] return "finished" elif has_both_eyes and not has_both_shoulders: # in this case try to make eyes into the upper 1/3 of the frame, - eyes_middle = ((keypoint_info["leftEye"][0] + keypoint_info["rightEye"][0]) / 2, (keypoint_info["leftEye"][1] + keypoint_info["rightEye"][1]) / 2) + eyes_middle = ( + (keypoint_info["leftEye"][0] + keypoint_info["rightEye"][0]) / 2, + (keypoint_info["leftEye"][1] + keypoint_info["rightEye"][1]) / 2, + ) # if y at down 1/5: down move 2 steps - if eyes_middle[1] >= 4/5: + if eyes_middle[1] >= 4 / 5: self.position[0] -= 2 # if y at down 1/2: down move 1 step - elif eyes_middle[1] >= 1/2: + elif eyes_middle[1] >= 1 / 2: self.position[0] -= 1 # if y at upper 1/3: wonder why no shoulders but never mind in this case else: pass # if x at left 1/3 or left shoulder dissappear, move left 1 step - if eyes_middle[0] <= 1/3: + if eyes_middle[0] <= 1 / 3: self.position[1] -= 1 # if x at right 1/3 or right shoulder dissappear, move right 1 step - elif eyes_middle[0] >= 2/3: + elif eyes_middle[0] >= 2 / 3: self.position[1] += 1 pass elif not has_both_eyes and has_both_shoulders: - shoulders_middle = ((keypoint_info["leftShoulder"][0] + keypoint_info["rightShoulder"][0]) / 2, (keypoint_info["leftEye"][1] + keypoint_info["rightEye"][1]) / 2) + shoulders_middle = ( + ( + keypoint_info["leftShoulder"][0] + + keypoint_info["rightShoulder"][0] + ) + / 2, + (keypoint_info["leftEye"][1] + keypoint_info["rightEye"][1]) / 2, + ) # if y at down 1/5: down move 1 step - if shoulders_middle[1] >= 4/5: + if shoulders_middle[1] >= 4 / 5: self.position[0] -= 1 # if y at upper 1/4: up move 1 step - elif shoulders_middle[1] <= 1/4: + elif shoulders_middle[1] <= 1 / 4: self.position[0] += 1 # if x at left 1/3, move left 1 step - if shoulders_middle[0] <= 1/3: + if shoulders_middle[0] <= 1 / 3: self.position[1] -= 1 # if x at right 1/3, move right 1 step - elif shoulders_middle[0] >= 2/3: + elif shoulders_middle[0] >= 2 / 3: self.position[1] += 1 pass elif has_both_eyes and has_both_shoulders: - eyes_middle = ((keypoint_info["leftEye"][0] + keypoint_info["rightEye"][0]) / 2, (keypoint_info["leftEye"][1] + keypoint_info["rightEye"][1]) / 2) - shoulders_middle = ((keypoint_info["leftShoulder"][0] + keypoint_info["rightShoulder"][0]) / 2, (keypoint_info["leftEye"][1] + keypoint_info["rightEye"][1]) / 2) - very_middle = ((eyes_middle[0] + shoulders_middle[0]) / 2, (eyes_middle[1] + shoulders_middle[1]) / 2) + eyes_middle = ( + (keypoint_info["leftEye"][0] + keypoint_info["rightEye"][0]) / 2, + (keypoint_info["leftEye"][1] + keypoint_info["rightEye"][1]) / 2, + ) + shoulders_middle = ( + ( + keypoint_info["leftShoulder"][0] + + keypoint_info["rightShoulder"][0] + ) + / 2, + (keypoint_info["leftEye"][1] + keypoint_info["rightEye"][1]) / 2, + ) + very_middle = ( + (eyes_middle[0] + shoulders_middle[0]) / 2, + (eyes_middle[1] + shoulders_middle[1]) / 2, + ) rospy.logwarn(f"very middle {very_middle}") # if y at upper 1/5 for eyes: move up 1 step - if eyes_middle[1] <= 1/5: + if eyes_middle[1] <= 1 / 5: self.position[0] += 1 - print('if y at upper 1/5 for eyes: move up 1 step') + print("if y at upper 1/5 for eyes: move up 1 step") else: - if 1/4 <= very_middle[1] <= 2/3 and 1/3 <= very_middle[0] <= 2/3: - print('finished.') + if ( + 1 / 4 <= very_middle[1] <= 2 / 3 + and 1 / 3 <= very_middle[0] <= 2 / 3 + ): + print("finished.") return "finished" # if y at down 1/3: down move 1 step - if very_middle[1] >= 2/3: + if very_middle[1] >= 2 / 3: self.position[0] -= 1 - print('if y at down 1/3: down move 1 step.') + print("if y at down 1/3: down move 1 step.") # if y at upper 1/4: up move 1 step - elif very_middle[1] <= 1/4: + elif very_middle[1] <= 1 / 4: self.position[0] += 1 - print('if y at upper 1/3: up move 1 step.') + print("if y at upper 1/3: up move 1 step.") # if x at left 1/3, move left 1 step - if very_middle[0] <= 1/3: + if very_middle[0] <= 1 / 3: self.position[1] -= 1 - print('if x at left 1/3, move left 1 step.') + print("if x at left 1/3, move left 1 step.") # if x at right 1/3, move right 1 step - elif very_middle[0] >= 2/3: + elif very_middle[0] >= 2 / 3: self.position[1] += 1 - print('if x at right 1/3, move right 1 step.') + print("if x at right 1/3, move right 1 step.") pass elif has_more_than_one_shoulder: # but not both # shoulders_middle = ((keypoint_info["leftShoulder"][0] + keypoint_info["rightShoulder"][0]) / 2, (keypoint_info["leftEye"][1] + keypoint_info["rightEye"][1]) / 2) @@ -357,5 +441,5 @@ def execute(self, userdata): self.position[1] = -1 elif self.position[1] > 1: self.position[1] = 1 - + return position_dict[(self.position[0], self.position[1])] diff --git a/skills/src/lasr_skills/describe_people.py b/skills/src/lasr_skills/describe_people.py index d6579bb8e..8479ed251 100755 --- a/skills/src/lasr_skills/describe_people.py +++ b/skills/src/lasr_skills/describe_people.py @@ -241,4 +241,3 @@ def execute(self, userdata): # - mask userdata.people = people return "succeeded" - \ No newline at end of file diff --git a/skills/src/lasr_skills/detect_3d_in_area.py b/skills/src/lasr_skills/detect_3d_in_area.py index b4c785d65..7b5f3686b 100644 --- a/skills/src/lasr_skills/detect_3d_in_area.py +++ b/skills/src/lasr_skills/detect_3d_in_area.py @@ -8,6 +8,7 @@ from shapely.geometry.polygon import Polygon as ShapelyPolygon from std_msgs.msg import Header + class Detect3DInArea(smach.StateMachine): class FilterDetections(smach.State): def __init__( @@ -35,7 +36,9 @@ def execute(self, userdata): Point32(x=point[0], y=point[1], z=0.0) for point in self.area_polygon.exterior.coords ] - self.debug_publisher.publish(PolygonStamped(polygon=polygon_msg, header=Header(frame_id="map"))) + self.debug_publisher.publish( + PolygonStamped(polygon=polygon_msg, header=Header(frame_id="map")) + ) satisfied_points = [ self.area_polygon.contains(Point(object.point.x, object.point.y)) for object in detected_objects diff --git a/skills/src/lasr_skills/vision/get_cropped_image.py b/skills/src/lasr_skills/vision/get_cropped_image.py index 6730f5855..459ee258d 100755 --- a/skills/src/lasr_skills/vision/get_cropped_image.py +++ b/skills/src/lasr_skills/vision/get_cropped_image.py @@ -16,6 +16,7 @@ class GetCroppedImage(smach.State): This state calls CroppedDetection service instead of running on its own. THis is a much faster version than the older one. """ + def __init__( self, object_name: str, @@ -45,12 +46,12 @@ def __init__( def execute(self, userdata) -> str: req = CDRequest() - req.method=self.method - req.use_mask=self.use_mask - req.yolo_model=self.yolo_model - req.yolo_model_confidence=self.yolo_model_confidence - req.yolo_nms_threshold=self.yolo_nms_threshold - req.object_names=[self.object_name] + req.method = self.method + req.use_mask = self.use_mask + req.yolo_model = self.yolo_model + req.yolo_model_confidence = self.yolo_model_confidence + req.yolo_nms_threshold = self.yolo_nms_threshold + req.object_names = [self.object_name] cropped_detection_req: CroppedDetectionRequest = CroppedDetectionRequest() cropped_detection_req.requests = [req] @@ -69,7 +70,7 @@ def execute(self, userdata) -> str: except Exception as e: # Got some errors that is not rospy. rospy.logerr(f"Service call failed: {e}") return "failed" - + if __name__ == "__main__": rospy.init_node("get_cropped_image") diff --git a/tasks/receptionist/scripts/test_seat_guest.py b/tasks/receptionist/scripts/test_seat_guest.py index f39213aa9..ae97aea10 100755 --- a/tasks/receptionist/scripts/test_seat_guest.py +++ b/tasks/receptionist/scripts/test_seat_guest.py @@ -13,7 +13,6 @@ seat_area_param = rospy.get_param("/seat_area") - max_people_on_sofa = rospy.get_param("/max_people_on_sofa") rospy.sleep(5) @@ -35,4 +34,3 @@ outcome = sm.execute() rospy.loginfo("Outcome: %s", outcome) rospy.signal_shutdown("Test completed.") - diff --git a/tasks/receptionist/src/receptionist/states/find_and_look_at.py b/tasks/receptionist/src/receptionist/states/find_and_look_at.py index c6173f15f..253f7330c 100755 --- a/tasks/receptionist/src/receptionist/states/find_and_look_at.py +++ b/tasks/receptionist/src/receptionist/states/find_and_look_at.py @@ -54,11 +54,13 @@ def execute(self, userdata): return "succeeded" else: if len(userdata.matched_face_detections) > 0: - self._set_userdata_look_position(userdata, userdata.matched_face_detections[0]) + self._set_userdata_look_position( + userdata, userdata.matched_face_detections[0] + ) return "succeeded" userdata.look_position = PointStamped() return "failed" - + def _check_named_host(self, userdata): for detection in userdata.matched_face_detections: if detection.name == "host": @@ -73,7 +75,6 @@ def _set_userdata_look_position(self, userdata, detection): point=look_position, header=Header(frame_id="map") ) - def __init__( self, guest_id: Union[None, str] = None, mask: Union[None, List[str]] = None ): From 39a5aacf432d4872a13df608408e635e6830f556 Mon Sep 17 00:00:00 2001 From: Matt Date: Fri, 5 Jul 2024 03:18:07 +0100 Subject: [PATCH 08/17] feat: create panorama --- .../src/receptionist/states/stitch_images.py | 74 +++++++++++++++++++ 1 file changed, 74 insertions(+) create mode 100644 tasks/receptionist/src/receptionist/states/stitch_images.py diff --git a/tasks/receptionist/src/receptionist/states/stitch_images.py b/tasks/receptionist/src/receptionist/states/stitch_images.py new file mode 100644 index 000000000..43ba985e7 --- /dev/null +++ b/tasks/receptionist/src/receptionist/states/stitch_images.py @@ -0,0 +1,74 @@ +#!/usr/bin/env python3 +from typing import List +import rospy +import smach +import numpy as np +from receptionist.states import PointCloudSweep +from geometry_msgs.msg import Polygon, Point, PointStamped +from sensor_msgs.msg import Image +from std_msgs.msg import Header +from stitching import Stitcher +from cv2_pcl import pcl_to_cv2 +from cv2_img import cv2_img_to_msg + + +class StitchImage(smach.State): + def __init__(self): + smach.State.__init__( + self, + outcomes=["succeeded", "failed"], + input_keys=["transformed_pointclouds"], + output_keys=["stitched_image"], + ) + self.stitcher = Stitcher() + self.sticher_pub = rospy.Publisher("/vision/sitched_img", Image, queue_size=1) + + def execute(self, userdata): + transformed_pointclouds = userdata.transformed_pointclouds + if len(transformed_pointclouds) == 0: + rospy.logerr("No point clouds to stitch") + return "failed" + + imgs = [pcl_to_cv2(pcl) for pcl in transformed_pointclouds] + stitched_image = self.stitcher.stitch(imgs) + self.sticher_pub.publish(cv2_img_to_msg(stitched_image)) + userdata.stitched_image = stitched_image + return "succeeded" + + +if __name__ == "__main__": + rospy.init_node("stitch_images") + sweep_points = [ + # Point(5.71, 3.06, 0.8), + Point(6.50, 3.53, 0.8), + Point(6.25, 3.53, 0.8), + Point(5.5, 3.53, 0.8), + Point(5.0, 3.53, 0.8), + Point(4.96, 3.53, 0.8), + Point(4.80, 3.73, 0.8), + ] + while not rospy.is_shutdown(): + sm = smach.StateMachine(outcomes=["succeeded", "failed"]) + with sm: + sm.userdata.transformed_pointclouds = [] + smach.StateMachine.add( + "Sweep", + PointCloudSweep( + sweep_points=sweep_points, + ), + transitions={ + "succeeded": "StitchImages", + "failed": "failed", + }, + ) + + smach.StateMachine.add( + "StitchImages", + StitchImage(), + transitions={"succeeded": "succeeded", "failed": "failed"}, + ) + + outcome = sm.execute() + # wait for user to press enter before running again + input("Press Enter to run again...") + rospy.spin() From 66c387b849fd091d49a5543377b8cf6ac75bceed Mon Sep 17 00:00:00 2001 From: Haiwei L Date: Fri, 5 Jul 2024 03:47:58 +0100 Subject: [PATCH 09/17] Add speech recovery (local) --- tasks/receptionist/config/lab.yaml | 11 +- .../states/local_speech_recognition.py | 180 ++++++++++++++++++ 2 files changed, 185 insertions(+), 6 deletions(-) create mode 100644 tasks/receptionist/src/receptionist/states/local_speech_recognition.py diff --git a/tasks/receptionist/config/lab.yaml b/tasks/receptionist/config/lab.yaml index 691ea63f3..75f68a4e0 100644 --- a/tasks/receptionist/config/lab.yaml +++ b/tasks/receptionist/config/lab.yaml @@ -31,20 +31,19 @@ priors: wait_pose: position: - x: 8.245934441303595 - y: 24.285935396834816 + x: 4.637585370589175 + y: 6.715591164127531 z: 0.0 orientation: x: 0.0 y: 0.0 - z: 0.08719394681831685 - w: 0.9961913549304895 - + z: 0.7827525387329473 + w: 0.6223330805180823 #0.478893309417269 #0.8778731105321406 # wait_area: [[2.65, -0.61], [4.21, -0.33], [4.58, -2.27], [2.67, -2.66]] -wait_area: [[9.16, 25.3], [10.6, 25.7], [11, 24.1], [9.4, 23.7]] +wait_area: [[3.60, 8.56], [4.69, 8.90], [4.98, 7.59], [3.95, 7.34]] seat_pose: position: x: 7.352258327332648 diff --git a/tasks/receptionist/src/receptionist/states/local_speech_recognition.py b/tasks/receptionist/src/receptionist/states/local_speech_recognition.py new file mode 100644 index 000000000..10bac5a5a --- /dev/null +++ b/tasks/receptionist/src/receptionist/states/local_speech_recognition.py @@ -0,0 +1,180 @@ +import jellyfish as jf + +from itertools import groupby +import pandas as pd +import numpy as np + + +available_names = ["adel", "angel", "axel", "charlie", "jane", "jules", "morgan", "paris", "robin", "simone"] +available_single_drinks = ["cola", "milk",] +available_double_drinks = ["iced", "tea", "pack", "juice", "orange", "red", "wine", "tropical"] +double_drinks_dict = {"iced" : "iced tea", + "tea": "iced tea", + "pack" : "juice pack", + "orange" : "orange juice", + "red" : "red wine", + "wine" : "red wine", + "tropical" : "tropical juice", + # "juice": ["orange juice", "tropical juice", "juice pack"], + } +available_drinks = list(set(available_single_drinks).union(set(available_double_drinks))) +excluded_words = ["my", "name", "is", "and", "favourite","drink", "you", "can", "call", "me"] + + +def speech_recovery(sentence): + sentence_split = sentence.split() + sentence_list = list(set(sentence_split) - set(excluded_words)) + print(f"final name: {handle_name(sentence_list, True)}") + print(f"final drink: {handle_drink(sentence_list, True)}") + + +def handle_name(sentence_list, last_resort): + result = handle_similar_spelt(sentence_list, available_names, 1) + if result != "unknown": + print(f"name (spelt): {result}") + return result + else: + result = handle_similar_sound(sentence_list, available_names, 0) + print(f"name (sound): {result}") + if not last_resort or result != "unknown": + return result + else: + print("Last resort name") + return handle_closest_spelt(sentence_list, available_names) + + +def handle_drink(sentence_list, last_resort): + result = infer_second_drink(sentence_list) + if result != "unknown": + return result + result = handle_similar_spelt(sentence_list, available_drinks, 1) + if result != "unknown": + print(f"drink (spelt): {result}") + else: + result = handle_similar_sound(sentence_list, available_drinks, 0) + print(f"drink (sound): {result}") + + if result != "unknown": + if result in available_single_drinks: + print(f"final attempt drink: {result}") + return result + else: + sentence_list.append(result) + return infer_second_drink(sentence_list) + else: + if not last_resort: + return "unknown" + else: + print("Last resort drink") + closest_spelt = handle_closest_spelt(sentence_list, available_drinks) + if closest_spelt in available_single_drinks: + print(f"final attempt during last resort drink: {closest_spelt}") + return closest_spelt + else: + sentence_list.append(closest_spelt) + return infer_second_drink(closest_spelt) + + +def handle_similar_spelt(sentence_list, available_words, distance_threshold): + for input_word in sentence_list: + for available_word in available_words: + distance = get_damerau_levenshtein_distance(input_word, available_word) + if distance <= distance_threshold: + return available_word + return "unknown" + + +def handle_similar_sound(sentence_list, available_words, distance_threshold): + for input_word in sentence_list: + for available_word in available_words: + distance = get_levenshtein_soundex_distance(input_word, available_word) + if distance <= distance_threshold: + print(input_word, available_word) + return available_word + return "unknown" + +def infer_second_drink(sentence_list): + for input_word in sentence_list: + if input_word == "juice": + choices = ["pack", "orange", "tropical"] + closest_word = handle_closest_spelt(sentence_list, choices) + if closest_word == "pack": + return "juice pack" + elif closest_word == "orange": + return "orange juice" + else: + return "tropical juice" + for available_word in available_double_drinks: + if input_word == available_word: + return double_drinks_dict[input_word] + return "unknown" + +def handle_closest_spelt(sentence_list, choices): + closest_distance = float('inf') + closest_word = None + for input_word in sentence_list: + for available_word in choices: + distance = get_damerau_levenshtein_distance(input_word, available_word) + if distance < closest_distance: + closest_distance = distance + closest_word = available_word + return closest_word + + +def get_damerau_levenshtein_distance(word_1, word_2): + return jf.damerau_levenshtein_distance(word_1, word_2) + +def get_levenshtein_soundex_distance(word_1, word_2): + soundex_word_1 = jf.soundex(word_1) + soundex_word_2 = jf.soundex(word_2) + return jf.levenshtein_distance(soundex_word_1, soundex_word_2) + +# print(get_damerau_levenshtein_distance("juice", "shoes")) +# print(get_levenshtein_soundex_distance("juice", "shoes")) + + +# print(jf.levenshtein_distance(jf.metaphone("my"), jf.metaphone("tea"))) + +# print(jf.soundex("juice"), jf.soundex("shoes")) + +# print(jf.levenshtein_distance(jf.soundex("jane"), jf.soundex("axasel"))) + +# available_names = ["adel", "angel", "axel", "charlie", "jane", "jules", "morgan", "paris", "robin", "simone"] +# available_single_drinks = ["cola", "milk",] +# available_double_drinks = ["iced", "tea", "juice", "pack", "orange", "red", "wine", "tropical"] +# double_drinks_dict = {"iced" : "iced tea", +# "tea": "iced tea", +# "pack" : "juice pack", +# "orange" : "orange juice", +# "red" : "red wine", +# "wine" : "red wine", +# "tropical" : "tropical juice", +# "juice": ["orange juice", "tropical juice", "juice pack"], +# } + +# available_names_and_drinks = list(set(available_names).union(set(available_single_drinks)).union(set(available_double_drinks))) + +# sentence = "my name is axl and my favourite drink is orange shoes" + +# dataList = set(sentence.split()).union(set(available_names_and_drinks)) + + +if __name__ == "__main__": + sentence = "my name is jay and my favourite drink is mill" + speech_recovery(sentence) + print("======") + sentence = "my name is jayne and my favourite drink is oras juice" + speech_recovery(sentence) + print("======") + sentence = "my name is axl and my favourite drink is tropical ef" + speech_recovery(sentence) + print("======") + sentence = "my name is axl and my favourite drink is p jews" + speech_recovery(sentence) + print("======") + sentence = "my name is axasel and my favourite drink is orange juice juice" + speech_recovery(sentence) + print("======") + sentence = "my name is morgen and my favourite drink is mll" + speech_recovery(sentence) + print("======") \ No newline at end of file From 25dc1d6aa09e1423ae62e51dd89194aa369eabbd Mon Sep 17 00:00:00 2001 From: Haiwei L Date: Fri, 5 Jul 2024 03:48:30 +0100 Subject: [PATCH 10/17] Revert lab yaml changes --- tasks/receptionist/config/lab.yaml | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/tasks/receptionist/config/lab.yaml b/tasks/receptionist/config/lab.yaml index 75f68a4e0..691ea63f3 100644 --- a/tasks/receptionist/config/lab.yaml +++ b/tasks/receptionist/config/lab.yaml @@ -31,19 +31,20 @@ priors: wait_pose: position: - x: 4.637585370589175 - y: 6.715591164127531 + x: 8.245934441303595 + y: 24.285935396834816 z: 0.0 orientation: x: 0.0 y: 0.0 - z: 0.7827525387329473 - w: 0.6223330805180823 + z: 0.08719394681831685 + w: 0.9961913549304895 + #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]] +wait_area: [[9.16, 25.3], [10.6, 25.7], [11, 24.1], [9.4, 23.7]] seat_pose: position: x: 7.352258327332648 From 6e989b8ec7466553ae1935ea4ef484d291de98bc Mon Sep 17 00:00:00 2001 From: Haiwei L Date: Fri, 5 Jul 2024 14:49:28 +0100 Subject: [PATCH 11/17] Setup jellyfish dependency --- skills/package.xml | 2 +- tasks/receptionist/CMakeLists.txt | 5 +++++ tasks/receptionist/package.xml | 4 +++- tasks/receptionist/requirements.in | 1 + tasks/receptionist/requirements.txt | 1 + tasks/receptionist/scripts/main.py | 0 tasks/receptionist/scripts/test_find_and_look_at.py | 0 .../src/receptionist/states/local_speech_recognition.py | 3 --- 8 files changed, 11 insertions(+), 5 deletions(-) create mode 100644 tasks/receptionist/requirements.in create mode 100644 tasks/receptionist/requirements.txt mode change 100755 => 100644 tasks/receptionist/scripts/main.py mode change 100755 => 100644 tasks/receptionist/scripts/test_find_and_look_at.py diff --git a/skills/package.xml b/skills/package.xml index 053a2c85b..834ad7fc2 100644 --- a/skills/package.xml +++ b/skills/package.xml @@ -58,6 +58,6 @@ - + diff --git a/tasks/receptionist/CMakeLists.txt b/tasks/receptionist/CMakeLists.txt index 4a53b4d0e..e99a3b839 100644 --- a/tasks/receptionist/CMakeLists.txt +++ b/tasks/receptionist/CMakeLists.txt @@ -8,6 +8,7 @@ project(receptionist) ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS + catkin_virtualenv geometry_msgs message_generation roscpp @@ -23,6 +24,10 @@ find_package(catkin REQUIRED COMPONENTS ## modules and global scripts declared therein get installed ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html catkin_python_setup() +catkin_generate_virtualenv( + INPUT_REQUIREMENTS requirements.in + PYTHON_INTERPRETER python3.8 +) ################################################ ## Declare ROS messages, services and actions ## diff --git a/tasks/receptionist/package.xml b/tasks/receptionist/package.xml index 6e9881b92..4703b40f2 100644 --- a/tasks/receptionist/package.xml +++ b/tasks/receptionist/package.xml @@ -62,11 +62,13 @@ roscpp rospy std_msgs + catkin_virtualenv - + requirements.txt + diff --git a/tasks/receptionist/requirements.in b/tasks/receptionist/requirements.in new file mode 100644 index 000000000..596ca6153 --- /dev/null +++ b/tasks/receptionist/requirements.in @@ -0,0 +1 @@ +jellyfish==1.0.4 \ No newline at end of file diff --git a/tasks/receptionist/requirements.txt b/tasks/receptionist/requirements.txt new file mode 100644 index 000000000..b13c4dced --- /dev/null +++ b/tasks/receptionist/requirements.txt @@ -0,0 +1 @@ +jellyfish==1.0.4 # via -r requirements.in diff --git a/tasks/receptionist/scripts/main.py b/tasks/receptionist/scripts/main.py old mode 100755 new mode 100644 diff --git a/tasks/receptionist/scripts/test_find_and_look_at.py b/tasks/receptionist/scripts/test_find_and_look_at.py old mode 100755 new mode 100644 diff --git a/tasks/receptionist/src/receptionist/states/local_speech_recognition.py b/tasks/receptionist/src/receptionist/states/local_speech_recognition.py index 10bac5a5a..565111d07 100644 --- a/tasks/receptionist/src/receptionist/states/local_speech_recognition.py +++ b/tasks/receptionist/src/receptionist/states/local_speech_recognition.py @@ -1,8 +1,5 @@ import jellyfish as jf -from itertools import groupby -import pandas as pd -import numpy as np available_names = ["adel", "angel", "axel", "charlie", "jane", "jules", "morgan", "paris", "robin", "simone"] From d5a8f39e6ca3c84aa652b3db1427116cf9fdc47a Mon Sep 17 00:00:00 2001 From: Haiwei L Date: Fri, 5 Jul 2024 15:16:48 +0100 Subject: [PATCH 12/17] Incorporate speech recovery as separate state --- .../receptionist/states/speech_recovery.py | 168 ++++++++++++++++++ 1 file changed, 168 insertions(+) create mode 100644 tasks/receptionist/src/receptionist/states/speech_recovery.py diff --git a/tasks/receptionist/src/receptionist/states/speech_recovery.py b/tasks/receptionist/src/receptionist/states/speech_recovery.py new file mode 100644 index 000000000..775d7f75c --- /dev/null +++ b/tasks/receptionist/src/receptionist/states/speech_recovery.py @@ -0,0 +1,168 @@ +import rospy +import smach +import jellyfish as jf +from smach import UserData +from typing import List, Dict, Any + +class SpeechRecovery(smach.State): + def __init__( + self, + guest_id: int, + last_resort: bool, + input_type: str = "", + ): + smach.State.__init__( + self, + outcomes=["succeeded", "failed"], + input_keys=["guest_transcription", "guest_data"], + output_keys=["guest_data", "guest_transcription"], + ) + + self._guest_id = guest_id + self._last_resort = last_resort + self._input_type = input_type + self._available_names = ["adel", "angel", "axel", "charlie", "jane", "jules", "morgan", "paris", "robin", "simone"] + self._available_single_drinks = ["cola", "milk",] + self._available_double_drinks = ["iced", "tea", "pack", "juice", "orange", "red", "wine", "tropical"] + self._double_drinks_dict = {"iced" : "iced tea", + "tea": "iced tea", + "pack" : "juice pack", + "orange" : "orange juice", + "red" : "red wine", + "wine" : "red wine", + "tropical" : "tropical juice", + # "juice": ["orange juice", "tropical juice", "juice pack"], + } + self._available_drinks = list(set(self._available_single_drinks).union(set(self._available_double_drinks))) + self._excluded_words = ["my", "name", "is", "and", "favourite","drink", "you", "can", "call", "me"] + + def execute(self, userdata: UserData) -> str: + sentence_split = userdata.guest_transcription.lower() + sentence_list = list(set(sentence_split) - set(self._excluded_words)) + if self._input_type == "name": + final_name = self._handle_name(sentence_list, self._last_resort) + if final_name != "unknown": + userdata.guest_data[self._guest_id]["name"] = final_name + return "succeeded" + else: + return "failed" + elif self._input_type == "drink": + final_drink = self._handle_drink(sentence_list, self._last_resort) + if final_drink != "unknown": + userdata.guest_data[self._guest_id]["drink"] = final_drink + return "succeeded" + else: + return "failed" + else: + if userdata.guest_data[self._guest_id]["name"] == "unknown": + final_name = self._handle_name(sentence_list, self._last_resort) + userdata.guest_data[self._guest_id]["name"] = final_name + if userdata.guest_data[self._guest_id]["drink"] == "unknown": + final_drink = self._handle_drink(sentence_list, self._last_resort) + userdata.guest_data[self._guest_id]["drink"] = final_drink + if final_name == "unknown" or final_drink == "unknown": + return "failed" + else: + return "succeeded" + + + def _handle_name(self, sentence_list, last_resort): + result = self._handle_similar_spelt(sentence_list, self._available_names, 1) + if result != "unknown": + print(f"name (spelt): {result}") + return result + else: + result = self._handle_similar_sound(sentence_list, self._available_names, 0) + print(f"name (sound): {result}") + if not last_resort or result != "unknown": + return result + else: + print("Last resort name") + return self._handle_closest_spelt(sentence_list, self._available_names) + + + def _handle_drink(self, sentence_list, last_resort): + result = self._infer_second_drink(sentence_list) + if result != "unknown": + return result + result = self._handle_similar_spelt(sentence_list, self._available_drinks, 1) + if result != "unknown": + print(f"drink (spelt): {result}") + else: + result = self._handle_similar_sound(sentence_list, self._available_drinks, 0) + print(f"drink (sound): {result}") + + if result != "unknown": + if result in self._available_single_drinks: + print(f"final attempt drink: {result}") + return result + else: + sentence_list.append(result) + return self._infer_second_drink(sentence_list) + else: + if not last_resort: + return "unknown" + else: + print("Last resort drink") + closest_spelt = self._handle_closest_spelt(sentence_list, self._available_drinks) + if closest_spelt in self._available_single_drinks: + print(f"final attempt during last resort drink: {closest_spelt}") + return closest_spelt + else: + sentence_list.append(closest_spelt) + return self._infer_second_drink(closest_spelt) + + + def _handle_similar_spelt(self, sentence_list, available_words, distance_threshold): + for input_word in sentence_list: + for available_word in available_words: + distance = self._get_damerau_levenshtein_distance(input_word, available_word) + if distance <= distance_threshold: + return available_word + return "unknown" + + + def _handle_similar_sound(self, sentence_list, available_words, distance_threshold): + for input_word in sentence_list: + for available_word in available_words: + distance = self._get_levenshtein_soundex_distance(input_word, available_word) + if distance <= distance_threshold: + print(input_word, available_word) + return available_word + return "unknown" + + def _infer_second_drink(self, sentence_list): + for input_word in sentence_list: + if input_word == "juice": + choices = ["pack", "orange", "tropical"] + closest_word = self._handle_closest_spelt(sentence_list, choices) + if closest_word == "pack": + return "juice pack" + elif closest_word == "orange": + return "orange juice" + else: + return "tropical juice" + for available_word in self._available_double_drinks: + if input_word == available_word: + return self._double_drinks_dict[input_word] + return "unknown" + + def _handle_closest_spelt(self, sentence_list, choices): + closest_distance = float('inf') + closest_word = None + for input_word in sentence_list: + for available_word in choices: + distance = self._get_damerau_levenshtein_distance(input_word, available_word) + if distance < closest_distance: + closest_distance = distance + closest_word = available_word + return closest_word + + + def _get_damerau_levenshtein_distance(self, word_1, word_2): + return jf.damerau_levenshtein_distance(word_1, word_2) + + def _get_levenshtein_soundex_distance(self, word_1, word_2): + soundex_word_1 = jf.soundex(word_1) + soundex_word_2 = jf.soundex(word_2) + return jf.levenshtein_distance(soundex_word_1, soundex_word_2) \ No newline at end of file From 0868f59f58dd7fb89a24c79f202f07fe3b85532b Mon Sep 17 00:00:00 2001 From: Haiwei L Date: Fri, 5 Jul 2024 15:20:32 +0100 Subject: [PATCH 13/17] Minor cleanup --- .../states/local_speech_recognition.py | 177 ------------------ .../receptionist/states/speech_recovery.py | 4 + 2 files changed, 4 insertions(+), 177 deletions(-) delete mode 100644 tasks/receptionist/src/receptionist/states/local_speech_recognition.py diff --git a/tasks/receptionist/src/receptionist/states/local_speech_recognition.py b/tasks/receptionist/src/receptionist/states/local_speech_recognition.py deleted file mode 100644 index 565111d07..000000000 --- a/tasks/receptionist/src/receptionist/states/local_speech_recognition.py +++ /dev/null @@ -1,177 +0,0 @@ -import jellyfish as jf - - - -available_names = ["adel", "angel", "axel", "charlie", "jane", "jules", "morgan", "paris", "robin", "simone"] -available_single_drinks = ["cola", "milk",] -available_double_drinks = ["iced", "tea", "pack", "juice", "orange", "red", "wine", "tropical"] -double_drinks_dict = {"iced" : "iced tea", - "tea": "iced tea", - "pack" : "juice pack", - "orange" : "orange juice", - "red" : "red wine", - "wine" : "red wine", - "tropical" : "tropical juice", - # "juice": ["orange juice", "tropical juice", "juice pack"], - } -available_drinks = list(set(available_single_drinks).union(set(available_double_drinks))) -excluded_words = ["my", "name", "is", "and", "favourite","drink", "you", "can", "call", "me"] - - -def speech_recovery(sentence): - sentence_split = sentence.split() - sentence_list = list(set(sentence_split) - set(excluded_words)) - print(f"final name: {handle_name(sentence_list, True)}") - print(f"final drink: {handle_drink(sentence_list, True)}") - - -def handle_name(sentence_list, last_resort): - result = handle_similar_spelt(sentence_list, available_names, 1) - if result != "unknown": - print(f"name (spelt): {result}") - return result - else: - result = handle_similar_sound(sentence_list, available_names, 0) - print(f"name (sound): {result}") - if not last_resort or result != "unknown": - return result - else: - print("Last resort name") - return handle_closest_spelt(sentence_list, available_names) - - -def handle_drink(sentence_list, last_resort): - result = infer_second_drink(sentence_list) - if result != "unknown": - return result - result = handle_similar_spelt(sentence_list, available_drinks, 1) - if result != "unknown": - print(f"drink (spelt): {result}") - else: - result = handle_similar_sound(sentence_list, available_drinks, 0) - print(f"drink (sound): {result}") - - if result != "unknown": - if result in available_single_drinks: - print(f"final attempt drink: {result}") - return result - else: - sentence_list.append(result) - return infer_second_drink(sentence_list) - else: - if not last_resort: - return "unknown" - else: - print("Last resort drink") - closest_spelt = handle_closest_spelt(sentence_list, available_drinks) - if closest_spelt in available_single_drinks: - print(f"final attempt during last resort drink: {closest_spelt}") - return closest_spelt - else: - sentence_list.append(closest_spelt) - return infer_second_drink(closest_spelt) - - -def handle_similar_spelt(sentence_list, available_words, distance_threshold): - for input_word in sentence_list: - for available_word in available_words: - distance = get_damerau_levenshtein_distance(input_word, available_word) - if distance <= distance_threshold: - return available_word - return "unknown" - - -def handle_similar_sound(sentence_list, available_words, distance_threshold): - for input_word in sentence_list: - for available_word in available_words: - distance = get_levenshtein_soundex_distance(input_word, available_word) - if distance <= distance_threshold: - print(input_word, available_word) - return available_word - return "unknown" - -def infer_second_drink(sentence_list): - for input_word in sentence_list: - if input_word == "juice": - choices = ["pack", "orange", "tropical"] - closest_word = handle_closest_spelt(sentence_list, choices) - if closest_word == "pack": - return "juice pack" - elif closest_word == "orange": - return "orange juice" - else: - return "tropical juice" - for available_word in available_double_drinks: - if input_word == available_word: - return double_drinks_dict[input_word] - return "unknown" - -def handle_closest_spelt(sentence_list, choices): - closest_distance = float('inf') - closest_word = None - for input_word in sentence_list: - for available_word in choices: - distance = get_damerau_levenshtein_distance(input_word, available_word) - if distance < closest_distance: - closest_distance = distance - closest_word = available_word - return closest_word - - -def get_damerau_levenshtein_distance(word_1, word_2): - return jf.damerau_levenshtein_distance(word_1, word_2) - -def get_levenshtein_soundex_distance(word_1, word_2): - soundex_word_1 = jf.soundex(word_1) - soundex_word_2 = jf.soundex(word_2) - return jf.levenshtein_distance(soundex_word_1, soundex_word_2) - -# print(get_damerau_levenshtein_distance("juice", "shoes")) -# print(get_levenshtein_soundex_distance("juice", "shoes")) - - -# print(jf.levenshtein_distance(jf.metaphone("my"), jf.metaphone("tea"))) - -# print(jf.soundex("juice"), jf.soundex("shoes")) - -# print(jf.levenshtein_distance(jf.soundex("jane"), jf.soundex("axasel"))) - -# available_names = ["adel", "angel", "axel", "charlie", "jane", "jules", "morgan", "paris", "robin", "simone"] -# available_single_drinks = ["cola", "milk",] -# available_double_drinks = ["iced", "tea", "juice", "pack", "orange", "red", "wine", "tropical"] -# double_drinks_dict = {"iced" : "iced tea", -# "tea": "iced tea", -# "pack" : "juice pack", -# "orange" : "orange juice", -# "red" : "red wine", -# "wine" : "red wine", -# "tropical" : "tropical juice", -# "juice": ["orange juice", "tropical juice", "juice pack"], -# } - -# available_names_and_drinks = list(set(available_names).union(set(available_single_drinks)).union(set(available_double_drinks))) - -# sentence = "my name is axl and my favourite drink is orange shoes" - -# dataList = set(sentence.split()).union(set(available_names_and_drinks)) - - -if __name__ == "__main__": - sentence = "my name is jay and my favourite drink is mill" - speech_recovery(sentence) - print("======") - sentence = "my name is jayne and my favourite drink is oras juice" - speech_recovery(sentence) - print("======") - sentence = "my name is axl and my favourite drink is tropical ef" - speech_recovery(sentence) - print("======") - sentence = "my name is axl and my favourite drink is p jews" - speech_recovery(sentence) - print("======") - sentence = "my name is axasel and my favourite drink is orange juice juice" - speech_recovery(sentence) - print("======") - sentence = "my name is morgen and my favourite drink is mll" - speech_recovery(sentence) - print("======") \ No newline at end of file diff --git a/tasks/receptionist/src/receptionist/states/speech_recovery.py b/tasks/receptionist/src/receptionist/states/speech_recovery.py index 775d7f75c..ef402c57d 100644 --- a/tasks/receptionist/src/receptionist/states/speech_recovery.py +++ b/tasks/receptionist/src/receptionist/states/speech_recovery.py @@ -43,6 +43,7 @@ def execute(self, userdata: UserData) -> str: final_name = self._handle_name(sentence_list, self._last_resort) if final_name != "unknown": userdata.guest_data[self._guest_id]["name"] = final_name + print(f"Recovered name: {final_name} ") return "succeeded" else: return "failed" @@ -50,6 +51,7 @@ def execute(self, userdata: UserData) -> str: final_drink = self._handle_drink(sentence_list, self._last_resort) if final_drink != "unknown": userdata.guest_data[self._guest_id]["drink"] = final_drink + print(f"Recovered drink: {final_drink} ") return "succeeded" else: return "failed" @@ -57,9 +59,11 @@ def execute(self, userdata: UserData) -> str: if userdata.guest_data[self._guest_id]["name"] == "unknown": final_name = self._handle_name(sentence_list, self._last_resort) userdata.guest_data[self._guest_id]["name"] = final_name + print(f"Recovered name: {final_name} ") if userdata.guest_data[self._guest_id]["drink"] == "unknown": final_drink = self._handle_drink(sentence_list, self._last_resort) userdata.guest_data[self._guest_id]["drink"] = final_drink + print(f"Recovered drink: {final_drink} ") if final_name == "unknown" or final_drink == "unknown": return "failed" else: From 4c357d0dc850838c9e332db6c6ee6bc97af03609 Mon Sep 17 00:00:00 2001 From: Haiwei L Date: Fri, 5 Jul 2024 16:13:16 +0100 Subject: [PATCH 14/17] Incorporate speech recovery into main state machine --- .../src/receptionist/states/__init__.py | 1 + .../receptionist/states/get_name_and_drink.py | 10 ++++++++++ .../receptionist/states/get_name_or_drink.py | 19 ++++++++++--------- .../src/receptionist/states/handle_guest.py | 8 ++++---- 4 files changed, 25 insertions(+), 13 deletions(-) diff --git a/tasks/receptionist/src/receptionist/states/__init__.py b/tasks/receptionist/src/receptionist/states/__init__.py index a21ff63cf..5a8297de3 100644 --- a/tasks/receptionist/src/receptionist/states/__init__.py +++ b/tasks/receptionist/src/receptionist/states/__init__.py @@ -12,3 +12,4 @@ from .seat_guest import SeatGuest from .check_sofa import CheckSofa from .introduce_and_seat_guest import IntroduceAndSeatGuest +from .speech_recovery import SpeechRecovery diff --git a/tasks/receptionist/src/receptionist/states/get_name_and_drink.py b/tasks/receptionist/src/receptionist/states/get_name_and_drink.py index d4f8b2e57..11171f088 100644 --- a/tasks/receptionist/src/receptionist/states/get_name_and_drink.py +++ b/tasks/receptionist/src/receptionist/states/get_name_and_drink.py @@ -7,6 +7,9 @@ import smach from smach import UserData from typing import List, Dict, Any +from receptionist.states import ( + SpeechRecovery +) class GetNameAndDrink(smach.StateMachine): class ParseNameAndDrink(smach.State): @@ -119,11 +122,13 @@ def _recovery_name_and_drink_required(self, userdata: UserData) -> bool: def __init__( self, guest_id: str, + last_resort: bool, param_key: str = "/receptionist/priors", ): self._guest_id = guest_id self._param_key = param_key + self._last_resort = last_resort smach.StateMachine.__init__( self, @@ -137,6 +142,11 @@ def __init__( smach.StateMachine.add( "PARSE_NAME_AND_DRINK", self.ParseNameAndDrink(guest_id=self._guest_id, param_key=self._param_key), + transitions={"succeeded": "succeeded", "failed": "SPEECH_RECOVERY"}, + ) + smach.StateMachine.add( + "SPEECH_RECOVERY", + SpeechRecovery(self._guest_id, self._last_resort), transitions={"succeeded": "succeeded", "failed": "POST_RECOVERY_DECISION"}, ) smach.StateMachine.add( diff --git a/tasks/receptionist/src/receptionist/states/get_name_or_drink.py b/tasks/receptionist/src/receptionist/states/get_name_or_drink.py index 9db23ff96..62aabdc3a 100644 --- a/tasks/receptionist/src/receptionist/states/get_name_or_drink.py +++ b/tasks/receptionist/src/receptionist/states/get_name_or_drink.py @@ -7,6 +7,9 @@ import smach from smach import UserData from typing import List, Dict, Any +from receptionist.states import ( + SpeechRecovery +) class GetNameOrDrink(smach.StateMachine): class ParseTranscribedInfo(smach.State): @@ -75,10 +78,12 @@ def execute(self, userdata: UserData) -> str: def __init__( self, guest_id: str, + last_resort: bool, info_type: str, param_key: str = "/receptionist/priors", ): + self._last_resort = last_resort self._guest_id = guest_id self._info_type = info_type self._param_key = param_key @@ -95,14 +100,10 @@ def __init__( smach.StateMachine.add( "PARSE_NAME_OR_DRINK", self.ParseTranscribedInfo(guest_id=self._guest_id, info_type=self._info_type, param_key=self._param_key), + transitions={"succeeded": "succeeded", "failed": "SPEECH_RECOVERY"}, + ) + smach.StateMachine.add( + "SPEECH_RECOVERY", + SpeechRecovery(self._guest_id, self._last_resort, self._info_type), transitions={"succeeded": "succeeded", "failed": "failed"}, ) - # smach.StateMachine.add( - # "POST_RECOVERY_DECISION", - # self.PostRecoveryDecision(guest_id=self._guest_id, param_key=self._param_key), - # transitions={ - # "failed": "failed", - # "failed_name": "failed_name", - # "failed_drink": "failed_drink", - # } - # ) diff --git a/tasks/receptionist/src/receptionist/states/handle_guest.py b/tasks/receptionist/src/receptionist/states/handle_guest.py index 71a2dafbc..8f9c39947 100644 --- a/tasks/receptionist/src/receptionist/states/handle_guest.py +++ b/tasks/receptionist/src/receptionist/states/handle_guest.py @@ -31,7 +31,7 @@ def __init__(self, guest_id: str): smach.StateMachine.add( f"PARSE_NAME_AND_DRINK_GUEST_{guest_id}", - GetNameAndDrink(guest_id), + GetNameAndDrink(guest_id, False), transitions={ "succeeded": "succeeded", "failed": f"REPEAT_GET_NAME_AND_DRINK_GUEST_{guest_id}", @@ -54,7 +54,7 @@ def __init__(self, guest_id: str): smach.StateMachine.add( f"REPEAT_PARSE_NAME_AND_DRINK_GUEST_{guest_id}", - GetNameAndDrink(guest_id), + GetNameAndDrink(guest_id, True), transitions={ "succeeded": "succeeded", "failed": "succeeded", @@ -79,7 +79,7 @@ def __init__(self, guest_id: str): smach.StateMachine.add( f"REPEAT_PARSE_NAME_GUEST_{guest_id}", - GetNameOrDrink(guest_id, "name"), + GetNameOrDrink(guest_id, True, "name"), transitions={ "succeeded": "succeeded", "failed": "succeeded", @@ -104,7 +104,7 @@ def __init__(self, guest_id: str): smach.StateMachine.add( f"REPEAT_PARSE_DRINK_GUEST_{guest_id}", - GetNameOrDrink(guest_id, "drink"), + GetNameOrDrink(guest_id, True, "drink"), transitions={ "succeeded": "succeeded", "failed": "succeeded", From 0c1621e95324cf4180e04c4c32e7c7f2fdad997a Mon Sep 17 00:00:00 2001 From: Haiwei L Date: Fri, 5 Jul 2024 18:45:00 +0100 Subject: [PATCH 15/17] Fix empty sentence input --- skills/src/lasr_skills/ask_and_listen.py | 18 +- .../src/receptionist/state_machine.py | 413 +++++++++--------- .../src/receptionist/states/__init__.py | 2 +- .../receptionist/states/get_name_or_drink.py | 3 +- .../states/local_speech_recognition.py | 177 ++++++++ .../receptionist/states/speech_recovery.py | 9 +- 6 files changed, 406 insertions(+), 216 deletions(-) create mode 100644 tasks/receptionist/src/receptionist/states/local_speech_recognition.py diff --git a/skills/src/lasr_skills/ask_and_listen.py b/skills/src/lasr_skills/ask_and_listen.py index 49221283f..5dc380c5b 100644 --- a/skills/src/lasr_skills/ask_and_listen.py +++ b/skills/src/lasr_skills/ask_and_listen.py @@ -18,15 +18,15 @@ def __init__( output_keys=["transcribed_speech"], ) with self: - smach.StateMachine.add( - "SAY", - Say(text=tts_phrase), - transitions={ - "succeeded": "LISTEN", - "aborted": "failed", - "preempted": "failed", - }, - ) + # smach.StateMachine.add( + # "SAY", + # Say(text=tts_phrase), + # transitions={ + # "succeeded": "LISTEN", + # "aborted": "failed", + # "preempted": "failed", + # }, + # ) smach.StateMachine.add( "LISTEN", Listen(), diff --git a/tasks/receptionist/src/receptionist/state_machine.py b/tasks/receptionist/src/receptionist/state_machine.py index 78ed090ba..823647d68 100755 --- a/tasks/receptionist/src/receptionist/state_machine.py +++ b/tasks/receptionist/src/receptionist/state_machine.py @@ -62,221 +62,230 @@ def __init__( self.userdata.dataset = "receptionist" self.userdata.seat_position = PointStamped() - smach.StateMachine.add( - "SAY_START", - Say(text="Start of receptionist task. Going to waiting area."), - transitions={ - "succeeded": "GO_TO_WAIT_LOCATION_GUEST_1", - "aborted": "GO_TO_WAIT_LOCATION_GUEST_1", - "preempted": "GO_TO_WAIT_LOCATION_GUEST_1", - }, - ) - - """ - First guest - """ - - self._goto_waiting_area(guest_id=1) - - smach.StateMachine.add( - "INTRODUCE_ROBOT", - Say( - text="Hello my name is Tiago, nice to meet you, I shall be your receptionist for today. I will try and be polite by looking at you when I speak, so I hope you will do the same by looking into my eyes whenever possible. First let me get to know you a little bit better." - ), - transitions={ - "succeeded": f"HANDLE_GUEST_1", - "aborted": f"HANDLE_GUEST_1", - "preempted": f"HANDLE_GUEST_1", - }, - ) + # smach.StateMachine.add( + # "SAY_START", + # Say(text="Start of receptionist task. Going to waiting area."), + # transitions={ + # "succeeded": "GO_TO_WAIT_LOCATION_GUEST_1", + # "aborted": "GO_TO_WAIT_LOCATION_GUEST_1", + # "preempted": "GO_TO_WAIT_LOCATION_GUEST_1", + # }, + # ) # """ - # GET GUEST ATTRIBUTES + # First guest # """ - smach.StateMachine.add( - "HANDLE_GUEST_1", - HandleGuest("guest1", learn_guest_1), - transitions={ - "succeeded": "SAY_FOLLOW_GUEST_1", - "failed": "SAY_FOLLOW_GUEST_1", - }, - ) - - self._guide_guest(guest_id=1) + # self._goto_waiting_area(guest_id=1) - smach.StateMachine.add( - "INTRODUCE_AND_SEAT_GUEST_1", - IntroduceAndSeatGuest( - "guest1", - ["host"], - seat_area, - sofa_area, - sofa_point, - max_people_on_sofa, - ), - transitions={ - "succeeded": "SAY_RETURN_WAITING_AREA", - "failed": "SAY_RETURN_WAITING_AREA", - }, - ) - - smach.StateMachine.add( - "SAY_RETURN_WAITING_AREA", - Say(text="Let me go back to the waiting area."), - transitions={ - "succeeded": "GO_TO_WAIT_LOCATION_GUEST_2", - "aborted": "GO_TO_WAIT_LOCATION_GUEST_2", - "preempted": "GO_TO_WAIT_LOCATION_GUEST_2", - }, - ) + # smach.StateMachine.add( + # "INTRODUCE_ROBOT", + # Say( + # text="Hello my name is Tiago, nice to meet you, I shall be your receptionist for today. I will try and be polite by looking at you when I speak, so I hope you will do the same by looking into my eyes whenever possible. First let me get to know you a little bit better." + # ), + # transitions={ + # "succeeded": f"HANDLE_GUEST_1", + # "aborted": f"HANDLE_GUEST_1", + # "preempted": f"HANDLE_GUEST_1", + # }, + # ) # """ - # Guest 2 + # GET GUEST ATTRIBUTES # """ - self._goto_waiting_area(2) + # smach.StateMachine.add( + # "HANDLE_GUEST_1", + # HandleGuest("guest1", learn_guest_1), + # transitions={ + # "succeeded": "SAY_FOLLOW_GUEST_1", + # "failed": "SAY_FOLLOW_GUEST_1", + # }, + # ) smach.StateMachine.add( - "HANDLE_GUEST_2", - HandleGuest("guest2", False), - transitions={ - "succeeded": "SAY_FOLLOW_GUEST_2", - "failed": "SAY_FOLLOW_GUEST_2", - }, - ) - - self._guide_guest(guest_id=2) - - smach.StateMachine.add( - "INTRODUCE_AND_SEAT_GUEST_2", - IntroduceAndSeatGuest( - "guest2", - ["host", "guest1"], - seat_area, - sofa_area, - sofa_point, - max_people_on_sofa, - ), - transitions={ - "succeeded": "SAY_GOODBYE", - "failed": "SAY_GOODBYE", - }, - ) - - """ - Finish - """ - smach.StateMachine.add( - "SAY_GOODBYE", - Say( - text="Goodbye fellow humans, I shall be going back where I came from" - ), - transitions={ - "succeeded": "GO_TO_FINISH_LOCATION", - "aborted": "failed", - "preempted": "GO_TO_FINISH_LOCATION", - }, - ) - - smach.StateMachine.add( - "GO_TO_FINISH_LOCATION", - GoToLocation(wait_pose), - transitions={ - "succeeded": "SAY_FINISHED", - "failed": "GO_TO_FINISH_LOCATION", - }, - ) - smach.StateMachine.add( - "SAY_FINISHED", - Say(text="I am done."), + "HANDLE_GUEST_1", + HandleGuest("guest1", learn_guest_1), transitions={ "succeeded": "succeeded", - "aborted": "failed", - "preempted": "succeeded", + "failed": "succeeded", }, ) - def _goto_waiting_area(self, guest_id: int) -> None: - """Adds the states to go to the waiting area. - - Args: - guest_id (int): Identifier for the guest. - """ - - smach.StateMachine.add( - f"GO_TO_WAIT_LOCATION_GUEST_{guest_id}", - GoToLocation(self.wait_pose), - transitions={ - "succeeded": f"SAY_WAITING_GUEST_{guest_id}", - "failed": f"GO_TO_WAIT_LOCATION_GUEST_{guest_id}", - }, - ) - - smach.StateMachine.add( - f"SAY_WAITING_GUEST_{guest_id}", - Say(text="I am waiting for a guest."), - transitions={ - "succeeded": f"WAIT_FOR_PERSON_GUEST_{guest_id}", - "aborted": f"WAIT_FOR_PERSON_GUEST_{guest_id}", - "preempted": f"WAIT_FOR_PERSON_GUEST_{guest_id}", - }, - ) - - smach.StateMachine.add( - f"WAIT_FOR_PERSON_GUEST_{guest_id}", - WaitForPersonInArea(self.wait_area), - transitions={ - "succeeded": f"CHECK_GUEST_ID_GUEST_{guest_id}", - "failed": f"CHECK_GUEST_ID_GUEST_{guest_id}", - }, - ) - - def check_guest_id(ud): - if guest_id == 2: - return "guest_2" - else: - return "guest_1" - - smach.StateMachine.add( - f"CHECK_GUEST_ID_GUEST_{guest_id}", - smach.CBState(check_guest_id, outcomes=["guest_1", "guest_2"]), - transitions={"guest_2": "HANDLE_GUEST_2", "guest_1": "INTRODUCE_ROBOT"}, - ) - - def _guide_guest(self, guest_id: int) -> None: - """Adds the states to guide a guest to the - seating area. - - Args: - guest_id (int): Identifier for the guest. - """ - - smach.StateMachine.add( - f"SAY_FOLLOW_GUEST_{guest_id}", - Say(text="Please follow me, I will guide you to the other guests"), - transitions={ - "succeeded": f"GO_TO_SEAT_LOCATION_GUEST_{guest_id}", - "preempted": "failed", - "aborted": "failed", - }, - ) - - smach.StateMachine.add( - f"GO_TO_SEAT_LOCATION_GUEST_{guest_id}", - GoToLocation(self.seat_pose), - transitions={ - "succeeded": f"SAY_WAIT_GUEST_{guest_id}", - "failed": f"GO_TO_SEAT_LOCATION_GUEST_{guest_id}", - }, - ) - - smach.StateMachine.add( - f"SAY_WAIT_GUEST_{guest_id}", - Say(text="Please wait here on my left."), - transitions={ - "succeeded": f"INTRODUCE_AND_SEAT_GUEST_{guest_id}", - "preempted": "failed", - "aborted": "failed", - }, - ) + # self._guide_guest(guest_id=1) + + # smach.StateMachine.add( + # "INTRODUCE_AND_SEAT_GUEST_1", + # IntroduceAndSeatGuest( + # "guest1", + # ["host"], + # seat_area, + # sofa_area, + # sofa_point, + # max_people_on_sofa, + # ), + # transitions={ + # "succeeded": "SAY_RETURN_WAITING_AREA", + # "failed": "SAY_RETURN_WAITING_AREA", + # }, + # ) + + # smach.StateMachine.add( + # "SAY_RETURN_WAITING_AREA", + # Say(text="Let me go back to the waiting area."), + # transitions={ + # "succeeded": "GO_TO_WAIT_LOCATION_GUEST_2", + # "aborted": "GO_TO_WAIT_LOCATION_GUEST_2", + # "preempted": "GO_TO_WAIT_LOCATION_GUEST_2", + # }, + # ) + + # # """ + # # Guest 2 + # # """ + + # self._goto_waiting_area(2) + + # smach.StateMachine.add( + # "HANDLE_GUEST_2", + # HandleGuest("guest2", False), + # transitions={ + # "succeeded": "SAY_FOLLOW_GUEST_2", + # "failed": "SAY_FOLLOW_GUEST_2", + # }, + # ) + + # self._guide_guest(guest_id=2) + + # smach.StateMachine.add( + # "INTRODUCE_AND_SEAT_GUEST_2", + # IntroduceAndSeatGuest( + # "guest2", + # ["host", "guest1"], + # seat_area, + # sofa_area, + # sofa_point, + # max_people_on_sofa, + # ), + # transitions={ + # "succeeded": "SAY_GOODBYE", + # "failed": "SAY_GOODBYE", + # }, + # ) + + # """ + # Finish + # """ + # smach.StateMachine.add( + # "SAY_GOODBYE", + # Say( + # text="Goodbye fellow humans, I shall be going back where I came from" + # ), + # transitions={ + # "succeeded": "GO_TO_FINISH_LOCATION", + # "aborted": "failed", + # "preempted": "GO_TO_FINISH_LOCATION", + # }, + # ) + + # smach.StateMachine.add( + # "GO_TO_FINISH_LOCATION", + # GoToLocation(wait_pose), + # transitions={ + # "succeeded": "SAY_FINISHED", + # "failed": "GO_TO_FINISH_LOCATION", + # }, + # ) + # smach.StateMachine.add( + # "SAY_FINISHED", + # Say(text="I am done."), + # transitions={ + # "succeeded": "succeeded", + # "aborted": "failed", + # "preempted": "succeeded", + # }, + # ) + + # def _goto_waiting_area(self, guest_id: int) -> None: + # """Adds the states to go to the waiting area. + + # Args: + # guest_id (int): Identifier for the guest. + # """ + + # smach.StateMachine.add( + # f"GO_TO_WAIT_LOCATION_GUEST_{guest_id}", + # GoToLocation(self.wait_pose), + # transitions={ + # "succeeded": f"SAY_WAITING_GUEST_{guest_id}", + # "failed": f"GO_TO_WAIT_LOCATION_GUEST_{guest_id}", + # }, + # ) + + # smach.StateMachine.add( + # f"SAY_WAITING_GUEST_{guest_id}", + # Say(text="I am waiting for a guest."), + # transitions={ + # "succeeded": f"WAIT_FOR_PERSON_GUEST_{guest_id}", + # "aborted": f"WAIT_FOR_PERSON_GUEST_{guest_id}", + # "preempted": f"WAIT_FOR_PERSON_GUEST_{guest_id}", + # }, + # ) + + # smach.StateMachine.add( + # f"WAIT_FOR_PERSON_GUEST_{guest_id}", + # WaitForPersonInArea(self.wait_area), + # transitions={ + # "succeeded": f"CHECK_GUEST_ID_GUEST_{guest_id}", + # "failed": f"CHECK_GUEST_ID_GUEST_{guest_id}", + # }, + # ) + + # def check_guest_id(ud): + # if guest_id == 2: + # return "guest_2" + # else: + # return "guest_1" + + # smach.StateMachine.add( + # f"CHECK_GUEST_ID_GUEST_{guest_id}", + # smach.CBState(check_guest_id, outcomes=["guest_1", "guest_2"]), + # transitions={"guest_2": "HANDLE_GUEST_2", "guest_1": "INTRODUCE_ROBOT"}, + # ) + + # def _guide_guest(self, guest_id: int) -> None: + # """Adds the states to guide a guest to the + # seating area. + + # Args: + # guest_id (int): Identifier for the guest. + # """ + + # smach.StateMachine.add( + # f"SAY_FOLLOW_GUEST_{guest_id}", + # Say(text="Please follow me, I will guide you to the other guests"), + # transitions={ + # "succeeded": f"GO_TO_SEAT_LOCATION_GUEST_{guest_id}", + # "preempted": "failed", + # "aborted": "failed", + # }, + # ) + + # smach.StateMachine.add( + # f"GO_TO_SEAT_LOCATION_GUEST_{guest_id}", + # GoToLocation(self.seat_pose), + # transitions={ + # "succeeded": f"SAY_WAIT_GUEST_{guest_id}", + # "failed": f"GO_TO_SEAT_LOCATION_GUEST_{guest_id}", + # }, + # ) + + # smach.StateMachine.add( + # f"SAY_WAIT_GUEST_{guest_id}", + # Say(text="Please wait here on my left."), + # transitions={ + # "succeeded": f"INTRODUCE_AND_SEAT_GUEST_{guest_id}", + # "preempted": "failed", + # "aborted": "failed", + # }, + # ) diff --git a/tasks/receptionist/src/receptionist/states/__init__.py b/tasks/receptionist/src/receptionist/states/__init__.py index 5a8297de3..75796757d 100644 --- a/tasks/receptionist/src/receptionist/states/__init__.py +++ b/tasks/receptionist/src/receptionist/states/__init__.py @@ -1,3 +1,4 @@ +from .speech_recovery import SpeechRecovery from .get_name_and_drink import GetNameAndDrink from .get_attributes import GetGuestAttributes from .introduce import Introduce @@ -12,4 +13,3 @@ from .seat_guest import SeatGuest from .check_sofa import CheckSofa from .introduce_and_seat_guest import IntroduceAndSeatGuest -from .speech_recovery import SpeechRecovery diff --git a/tasks/receptionist/src/receptionist/states/get_name_or_drink.py b/tasks/receptionist/src/receptionist/states/get_name_or_drink.py index 62aabdc3a..0ca946bbe 100644 --- a/tasks/receptionist/src/receptionist/states/get_name_or_drink.py +++ b/tasks/receptionist/src/receptionist/states/get_name_or_drink.py @@ -62,10 +62,9 @@ def execute(self, userdata: UserData) -> str: transcription = userdata["guest_transcription"].lower() for key_phrase in self._possible_information: - print(self._possible_information) if key_phrase in transcription: userdata.guest_data[self._guest_id][self._type] = key_phrase - rospy.loginfo(f"Guest {self._type} identified as: {key_phrase}") + rospy.loginfo(f"Guest/Drink {self._type} identified as: {key_phrase}") information_found = True break if not information_found: diff --git a/tasks/receptionist/src/receptionist/states/local_speech_recognition.py b/tasks/receptionist/src/receptionist/states/local_speech_recognition.py new file mode 100644 index 000000000..565111d07 --- /dev/null +++ b/tasks/receptionist/src/receptionist/states/local_speech_recognition.py @@ -0,0 +1,177 @@ +import jellyfish as jf + + + +available_names = ["adel", "angel", "axel", "charlie", "jane", "jules", "morgan", "paris", "robin", "simone"] +available_single_drinks = ["cola", "milk",] +available_double_drinks = ["iced", "tea", "pack", "juice", "orange", "red", "wine", "tropical"] +double_drinks_dict = {"iced" : "iced tea", + "tea": "iced tea", + "pack" : "juice pack", + "orange" : "orange juice", + "red" : "red wine", + "wine" : "red wine", + "tropical" : "tropical juice", + # "juice": ["orange juice", "tropical juice", "juice pack"], + } +available_drinks = list(set(available_single_drinks).union(set(available_double_drinks))) +excluded_words = ["my", "name", "is", "and", "favourite","drink", "you", "can", "call", "me"] + + +def speech_recovery(sentence): + sentence_split = sentence.split() + sentence_list = list(set(sentence_split) - set(excluded_words)) + print(f"final name: {handle_name(sentence_list, True)}") + print(f"final drink: {handle_drink(sentence_list, True)}") + + +def handle_name(sentence_list, last_resort): + result = handle_similar_spelt(sentence_list, available_names, 1) + if result != "unknown": + print(f"name (spelt): {result}") + return result + else: + result = handle_similar_sound(sentence_list, available_names, 0) + print(f"name (sound): {result}") + if not last_resort or result != "unknown": + return result + else: + print("Last resort name") + return handle_closest_spelt(sentence_list, available_names) + + +def handle_drink(sentence_list, last_resort): + result = infer_second_drink(sentence_list) + if result != "unknown": + return result + result = handle_similar_spelt(sentence_list, available_drinks, 1) + if result != "unknown": + print(f"drink (spelt): {result}") + else: + result = handle_similar_sound(sentence_list, available_drinks, 0) + print(f"drink (sound): {result}") + + if result != "unknown": + if result in available_single_drinks: + print(f"final attempt drink: {result}") + return result + else: + sentence_list.append(result) + return infer_second_drink(sentence_list) + else: + if not last_resort: + return "unknown" + else: + print("Last resort drink") + closest_spelt = handle_closest_spelt(sentence_list, available_drinks) + if closest_spelt in available_single_drinks: + print(f"final attempt during last resort drink: {closest_spelt}") + return closest_spelt + else: + sentence_list.append(closest_spelt) + return infer_second_drink(closest_spelt) + + +def handle_similar_spelt(sentence_list, available_words, distance_threshold): + for input_word in sentence_list: + for available_word in available_words: + distance = get_damerau_levenshtein_distance(input_word, available_word) + if distance <= distance_threshold: + return available_word + return "unknown" + + +def handle_similar_sound(sentence_list, available_words, distance_threshold): + for input_word in sentence_list: + for available_word in available_words: + distance = get_levenshtein_soundex_distance(input_word, available_word) + if distance <= distance_threshold: + print(input_word, available_word) + return available_word + return "unknown" + +def infer_second_drink(sentence_list): + for input_word in sentence_list: + if input_word == "juice": + choices = ["pack", "orange", "tropical"] + closest_word = handle_closest_spelt(sentence_list, choices) + if closest_word == "pack": + return "juice pack" + elif closest_word == "orange": + return "orange juice" + else: + return "tropical juice" + for available_word in available_double_drinks: + if input_word == available_word: + return double_drinks_dict[input_word] + return "unknown" + +def handle_closest_spelt(sentence_list, choices): + closest_distance = float('inf') + closest_word = None + for input_word in sentence_list: + for available_word in choices: + distance = get_damerau_levenshtein_distance(input_word, available_word) + if distance < closest_distance: + closest_distance = distance + closest_word = available_word + return closest_word + + +def get_damerau_levenshtein_distance(word_1, word_2): + return jf.damerau_levenshtein_distance(word_1, word_2) + +def get_levenshtein_soundex_distance(word_1, word_2): + soundex_word_1 = jf.soundex(word_1) + soundex_word_2 = jf.soundex(word_2) + return jf.levenshtein_distance(soundex_word_1, soundex_word_2) + +# print(get_damerau_levenshtein_distance("juice", "shoes")) +# print(get_levenshtein_soundex_distance("juice", "shoes")) + + +# print(jf.levenshtein_distance(jf.metaphone("my"), jf.metaphone("tea"))) + +# print(jf.soundex("juice"), jf.soundex("shoes")) + +# print(jf.levenshtein_distance(jf.soundex("jane"), jf.soundex("axasel"))) + +# available_names = ["adel", "angel", "axel", "charlie", "jane", "jules", "morgan", "paris", "robin", "simone"] +# available_single_drinks = ["cola", "milk",] +# available_double_drinks = ["iced", "tea", "juice", "pack", "orange", "red", "wine", "tropical"] +# double_drinks_dict = {"iced" : "iced tea", +# "tea": "iced tea", +# "pack" : "juice pack", +# "orange" : "orange juice", +# "red" : "red wine", +# "wine" : "red wine", +# "tropical" : "tropical juice", +# "juice": ["orange juice", "tropical juice", "juice pack"], +# } + +# available_names_and_drinks = list(set(available_names).union(set(available_single_drinks)).union(set(available_double_drinks))) + +# sentence = "my name is axl and my favourite drink is orange shoes" + +# dataList = set(sentence.split()).union(set(available_names_and_drinks)) + + +if __name__ == "__main__": + sentence = "my name is jay and my favourite drink is mill" + speech_recovery(sentence) + print("======") + sentence = "my name is jayne and my favourite drink is oras juice" + speech_recovery(sentence) + print("======") + sentence = "my name is axl and my favourite drink is tropical ef" + speech_recovery(sentence) + print("======") + sentence = "my name is axl and my favourite drink is p jews" + speech_recovery(sentence) + print("======") + sentence = "my name is axasel and my favourite drink is orange juice juice" + speech_recovery(sentence) + print("======") + sentence = "my name is morgen and my favourite drink is mll" + speech_recovery(sentence) + print("======") \ No newline at end of file diff --git a/tasks/receptionist/src/receptionist/states/speech_recovery.py b/tasks/receptionist/src/receptionist/states/speech_recovery.py index ef402c57d..e2113bf8d 100644 --- a/tasks/receptionist/src/receptionist/states/speech_recovery.py +++ b/tasks/receptionist/src/receptionist/states/speech_recovery.py @@ -1,5 +1,6 @@ import rospy import smach +import string import jellyfish as jf from smach import UserData from typing import List, Dict, Any @@ -37,7 +38,11 @@ def __init__( self._excluded_words = ["my", "name", "is", "and", "favourite","drink", "you", "can", "call", "me"] def execute(self, userdata: UserData) -> str: - sentence_split = userdata.guest_transcription.lower() + filtered_sentence = userdata.guest_transcription.lower().translate(str.maketrans('', '', string.punctuation)) + sentence_split = filtered_sentence.split() + if sentence_split is None: + return "failed" + print(sentence_split) sentence_list = list(set(sentence_split) - set(self._excluded_words)) if self._input_type == "name": final_name = self._handle_name(sentence_list, self._last_resort) @@ -64,7 +69,7 @@ def execute(self, userdata: UserData) -> str: final_drink = self._handle_drink(sentence_list, self._last_resort) userdata.guest_data[self._guest_id]["drink"] = final_drink print(f"Recovered drink: {final_drink} ") - if final_name == "unknown" or final_drink == "unknown": + if userdata.guest_data[self._guest_id]["name"] == "unknown" or userdata.guest_data[self._guest_id]["drink"] == "unknown": return "failed" else: return "succeeded" From 378a4754497253a46cd4f6074fb8fafe0f44417c Mon Sep 17 00:00:00 2001 From: Haiwei L Date: Fri, 5 Jul 2024 18:47:15 +0100 Subject: [PATCH 16/17] Add back commented code --- skills/src/lasr_skills/ask_and_listen.py | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/skills/src/lasr_skills/ask_and_listen.py b/skills/src/lasr_skills/ask_and_listen.py index 5dc380c5b..49221283f 100644 --- a/skills/src/lasr_skills/ask_and_listen.py +++ b/skills/src/lasr_skills/ask_and_listen.py @@ -18,15 +18,15 @@ def __init__( output_keys=["transcribed_speech"], ) with self: - # smach.StateMachine.add( - # "SAY", - # Say(text=tts_phrase), - # transitions={ - # "succeeded": "LISTEN", - # "aborted": "failed", - # "preempted": "failed", - # }, - # ) + smach.StateMachine.add( + "SAY", + Say(text=tts_phrase), + transitions={ + "succeeded": "LISTEN", + "aborted": "failed", + "preempted": "failed", + }, + ) smach.StateMachine.add( "LISTEN", Listen(), From 925ab2f9df25d8a4d9a1ea6afc366ade6389d274 Mon Sep 17 00:00:00 2001 From: Haiwei L Date: Fri, 5 Jul 2024 18:50:17 +0100 Subject: [PATCH 17/17] Fix state machine --- .../src/receptionist/state_machine.py | 413 +++++++++--------- 1 file changed, 202 insertions(+), 211 deletions(-) diff --git a/tasks/receptionist/src/receptionist/state_machine.py b/tasks/receptionist/src/receptionist/state_machine.py index 823647d68..5ffc64b75 100755 --- a/tasks/receptionist/src/receptionist/state_machine.py +++ b/tasks/receptionist/src/receptionist/state_machine.py @@ -62,230 +62,221 @@ def __init__( self.userdata.dataset = "receptionist" self.userdata.seat_position = PointStamped() - # smach.StateMachine.add( - # "SAY_START", - # Say(text="Start of receptionist task. Going to waiting area."), - # transitions={ - # "succeeded": "GO_TO_WAIT_LOCATION_GUEST_1", - # "aborted": "GO_TO_WAIT_LOCATION_GUEST_1", - # "preempted": "GO_TO_WAIT_LOCATION_GUEST_1", - # }, - # ) + smach.StateMachine.add( + "SAY_START", + Say(text="Start of receptionist task. Going to waiting area."), + transitions={ + "succeeded": "GO_TO_WAIT_LOCATION_GUEST_1", + "aborted": "GO_TO_WAIT_LOCATION_GUEST_1", + "preempted": "GO_TO_WAIT_LOCATION_GUEST_1", + }, + ) - # """ - # First guest - # """ + """ + First guest + """ - # self._goto_waiting_area(guest_id=1) + self._goto_waiting_area(guest_id=1) - # smach.StateMachine.add( - # "INTRODUCE_ROBOT", - # Say( - # text="Hello my name is Tiago, nice to meet you, I shall be your receptionist for today. I will try and be polite by looking at you when I speak, so I hope you will do the same by looking into my eyes whenever possible. First let me get to know you a little bit better." - # ), - # transitions={ - # "succeeded": f"HANDLE_GUEST_1", - # "aborted": f"HANDLE_GUEST_1", - # "preempted": f"HANDLE_GUEST_1", - # }, - # ) + smach.StateMachine.add( + "INTRODUCE_ROBOT", + Say( + text="Hello my name is Tiago, nice to meet you, I shall be your receptionist for today. I will try and be polite by looking at you when I speak, so I hope you will do the same by looking into my eyes whenever possible. First let me get to know you a little bit better." + ), + transitions={ + "succeeded": f"HANDLE_GUEST_1", + "aborted": f"HANDLE_GUEST_1", + "preempted": f"HANDLE_GUEST_1", + }, + ) # """ # GET GUEST ATTRIBUTES # """ - # smach.StateMachine.add( - # "HANDLE_GUEST_1", - # HandleGuest("guest1", learn_guest_1), - # transitions={ - # "succeeded": "SAY_FOLLOW_GUEST_1", - # "failed": "SAY_FOLLOW_GUEST_1", - # }, - # ) - smach.StateMachine.add( "HANDLE_GUEST_1", HandleGuest("guest1", learn_guest_1), + transitions={ + "succeeded": "SAY_FOLLOW_GUEST_1", + "failed": "SAY_FOLLOW_GUEST_1", + }, + ) + + self._guide_guest(guest_id=1) + + smach.StateMachine.add( + "INTRODUCE_AND_SEAT_GUEST_1", + IntroduceAndSeatGuest( + "guest1", + ["host"], + seat_area, + sofa_area, + sofa_point, + max_people_on_sofa, + ), + transitions={ + "succeeded": "SAY_RETURN_WAITING_AREA", + "failed": "SAY_RETURN_WAITING_AREA", + }, + ) + + smach.StateMachine.add( + "SAY_RETURN_WAITING_AREA", + Say(text="Let me go back to the waiting area."), + transitions={ + "succeeded": "GO_TO_WAIT_LOCATION_GUEST_2", + "aborted": "GO_TO_WAIT_LOCATION_GUEST_2", + "preempted": "GO_TO_WAIT_LOCATION_GUEST_2", + }, + ) + + # """ + # Guest 2 + # """ + + self._goto_waiting_area(2) + + smach.StateMachine.add( + "HANDLE_GUEST_2", + HandleGuest("guest2", False), + transitions={ + "succeeded": "SAY_FOLLOW_GUEST_2", + "failed": "SAY_FOLLOW_GUEST_2", + }, + ) + + self._guide_guest(guest_id=2) + + smach.StateMachine.add( + "INTRODUCE_AND_SEAT_GUEST_2", + IntroduceAndSeatGuest( + "guest2", + ["host", "guest1"], + seat_area, + sofa_area, + sofa_point, + max_people_on_sofa, + ), + transitions={ + "succeeded": "SAY_GOODBYE", + "failed": "SAY_GOODBYE", + }, + ) + + """ + Finish + """ + smach.StateMachine.add( + "SAY_GOODBYE", + Say( + text="Goodbye fellow humans, I shall be going back where I came from" + ), + transitions={ + "succeeded": "GO_TO_FINISH_LOCATION", + "aborted": "failed", + "preempted": "GO_TO_FINISH_LOCATION", + }, + ) + + smach.StateMachine.add( + "GO_TO_FINISH_LOCATION", + GoToLocation(wait_pose), + transitions={ + "succeeded": "SAY_FINISHED", + "failed": "GO_TO_FINISH_LOCATION", + }, + ) + smach.StateMachine.add( + "SAY_FINISHED", + Say(text="I am done."), transitions={ "succeeded": "succeeded", - "failed": "succeeded", + "aborted": "failed", + "preempted": "succeeded", }, ) - # self._guide_guest(guest_id=1) - - # smach.StateMachine.add( - # "INTRODUCE_AND_SEAT_GUEST_1", - # IntroduceAndSeatGuest( - # "guest1", - # ["host"], - # seat_area, - # sofa_area, - # sofa_point, - # max_people_on_sofa, - # ), - # transitions={ - # "succeeded": "SAY_RETURN_WAITING_AREA", - # "failed": "SAY_RETURN_WAITING_AREA", - # }, - # ) - - # smach.StateMachine.add( - # "SAY_RETURN_WAITING_AREA", - # Say(text="Let me go back to the waiting area."), - # transitions={ - # "succeeded": "GO_TO_WAIT_LOCATION_GUEST_2", - # "aborted": "GO_TO_WAIT_LOCATION_GUEST_2", - # "preempted": "GO_TO_WAIT_LOCATION_GUEST_2", - # }, - # ) - - # # """ - # # Guest 2 - # # """ - - # self._goto_waiting_area(2) - - # smach.StateMachine.add( - # "HANDLE_GUEST_2", - # HandleGuest("guest2", False), - # transitions={ - # "succeeded": "SAY_FOLLOW_GUEST_2", - # "failed": "SAY_FOLLOW_GUEST_2", - # }, - # ) - - # self._guide_guest(guest_id=2) - - # smach.StateMachine.add( - # "INTRODUCE_AND_SEAT_GUEST_2", - # IntroduceAndSeatGuest( - # "guest2", - # ["host", "guest1"], - # seat_area, - # sofa_area, - # sofa_point, - # max_people_on_sofa, - # ), - # transitions={ - # "succeeded": "SAY_GOODBYE", - # "failed": "SAY_GOODBYE", - # }, - # ) - - # """ - # Finish - # """ - # smach.StateMachine.add( - # "SAY_GOODBYE", - # Say( - # text="Goodbye fellow humans, I shall be going back where I came from" - # ), - # transitions={ - # "succeeded": "GO_TO_FINISH_LOCATION", - # "aborted": "failed", - # "preempted": "GO_TO_FINISH_LOCATION", - # }, - # ) - - # smach.StateMachine.add( - # "GO_TO_FINISH_LOCATION", - # GoToLocation(wait_pose), - # transitions={ - # "succeeded": "SAY_FINISHED", - # "failed": "GO_TO_FINISH_LOCATION", - # }, - # ) - # smach.StateMachine.add( - # "SAY_FINISHED", - # Say(text="I am done."), - # transitions={ - # "succeeded": "succeeded", - # "aborted": "failed", - # "preempted": "succeeded", - # }, - # ) - - # def _goto_waiting_area(self, guest_id: int) -> None: - # """Adds the states to go to the waiting area. - - # Args: - # guest_id (int): Identifier for the guest. - # """ - - # smach.StateMachine.add( - # f"GO_TO_WAIT_LOCATION_GUEST_{guest_id}", - # GoToLocation(self.wait_pose), - # transitions={ - # "succeeded": f"SAY_WAITING_GUEST_{guest_id}", - # "failed": f"GO_TO_WAIT_LOCATION_GUEST_{guest_id}", - # }, - # ) - - # smach.StateMachine.add( - # f"SAY_WAITING_GUEST_{guest_id}", - # Say(text="I am waiting for a guest."), - # transitions={ - # "succeeded": f"WAIT_FOR_PERSON_GUEST_{guest_id}", - # "aborted": f"WAIT_FOR_PERSON_GUEST_{guest_id}", - # "preempted": f"WAIT_FOR_PERSON_GUEST_{guest_id}", - # }, - # ) - - # smach.StateMachine.add( - # f"WAIT_FOR_PERSON_GUEST_{guest_id}", - # WaitForPersonInArea(self.wait_area), - # transitions={ - # "succeeded": f"CHECK_GUEST_ID_GUEST_{guest_id}", - # "failed": f"CHECK_GUEST_ID_GUEST_{guest_id}", - # }, - # ) - - # def check_guest_id(ud): - # if guest_id == 2: - # return "guest_2" - # else: - # return "guest_1" - - # smach.StateMachine.add( - # f"CHECK_GUEST_ID_GUEST_{guest_id}", - # smach.CBState(check_guest_id, outcomes=["guest_1", "guest_2"]), - # transitions={"guest_2": "HANDLE_GUEST_2", "guest_1": "INTRODUCE_ROBOT"}, - # ) - - # def _guide_guest(self, guest_id: int) -> None: - # """Adds the states to guide a guest to the - # seating area. - - # Args: - # guest_id (int): Identifier for the guest. - # """ - - # smach.StateMachine.add( - # f"SAY_FOLLOW_GUEST_{guest_id}", - # Say(text="Please follow me, I will guide you to the other guests"), - # transitions={ - # "succeeded": f"GO_TO_SEAT_LOCATION_GUEST_{guest_id}", - # "preempted": "failed", - # "aborted": "failed", - # }, - # ) - - # smach.StateMachine.add( - # f"GO_TO_SEAT_LOCATION_GUEST_{guest_id}", - # GoToLocation(self.seat_pose), - # transitions={ - # "succeeded": f"SAY_WAIT_GUEST_{guest_id}", - # "failed": f"GO_TO_SEAT_LOCATION_GUEST_{guest_id}", - # }, - # ) - - # smach.StateMachine.add( - # f"SAY_WAIT_GUEST_{guest_id}", - # Say(text="Please wait here on my left."), - # transitions={ - # "succeeded": f"INTRODUCE_AND_SEAT_GUEST_{guest_id}", - # "preempted": "failed", - # "aborted": "failed", - # }, - # ) + def _goto_waiting_area(self, guest_id: int) -> None: + """Adds the states to go to the waiting area. + + Args: + guest_id (int): Identifier for the guest. + """ + + smach.StateMachine.add( + f"GO_TO_WAIT_LOCATION_GUEST_{guest_id}", + GoToLocation(self.wait_pose), + transitions={ + "succeeded": f"SAY_WAITING_GUEST_{guest_id}", + "failed": f"GO_TO_WAIT_LOCATION_GUEST_{guest_id}", + }, + ) + + smach.StateMachine.add( + f"SAY_WAITING_GUEST_{guest_id}", + Say(text="I am waiting for a guest."), + transitions={ + "succeeded": f"WAIT_FOR_PERSON_GUEST_{guest_id}", + "aborted": f"WAIT_FOR_PERSON_GUEST_{guest_id}", + "preempted": f"WAIT_FOR_PERSON_GUEST_{guest_id}", + }, + ) + + smach.StateMachine.add( + f"WAIT_FOR_PERSON_GUEST_{guest_id}", + WaitForPersonInArea(self.wait_area), + transitions={ + "succeeded": f"CHECK_GUEST_ID_GUEST_{guest_id}", + "failed": f"CHECK_GUEST_ID_GUEST_{guest_id}", + }, + ) + + def check_guest_id(ud): + if guest_id == 2: + return "guest_2" + else: + return "guest_1" + + smach.StateMachine.add( + f"CHECK_GUEST_ID_GUEST_{guest_id}", + smach.CBState(check_guest_id, outcomes=["guest_1", "guest_2"]), + transitions={"guest_2": "HANDLE_GUEST_2", "guest_1": "INTRODUCE_ROBOT"}, + ) + + def _guide_guest(self, guest_id: int) -> None: + """Adds the states to guide a guest to the + seating area. + + Args: + guest_id (int): Identifier for the guest. + """ + + smach.StateMachine.add( + f"SAY_FOLLOW_GUEST_{guest_id}", + Say(text="Please follow me, I will guide you to the other guests"), + transitions={ + "succeeded": f"GO_TO_SEAT_LOCATION_GUEST_{guest_id}", + "preempted": "failed", + "aborted": "failed", + }, + ) + + smach.StateMachine.add( + f"GO_TO_SEAT_LOCATION_GUEST_{guest_id}", + GoToLocation(self.seat_pose), + transitions={ + "succeeded": f"SAY_WAIT_GUEST_{guest_id}", + "failed": f"GO_TO_SEAT_LOCATION_GUEST_{guest_id}", + }, + ) + + smach.StateMachine.add( + f"SAY_WAIT_GUEST_{guest_id}", + Say(text="Please wait here on my left."), + transitions={ + "succeeded": f"INTRODUCE_AND_SEAT_GUEST_{guest_id}", + "preempted": "failed", + "aborted": "failed", + }, + ) \ No newline at end of file