diff --git a/tasks/receptionist/src/receptionist/state_machine.py b/tasks/receptionist/src/receptionist/state_machine.py index 8d7dee20e..f1f8be629 100755 --- a/tasks/receptionist/src/receptionist/state_machine.py +++ b/tasks/receptionist/src/receptionist/state_machine.py @@ -280,6 +280,16 @@ def _guide_guest(self, guest_id: int) -> None: smach.StateMachine.add( f"SAY_WAIT_GUEST_{guest_id}", Say(text="Please wait here on my left."), + transitions={ + "succeeded": f"LOOK_EYES_{guest_id}", + "preempted": "failed", + "aborted": "failed", + }, + ) + + smach.StateMachine.add( + f"LOOK_EYES_{guest_id}", + PlayMotion(motion_name="look_very_left"), transitions={ "succeeded": f"INTRODUCE_AND_SEAT_GUEST_{guest_id}", "preempted": "failed", diff --git a/tasks/receptionist/src/receptionist/states/introduce.py b/tasks/receptionist/src/receptionist/states/introduce.py index 2f14012ea..b0f091012 100644 --- a/tasks/receptionist/src/receptionist/states/introduce.py +++ b/tasks/receptionist/src/receptionist/states/introduce.py @@ -11,7 +11,9 @@ from typing import Dict, List, Any, Optional -def stringify_guest_data(guest_data: Dict[str, Any], guest_id: str) -> str: +def stringify_guest_data( + guest_data: Dict[str, Any], guest_id: str, describe_features: bool +) -> str: """Converts the guest data for a specified guest into a string that can be used for the robot to introduce the guest to the other guests/host. @@ -46,11 +48,9 @@ def stringify_guest_data(guest_data: Dict[str, Any], guest_id: str) -> str: }, ) - guest_str = "" + guest_str = f"{relevant_guest_data['name']}, their favourite drink is {relevant_guest_data['drink']}. " - guest_str += f"{relevant_guest_data['name']}, their favourite drink is {relevant_guest_data['drink']}. " - - if not relevant_guest_data["detection"]: + if not relevant_guest_data["detection"] or not describe_features: return guest_str filtered_attributes = {} @@ -107,6 +107,9 @@ def stringify_guest_data(guest_data: Dict[str, Any], guest_id: str) -> str: if len(top_4_attributes) < 4: top_4_attributes.append(sorted_attributes[4]) + wearing_items = [] + not_wearing_items = [] + for i in range(len(top_4_attributes)): attribute_name = top_4_attributes[i] attribute_value = filtered_attributes[top_4_attributes[i]] @@ -115,25 +118,47 @@ def stringify_guest_data(guest_data: Dict[str, Any], guest_id: str) -> str: if attribute_name == "hair": hair_shape = attribute_value["hair_shape"] hair_colour = attribute_value["hair_colour"] - guest_str += f"They have {hair_shape} and {hair_colour} . " + guest_str += f"They have {hair_shape} and {hair_colour}. " elif attribute_name == "facial_hair": if confidence < 0: guest_str += "They don't have facial hair. " else: guest_str += "They have facial hair. " else: + attribute = attribute_value["attribute"] if confidence < 0: - if isSingular(attribute_value["attribute"]): - guest_str += ( - f"They are not wearing a {attribute_value['attribute']}." - ) + if isSingular(attribute): + not_wearing_items.append(f"a {attribute}") else: - guest_str += f"They are not wearing {attribute_value['attribute']}." + not_wearing_items.append(attribute) else: - if isSingular(attribute_value["attribute"]): - guest_str += f"They are wearing a {attribute_value['attribute']}." + if isSingular(attribute): + wearing_items.append(f"a {attribute}") else: - guest_str += f"They are wearing {attribute_value['attribute']}." + wearing_items.append(attribute) + + def grammatical_concat(items): + if len(items) > 1: + return ", ".join(items[:-1]) + " and " + items[-1] + elif items: + return items[0] + else: + return "" + + # Combine wearing and not wearing items into guest_str + if wearing_items: + guest_str += "They are wearing " + grammatical_concat(wearing_items) + ". " + if not_wearing_items: + if wearing_items: + guest_str += ( + "They are also not wearing " + + grammatical_concat(not_wearing_items) + + "." + ) + else: + guest_str += ( + "They are not wearing " + grammatical_concat(not_wearing_items) + "." + ) return guest_str @@ -179,16 +204,19 @@ def isSingular(attribute: str): class GetStrGuestData(smach.State): - def __init__(self, guest_id: str): + def __init__(self, guest_id: str, describe_features: bool = False): super().__init__( outcomes=["succeeded"], input_keys=["guest_data"], output_keys=["guest_str"], ) self._guest_id = guest_id + self._describe_features = describe_features def execute(self, userdata: UserData) -> str: - guest_str = stringify_guest_data(userdata.guest_data, self._guest_id) + guest_str = stringify_guest_data( + userdata.guest_data, self._guest_id, self._describe_features + ) userdata.guest_str = guest_str return "succeeded" @@ -229,6 +257,7 @@ def __init__( self, guest_to_introduce: str, guest_to_introduce_to: Optional[str] = None, + describe_features: bool = False, everyone: Optional[bool] = False, ): super().__init__( @@ -241,7 +270,9 @@ def __init__( if everyone: smach.StateMachine.add( "GetStrGuestData", - GetStrGuestData(guest_id=guest_to_introduce), + GetStrGuestData( + guest_id=guest_to_introduce, describe_features=describe_features + ), transitions={"succeeded": "SayIntroduce"}, ) smach.StateMachine.add( @@ -260,7 +291,9 @@ def __init__( else: smach.StateMachine.add( "GetStrGuestData", - GetStrGuestData(guest_id=guest_to_introduce), + GetStrGuestData( + guest_id=guest_to_introduce, describe_features=describe_features + ), transitions={"succeeded": "GetGuestName"}, ) diff --git a/tasks/receptionist/src/receptionist/states/introduce_and_seat_guest.py b/tasks/receptionist/src/receptionist/states/introduce_and_seat_guest.py index 8579e637b..54193976e 100644 --- a/tasks/receptionist/src/receptionist/states/introduce_and_seat_guest.py +++ b/tasks/receptionist/src/receptionist/states/introduce_and_seat_guest.py @@ -466,7 +466,7 @@ class GetLookPoint(smach.State): def __init__(self, guest_id: str): smach.State.__init__( self, - outcomes=["succeeded"], + outcomes=["succeeded", "failed"], input_keys=["matched_face_detections"], output_keys=["look_point"], ) @@ -476,13 +476,14 @@ def execute(self, userdata): if len(userdata.matched_face_detections) == 0: userdata.look_point = PointStamped() rospy.logwarn(f"Failed to find guest: {self._guest_id}") - return "succeeded" + return "failed" for detection in userdata.matched_face_detections: if detection.name == self._guest_id: look_point = PointStamped( point=detection.point, header=Header(frame_id="map") ) + look_point.point.z = 0.75 # fixed height userdata.look_point = look_point return "succeeded" @@ -518,10 +519,13 @@ def execute(self, userdata): return "succeeded_sofa" if len(userdata.empty_seat_detections) > 0: - userdata.seat_position = PointStamped( + seat_position = PointStamped( point=userdata.empty_seat_detections[0].point, header=Header(frame_id="map"), ) + seat_position.point.z = 0.5 # fixed height + userdata.seat_position = seat_position + return "succeeded_chair" return "failed" @@ -633,7 +637,20 @@ def execute(self, userdata): smach.StateMachine.add( f"GET_LOOK_POINT_{guest_to_introduce_to}", GetLookPoint(guest_to_introduce_to), - transitions={"succeeded": f"LOOK_AT_{guest_to_introduce_to}"}, + transitions={ + "succeeded": f"LOOK_AT_{guest_to_introduce_to}", + "failed": f"LOOK_CENTRE_BACKUP_{guest_to_introduce_to}", + }, + ) + + smach.StateMachine.add( + f"LOOK_CENTRE_BACKUP_{guest_to_introduce_to}", + PlayMotion(motion_name="look_centre"), + transitions={ + "succeeded": f"INTRODUCE_{guest_id}_TO_{guest_to_introduce_to}", + "aborted": f"INTRODUCE_{guest_id}_TO_{guest_to_introduce_to}", + "preempted": f"INTRODUCE_{guest_id}_TO_{guest_to_introduce_to}", + }, ) # Look at the guest to introduce to @@ -654,6 +671,7 @@ def execute(self, userdata): Introduce( guest_to_introduce=guest_id, guest_to_introduce_to=guest_to_introduce_to, + describe_features=guest_to_introduce_to != "host", ), transitions={ "succeeded": f"LOOK_AT_WAITING_GUEST_{guest_id}_{guest_to_introduce_to}", @@ -675,6 +693,7 @@ def execute(self, userdata): Introduce( guest_to_introduce=guest_to_introduce_to, guest_to_introduce_to=guest_id, + describe_features=guest_to_introduce_to != "host", ), transitions={ "succeeded": ( @@ -692,7 +711,17 @@ def execute(self, userdata): transitions={ "succeeded_sofa": "SAY_SOFA", "succeeded_chair": "SAY_CHAIR", - "failed": "SAY_ANY", + "failed": "LOOK_CENTRE_SEAT", + }, + ) + + smach.StateMachine.add( + "LOOK_CENTRE_SEAT", + PlayMotion(motion_name="look_centre"), + transitions={ + "succeeded": "SAY_ANY", + "aborted": "SAY_ANY", + "preempted": "SAY_ANY", }, ) @@ -897,7 +926,7 @@ def __init__(self, guest_id: str): def execute(self, userdata): if len(userdata.matched_face_detections) == 0: userdata.look_point = PointStamped() - return "succeeded" + return "failed" for detection in userdata.matched_face_detections: if detection.name == self._guest_id: @@ -938,10 +967,12 @@ def execute(self, userdata): return "succeeded_sofa" if len(userdata.empty_seat_detections) > 0: - userdata.seat_position = PointStamped( + seat_position = PointStamped( point=userdata.empty_seat_detections[0][0].point, header=Header(frame_id="map"), ) + seat_position.point.z = 0.5 + userdata.seat_position = seat_position return "succeeded_chair" return "failed" @@ -1022,7 +1053,20 @@ def execute(self, userdata): smach.StateMachine.add( f"GET_LOOK_POINT_{guest_to_introduce_to}", GetLookPoint(guest_to_introduce_to), - transitions={"succeeded": f"LOOK_AT_{guest_to_introduce_to}"}, + transitions={ + "succeeded": f"LOOK_AT_{guest_to_introduce_to}", + "failed": "LOOK_CENTRE", + }, + ) + + smach.StateMachine.add( + "LOOK_CENTRE", + PlayMotion(motion_name="look_centre"), + transitions={ + "succeeded": f"INTRODUCE_{guest_id}_TO_{guest_to_introduce_to}", + "aborted": f"INTRODUCE_{guest_id}_TO_{guest_to_introduce_to}", + "preempted": f"INTRODUCE_{guest_id}_TO_{guest_to_introduce_to}", + }, ) smach.StateMachine.add( @@ -1041,6 +1085,7 @@ def execute(self, userdata): Introduce( guest_to_introduce=guest_id, guest_to_introduce_to=guest_to_introduce_to, + describe_features=guest_to_introduce_to != "host", ), transitions={ "succeeded": (