diff --git a/common/speech/lasr_speech_recognition_whisper/nodes/transcribe_microphone_server b/common/speech/lasr_speech_recognition_whisper/nodes/transcribe_microphone_server index afa55f215..3ab9d2983 100644 --- a/common/speech/lasr_speech_recognition_whisper/nodes/transcribe_microphone_server +++ b/common/speech/lasr_speech_recognition_whisper/nodes/transcribe_microphone_server @@ -256,7 +256,8 @@ def parse_args() -> dict: action="store_true", help="Disable warming up the model by running inference on a test file.", ) - args, unknown = parser.parse_known_args() + + args,unknown = parser.parse_known_args() return vars(args) @@ -300,8 +301,6 @@ def configure_whisper_cache() -> None: if __name__ == "__main__": configure_whisper_cache() config = parse_args() - rospy.init_node("speech_transcription_node") - server = TranscribeSpeechAction( - config["action_name"], configure_model_params(config) - ) + rospy.init_node("transcribe_speech_server") + server = TranscribeSpeechAction("transcribe_speech", configure_model_params(config)) rospy.spin() diff --git a/common/vision/lasr_vision_clip/CMakeLists.txt b/common/vision/lasr_vision_clip/CMakeLists.txt index c2ce23209..739bfda15 100644 --- a/common/vision/lasr_vision_clip/CMakeLists.txt +++ b/common/vision/lasr_vision_clip/CMakeLists.txt @@ -19,7 +19,7 @@ find_package(catkin REQUIRED catkin_virtualenv) catkin_python_setup() catkin_generate_virtualenv( INPUT_REQUIREMENTS requirements.in - PYTHON_INTERPRETER python3.9 + PYTHON_INTERPRETER python3.10 ) ################################################ ## Declare ROS messages, services and actions ## @@ -48,15 +48,14 @@ catkin_generate_virtualenv( ## Generate messages in the 'msg' folder # add_message_files( # FILES -# Message1.msg -# Message2.msg +# VqaResult.msg +# VqaResult.msg # ) -## Generate services in the 'srv' folder +# Generate services in the 'srv' folder # add_service_files( # FILES -# Service1.srv -# Service2.srv +# Vqa.srv # ) # Generate actions in the 'action' folder @@ -68,8 +67,7 @@ catkin_generate_virtualenv( # Generate added messages and services with any dependencies listed here # generate_messages( # DEPENDENCIES -# actionlib_msgs -# geometry_msgs +# sensor_msgs # ) ################################################ @@ -157,22 +155,10 @@ include_directories( ## Mark executable scripts (Python etc.) for installation ## in contrast to setup.py, you can choose the destination -# catkin_install_python(PROGRAMS -# nodes/qualification -# nodes/actions/wait_greet -# nodes/actions/identify -# nodes/actions/greet -# nodes/actions/get_name -# nodes/actions/learn_face -# nodes/actions/get_command -# nodes/actions/guide -# nodes/actions/find_person -# nodes/actions/detect_people -# nodes/actions/receive_object -# nodes/actions/handover_object -# nodes/better_qualification -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) +catkin_install_python(PROGRAMS + nodes/vqa + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) ## Mark executables for installation ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html diff --git a/tasks/receptionist/__init__.py b/common/vision/lasr_vision_clip/__init__.py similarity index 100% rename from tasks/receptionist/__init__.py rename to common/vision/lasr_vision_clip/__init__.py diff --git a/common/vision/lasr_vision_clip/nodes/vqa b/common/vision/lasr_vision_clip/nodes/vqa new file mode 100644 index 000000000..f5bc344a5 --- /dev/null +++ b/common/vision/lasr_vision_clip/nodes/vqa @@ -0,0 +1,50 @@ +#!/usr/bin/env python3 +import rospy +from typing import List +from lasr_vision_clip.clip_utils import load_model, query_image_stream +from lasr_vision_msgs.srv import VqaRequest, VqaResponse, Vqa +from sensor_msgs.msg import Image + + +class VqaService: + def __init__(self, model_device: str = "cuda") -> None: + """Caches the clip model. + + Args: + model_device (str, optional): device to load model onto. Defaults to "cuda". + + """ + + self._model = load_model(model_device) + self._debug_pub = rospy.Publisher("/clip_vqa/debug", Image, queue_size=1) + rospy.loginfo("Clip VQA service started") + + def query_clip(self, request: VqaRequest) -> VqaResponse: + """Queries CLIP from the robot's image stream and returns + the most likely answer and cosine similarity score. + + Args: + possible_answers (List[str]): set of possible answers. + + Returns: + VqaResult + """ + possible_answers = request.possible_answers + answer, cos_score, annotated_img = query_image_stream( + self._model, possible_answers, annotate=True + ) + + self._debug_pub.publish(annotated_img) + + result = VqaResponse() + result.answer = answer + rospy.loginfo(f"Answer: {answer}") + result.similarity = float(cos_score) + return result + + +if __name__ == "__main__": + rospy.init_node("clip_vqa_service") + service = VqaService() + rospy.Service("/clip_vqa/query_service", Vqa, service.query_clip) + rospy.spin() diff --git a/common/vision/lasr_vision_clip/package.xml b/common/vision/lasr_vision_clip/package.xml index e64ef13ef..165c21c6e 100644 --- a/common/vision/lasr_vision_clip/package.xml +++ b/common/vision/lasr_vision_clip/package.xml @@ -50,6 +50,11 @@ catkin catkin_virtualenv + message_generation + message_runtime + sensor_msgs + sensor_msgs + lasr_vision_msgs diff --git a/common/vision/lasr_vision_clip/requirements.txt b/common/vision/lasr_vision_clip/requirements.txt index 6d7e59d07..7c61ba101 100644 --- a/common/vision/lasr_vision_clip/requirements.txt +++ b/common/vision/lasr_vision_clip/requirements.txt @@ -1,22 +1,15 @@ ---extra-index-url https://pypi.ngc.nvidia.com ---trusted-host pypi.ngc.nvidia.com - certifi==2024.2.2 # via requests charset-normalizer==3.3.2 # via requests -click==8.1.7 # via nltk -clip @ git+https://github.com/openai/CLIP.git # via -r requirements.in -filelock==3.13.1 # via huggingface-hub, torch, transformers, triton -fsspec==2024.2.0 # via huggingface-hub, torch -ftfy==6.1.3 # via -r requirements.in, clip -huggingface-hub==0.20.3 # via sentence-transformers, tokenizers, transformers -idna==3.6 # via requests +filelock==3.13.4 # via huggingface-hub, torch, transformers, triton +fsspec==2024.3.1 # via huggingface-hub, torch +huggingface-hub==0.22.2 # via sentence-transformers, tokenizers, transformers +idna==3.7 # via requests jinja2==3.1.3 # via torch -joblib==1.3.2 # via nltk, scikit-learn +joblib==1.4.0 # via scikit-learn markupsafe==2.1.5 # via jinja2 mpmath==1.3.0 # via sympy networkx==3.2.1 # via torch -nltk==3.8.1 # via sentence-transformers -numpy==1.26.3 # via opencv-python, scikit-learn, scipy, sentence-transformers, torchvision, transformers +numpy==1.26.4 # via opencv-python, scikit-learn, scipy, sentence-transformers, 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 @@ -27,27 +20,24 @@ 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.19.3 # via torch -nvidia-nvjitlink-cu12==12.3.101 # via nvidia-cusolver-cu12, nvidia-cusparse-cu12 +nvidia-nvjitlink-cu12==12.4.127 # via nvidia-cusolver-cu12, nvidia-cusparse-cu12 nvidia-nvtx-cu12==12.1.105 # via torch opencv-python==4.9.0.80 # via -r requirements.in -packaging==23.2 # via huggingface-hub, transformers -pillow==10.2.0 # via sentence-transformers, torchvision +packaging==24.0 # via huggingface-hub, transformers +pillow==10.3.0 # via sentence-transformers pyyaml==6.0.1 # via huggingface-hub, transformers -regex==2023.12.25 # via -r requirements.in, clip, nltk, transformers -requests==2.31.0 # via huggingface-hub, torchvision, transformers -safetensors==0.4.2 # via transformers -scikit-learn==1.4.0 # via sentence-transformers -scipy==1.12.0 # via scikit-learn, sentence-transformers -sentence-transformers==2.3.1 # via -r requirements.in -sentencepiece==0.1.99 # via sentence-transformers +regex==2024.4.16 # via transformers +requests==2.31.0 # via huggingface-hub, transformers +safetensors==0.4.3 # via transformers +scikit-learn==1.4.2 # via sentence-transformers +scipy==1.13.0 # via scikit-learn, sentence-transformers +sentence-transformers==2.7.0 # via -r requirements.in sympy==1.12 # via torch -threadpoolctl==3.2.0 # via scikit-learn -tokenizers==0.15.1 # via transformers -torch==2.2.0 # via clip, sentence-transformers, torchvision -torchvision==0.17.0 # via clip -tqdm==4.66.1 # via -r requirements.in, clip, huggingface-hub, nltk, sentence-transformers, transformers -transformers==4.37.2 # via sentence-transformers +threadpoolctl==3.4.0 # via scikit-learn +tokenizers==0.15.2 # via transformers +torch==2.2.2 # via sentence-transformers +tqdm==4.66.2 # via huggingface-hub, sentence-transformers, transformers +transformers==4.39.3 # via sentence-transformers triton==2.2.0 # via torch -typing-extensions==4.9.0 # via huggingface-hub, torch -urllib3==2.2.0 # via requests -wcwidth==0.2.13 # via ftfy +typing-extensions==4.11.0 # via huggingface-hub, torch +urllib3==2.2.1 # 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 e69de29bb..8b1378917 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 @@ -0,0 +1 @@ + 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 81d6dd25a..fc92fe7cd 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 @@ -97,4 +97,4 @@ def query_image_stream( ) img = cv2_img.cv2_img_to_msg(cv2_im) - return answers[max_score], cos_scores, img + return answers[max_score], cos_scores[0, max_score], img diff --git a/common/vision/lasr_vision_msgs/CMakeLists.txt b/common/vision/lasr_vision_msgs/CMakeLists.txt index 1d196f37d..764829f05 100644 --- a/common/vision/lasr_vision_msgs/CMakeLists.txt +++ b/common/vision/lasr_vision_msgs/CMakeLists.txt @@ -63,6 +63,7 @@ add_service_files( TorchFaceFeatureDetection.srv Recognise.srv LearnFace.srv + Vqa.srv PointingDirection.srv ) diff --git a/common/vision/lasr_vision_msgs/srv/Vqa.srv b/common/vision/lasr_vision_msgs/srv/Vqa.srv new file mode 100644 index 000000000..2cb9a86f1 --- /dev/null +++ b/common/vision/lasr_vision_msgs/srv/Vqa.srv @@ -0,0 +1,9 @@ +string[] possible_answers + +--- + +# most likely answer +string answer + +# cosine similarity +float32 similarity \ No newline at end of file diff --git a/skills/src/lasr_skills/__init__.py b/skills/src/lasr_skills/__init__.py index babdd7ce2..b2d7eacc1 100755 --- a/skills/src/lasr_skills/__init__.py +++ b/skills/src/lasr_skills/__init__.py @@ -17,3 +17,4 @@ from .receive_object import ReceiveObject from .handover_object import HandoverObject from .ask_and_listen import AskAndListen +from .clip_vqa import QueryImage diff --git a/skills/src/lasr_skills/ask_and_listen.py b/skills/src/lasr_skills/ask_and_listen.py index 1763354ec..cd133437e 100644 --- a/skills/src/lasr_skills/ask_and_listen.py +++ b/skills/src/lasr_skills/ask_and_listen.py @@ -6,39 +6,92 @@ class AskAndListen(smach.StateMachine): - def __init__(self, question: Union[str, None] = None): - if question is not None: + def __init__( + self, + tts_phrase: Union[str, None] = None, + tts_phrase_format_str: Union[str, None] = None, + ): + + if tts_phrase is not None: smach.StateMachine.__init__( self, outcomes=["succeeded", "failed"], output_keys=["transcribed_speech"], ) - else: + with self: + smach.StateMachine.add( + "SAY", + Say(text=tts_phrase), + transitions={ + "succeeded": "LISTEN", + "aborted": "failed", + "preempted": "failed", + }, + ) + smach.StateMachine.add( + "LISTEN", + Listen(), + transitions={ + "succeeded": "succeeded", + "aborted": "failed", + "preempted": "failed", + }, + remapping={"sequence": "transcribed_speech"}, + ) + elif tts_phrase_format_str is not None: smach.StateMachine.__init__( self, outcomes=["succeeded", "failed"], - input_keys=["tts_phrase"], output_keys=["transcribed_speech"], + input_keys=["tts_phrase_placeholders"], ) + with self: + smach.StateMachine.add( + "SAY", + Say(format_str=tts_phrase_format_str), + transitions={ + "succeeded": "LISTEN", + "aborted": "failed", + "preempted": "failed", + }, + remapping={"placeholders": "tts_phrase_placeholders"}, + ) + smach.StateMachine.add( + "LISTEN", + Listen(), + transitions={ + "succeeded": "succeeded", + "aborted": "failed", + "preempted": "failed", + }, + remapping={"sequence": "transcribed_speech"}, + ) - with self: - smach.StateMachine.add( - "SAY", - Say(question), - transitions={ - "succeeded": "LISTEN", - "aborted": "failed", - "preempted": "failed", - }, - remapping={"text": "tts_phrase"} if question is None else {}, - ) - smach.StateMachine.add( - "LISTEN", - Listen(), - transitions={ - "succeeded": "succeeded", - "aborted": "failed", - "preempted": "failed", - }, - remapping={"sequence": "transcribed_speech"}, + else: + smach.StateMachine.__init__( + self, + outcomes=["succeeded", "failed"], + output_keys=["transcribed_speech"], + input_keys=["tts_phrase"], ) + with self: + smach.StateMachine.add( + "SAY", + Say(), + transitions={ + "succeeded": "LISTEN", + "aborted": "failed", + "preempted": "failed", + }, + remapping={"text": "tts_phrase"}, + ) + smach.StateMachine.add( + "LISTEN", + Listen(), + transitions={ + "succeeded": "succeeded", + "aborted": "failed", + "preempted": "failed", + }, + remapping={"sequence": "transcribed_speech"}, + ) diff --git a/skills/src/lasr_skills/clip_vqa.py b/skills/src/lasr_skills/clip_vqa.py new file mode 100755 index 000000000..7126d10c0 --- /dev/null +++ b/skills/src/lasr_skills/clip_vqa.py @@ -0,0 +1,24 @@ +import smach_ros +from lasr_vision_msgs.srv import Vqa, VqaRequest + +from typing import List, Union + + +class QueryImage(smach_ros.ServiceState): + + def __init__(self, possible_answers: Union[None, List[str]] = None): + + if possible_answers is not None: + super(QueryImage, self).__init__( + "/clip_vqa/query_service", + Vqa, + request=VqaRequest(possible_answers=possible_answers), + response_slots=["answer", "similarity"], + ) + else: + super(QueryImage, self).__init__( + "/clip_vqa/query_service", + Vqa, + request_slots=["possible_answers"], + response_slots=["answer", "similarity"], + ) diff --git a/skills/src/lasr_skills/go_to_location.py b/skills/src/lasr_skills/go_to_location.py index e1142f0a4..562aeaf61 100755 --- a/skills/src/lasr_skills/go_to_location.py +++ b/skills/src/lasr_skills/go_to_location.py @@ -137,9 +137,7 @@ def __init__(self, location: Union[Pose, None] = None): "RAISE_BASE", PlayMotion("post_navigation"), transitions={ - "succeeded": ( - "ENABLE_HEAD_MANAGER" if not IS_SIMULATION else "GO_TO_LOCATION" - ), + "succeeded": "succeeded", "aborted": "failed", "preempted": "failed", }, diff --git a/skills/src/lasr_skills/play_motion.py b/skills/src/lasr_skills/play_motion.py index b2b81a372..a24e480d2 100644 --- a/skills/src/lasr_skills/play_motion.py +++ b/skills/src/lasr_skills/play_motion.py @@ -3,22 +3,34 @@ from play_motion_msgs.msg import PlayMotionAction, PlayMotionGoal -from typing import Union +from typing import Union, List class PlayMotion(smach_ros.SimpleActionState): + + def _needs_planning(self, motion_name: str) -> bool: + joints: List[str] = rospy.get_param( + f"/play_motion/motions/{motion_name}/joints" + ) + needs_planning: bool = any( + "arm" in joint or "gripper" in joint for joint in joints + ) + + print(f"Motion {motion_name} needs planning: {needs_planning}") + + return needs_planning + def __init__(self, motion_name: Union[str, None] = None): + # TODO: the play motion action server is always returning 'aborted', figure out what's going on if motion_name is not None: super(PlayMotion, self).__init__( "play_motion", PlayMotionAction, goal=PlayMotionGoal( motion_name=motion_name, - skip_planning=rospy.get_param( - f"/play_motion/motions/{motion_name}/joints" - ) - == ["torso_lift_joint"], + skip_planning=not self._needs_planning(motion_name), ), + result_cb=lambda _, __, ___: "succeeded", ) else: super(PlayMotion, self).__init__( @@ -26,10 +38,8 @@ def __init__(self, motion_name: Union[str, None] = None): PlayMotionAction, goal_cb=lambda ud, _: PlayMotionGoal( motion_name=ud.motion_name, - skip_planning=rospy.get_param( - f"/play_motion/motions/{ud.motion_name}/joints" - == ["torso_lift_joint"] - ), + skip_planning=not self._needs_planning(ud.motion_name), ), input_keys=["motion_name"], + result_cb=lambda _, __, ___: "succeeded", ) diff --git a/tasks/gpsr/data/mock_data/names.json b/tasks/gpsr/data/mock_data/names.json index b388fee75..7dbbe563f 100644 --- a/tasks/gpsr/data/mock_data/names.json +++ b/tasks/gpsr/data/mock_data/names.json @@ -1,14 +1,14 @@ { "names": [ - "Adel", - "Angel", - "Axel", - "Charlie", - "Janes", - "Jules", - "Morgan", - "Paris", - "Robin", - "Simone" + "adel", + "angel", + "axel", + "charlie", + "janes", + "jules", + "morgan", + "paris", + "robin", + "simone" ] } \ No newline at end of file diff --git a/tasks/gpsr/nodes/command_parser b/tasks/gpsr/nodes/command_parser index cd8c59341..ca350bfd2 100644 --- a/tasks/gpsr/nodes/command_parser +++ b/tasks/gpsr/nodes/command_parser @@ -8,32 +8,6 @@ from gpsr.load_known_data import GPSRDataLoader from gpsr.regex_command_parser import Configuration, gpsr_compile_and_parse from lasr_skills import AskAndListen, Say -ENGISH_MAPPING = { - "goToLoc": "Go to location", - "findPrsInRoom": "Find people in room", - "meetPrsAtBeac": "Meet person at beacon", - "countPrsInRoom": "Count people in room", - "tellPrsInfoInLoc": "Tell person information in location", - "talkInfoToGestPrsInRoom": "Talk information to guest in room", - "answerToGestPrsInRoom": "Answer to guest in room", - "followNameFromBeacToRoom": "Follow name from beacon to room", - "guideNameFromBeacToBeac": "Guide name from beacon to beacon", - "guidePrsFromBeacToBeac": "Guide person from beacon to beacon", - "guideClothPrsFromBeacToBeac": "Guide clothed person from beacon to beacon", - "greetClothDscInRm": "Greet clothed description in room", - "greetNameInRm": "Greet name in room", - "meetNameAtLocThenFindInRm": "Meet name at location then find in room", - "countClothPrsInRoom": "Count clothed people in room", - "tellPrsInfoAtLocToPrsAtLoc": "Tell person information at location to person at location", - "followPrsAtLoc": "Follow person at location", - "takeObjFromPlcmt": "Take object from placement", - "findObjInRoom": "Find object in room", - "countObjOnPlcmt": "Count object on placement", - "tellObjPropOnPlcmt": "Tell object properties on placement", - "bringMeObjFromPlcmt": "Bring me object from placement", - "tellCatPropOnPlcmt": "Tell category properties on placement", -} - def parse_args() -> Dict: parser = argparse.ArgumentParser(description="GPSR Command Parser") @@ -58,6 +32,7 @@ class ParseCommand(smach.State): self.data_config = data_config def execute(self, userdata): + rospy.loginfo(f"Received command : {userdata.transcribed_speech.lower()}") try: userdata.command = gpsr_compile_and_parse( self.data_config, userdata.transcribed_speech.lower() @@ -77,18 +52,96 @@ class OutputParsedCommand(smach.State): output_keys=["command_string"], ) + def _get_english_translation(self, command_dict: Dict) -> str: + translation_str = "" + + for index, command in enumerate(command_dict["commands"]): + command_paramaters = command_dict["command_params"][index] + print(f"Command: {command}, parameters: {command_paramaters}") + if index == 0: + translation_str += "First, you want me to " + else: + translation_str += "Then, you want me to " + guide = False + if command == "take": + if "object" in command_paramaters: + translation_str += f"take the {command_paramaters['object']} " + if "location" in command_paramaters: + translation_str += f"from the {command_paramaters['location']} " + elif "person" in command_paramaters: + translation_str += f"from {command_paramaters['person']} " + elif "room" in command_paramaters: + translation_str += f"from the {command_paramaters['room']} " + else: # take corresponds to guiding + guide = True + elif command == "place": + translation_str += f"place the {command_paramaters['object']} on the {command_paramaters['location']} " + elif command == "deliver": + translation_str += f"deliver the {command_paramaters['object']} " + if "name" in command_paramaters: + translation_str += f"to {command_paramaters['name']} " + translation_str += f"in the {command_paramaters['location']} " + elif "gesture" in command_paramaters: + translation_str += ( + f"to the person who is {command_paramaters['gesture']} " + ) + translation_str += f"in the {command_paramaters['location']} " + else: + translation_str += f"to you." + elif command == "go": + translation_str += f"go to the " + if "location" in command_paramaters: + translation_str += f"{command_paramaters['location']} " + elif "room" in command_paramaters: + translation_str += f"{command_paramaters['room']} " + elif command == "find": + if "gesture" in command_paramaters: + translation_str += ( + f"find the person who is {command_paramaters['gesture']} " + ) + translation_str += f"in the {command_paramaters['location']} " + elif "object" in command_paramaters: + translation_str += f"find the {command_paramaters['object']} " + translation_str += f"in the {command_paramaters['location']} " + elif command == "talk": + pass + elif command == "answer": + pass + elif command == "meet": + translation_str += f"meet {command_paramaters['name']} " + if "location" in command_paramaters: + translation_str += f"in the {command_paramaters['location']} " + elif command == "tell": + pass + elif command == "answer": + pass + elif command == "meet": + pass + elif command == "tell": + pass + elif command == "greet": + pass + elif command == "remember": + pass + elif command == "count": + pass + elif command == "describe": + pass + elif command == "offer": + pass + elif command == "follow": + pass + elif command == "accompany": + pass + if command == "guide" or guide: + pass + + return translation_str + def execute(self, userdata): try: - command = userdata.command - english_command = ENGISH_MAPPING[command["command"]] - command_parameters = command[command["command"]] - rospy.loginfo(f"Command: {english_command}") - rospy.loginfo(f"Parameters: {command_parameters}") - tts_phrase = f"I parsed the command as you want me to: {english_command}, with the following parameters:" - for key, value in command_parameters.items(): - if isinstance(value, list): - value = " and ".join(value) - tts_phrase += f" {key}: {value}," + command: Dict = userdata.command + tts_phrase = self._get_english_translation(command) except Exception as e: rospy.logerr(e) return "failed" diff --git a/tasks/gpsr/src/gpsr/regex_command_parser.py b/tasks/gpsr/src/gpsr/regex_command_parser.py index 27a6913bc..ea091f052 100644 --- a/tasks/gpsr/src/gpsr/regex_command_parser.py +++ b/tasks/gpsr/src/gpsr/regex_command_parser.py @@ -1,9 +1,11 @@ import itertools import re -from typing import List, Union, TypedDict, Dict +from typing import List, Union, TypedDict, Dict, Any counter = 0 +sub_command_counter = 0 +seen_sub_command_group_names = [] def uniq(i: str) -> str: @@ -32,7 +34,7 @@ def list_to_regex(list: List[str], key: Union[str, None] = None): "meet": ["meet"], "tell": ["tell"], "greet": ["greet", "salute", "say hello to", "introduce yourself to"], - "remember": ["meet", "contact", "get to know", "get acquainted with"], + # "remember": ["meet", "contact", "get to know", "get acquainted with"], <--- LOOKS UNUSED "count": ["tell me how many"], "describe": ["tell me how", "describe"], "offer": ["offer"], @@ -44,7 +46,8 @@ def list_to_regex(list: List[str], key: Union[str, None] = None): def verb(v): # return list_to_regex(verb_dict[v], f"verb_{v}") - return list_to_regex(verb_dict[v], None if len(verb_dict[v]) == 1 else "verb") + return list_to_regex(verb_dict[v], "verb") + # return list_to_regex(verb_dict[v], None if len(verb_dict[v]) == 1 else "verb") prep_dict = { @@ -116,7 +119,6 @@ def pick(self, key: str, lists: List[str]): ) else: union = union + Configuration.key_to_list(self, list) - return f"(?P<{uniq(key)}>{'|'.join(union)})" @@ -228,28 +230,84 @@ def command(key: str, matcher: str): matcher = re.sub("\(\?P\<", f"(?P{matcher})") - def SUB_COMMAND_TODO_UNIMPLEMENTED(): - return f'(?P<{uniq("subcommand")}>.*)' + def get_possible_sub_commands(type: str) -> str: + sub_commands = [] + assertion_check = False + if type == "atLoc": + sub_commands.append( + f"{verb('find')} {art} {Configuration.pick(configuration, 'object', ['obj', 'singCat'])} and {get_possible_sub_commands('foundObj')}" + ) + sub_commands.append( + f"{verb('find')} the (?:{gesture_person_list}|{pose_person_list}) and {get_possible_sub_commands('foundPers')}" + ) + sub_commands.append( + f"{verb('meet')} {Configuration.pick(configuration, 'name', ['name'])} and {get_possible_sub_commands('foundPers')}" + ) + elif type == "hasObj": + sub_commands.append( + f"{verb('place')} it {prep('onLocPrep')} the {Configuration.pick(configuration, 'location', ['plcmtLoc'])}" + ) + sub_commands.append(f"{verb('deliver')} it to me") + sub_commands.append( + f"{verb('deliver')} it {prep('deliverPrep')} the (?:{gesture_person_list}|{pose_person_list}) {prep('inLocPrep')} the {Configuration.pick(configuration, 'location', ['room'])}" + ) + sub_commands.append( + f"{verb('deliver')} it {prep('deliverPrep')} {Configuration.pick(configuration, 'name', ['name'])} {prep('inLocPrep')} the {Configuration.pick(configuration, 'location', ['room'])}" + ) + elif type == "foundPers": + sub_commands.append(f"{verb('talk')} {talk_list}") + sub_commands.append(f"{verb('answer')} a {question_list}") + sub_commands.append(f"{verb('follow')} them") + sub_commands.append( + f"{verb('follow')} them {prep('toLocPrep')} the {Configuration.pick(configuration, 'location', ['room'])}" + ) + sub_commands.append( + f"{verb('guide')} them {prep('toLocPrep')} the {Configuration.pick(configuration, 'location', ['room'])}" + ) + elif type == "foundObj": + sub_commands.append( + f"{verb('take')} it and {get_possible_sub_commands('hasObj')}" + ) + assertion_check = True + + union = "|".join(sub_commands) + group_names = re.findall(r"\(\?P<([a-zA-Z0-9_]+)>", union) + global seen_sub_command_group_names + global sub_command_counter + for index, name in enumerate(group_names): + if name in seen_sub_command_group_names: + # make name unique + new_name = f"{name}_{sub_command_counter}" + seen_sub_command_group_names.append(new_name) + sub_command_counter += 1 + union = re.sub(rf"\(\?P<{name}>", f"(?P<{new_name}>", union) + else: + seen_sub_command_group_names.append(name) + + # groups = re.search(r"(\b[A-Z]+\b).+(\b\d+)", union) + return f"(?:{union})" + # return f"(?P<{uniq(key)}>{'|'.join(union)})" + # print(get_possible_sub_commands("atLoc")) command( "goToLoc", - f"{verb('go')} {prep('toLocPrep')} the {Configuration.pick(configuration, 'location', ['loc', 'room'])} then {SUB_COMMAND_TODO_UNIMPLEMENTED()}", + f"{verb('go')} {prep('toLocPrep')} the {Configuration.pick(configuration, 'location', ['loc', 'room'])} then {get_possible_sub_commands('atLoc')}", ) command( "takeObjFromPlcmt", - f"{verb('take')} {art} {Configuration.pick(configuration, 'object', ['obj', 'singCat'])} {prep('fromLocPrep')} the {Configuration.pick(configuration, 'location', ['plcmtLoc'])} and {SUB_COMMAND_TODO_UNIMPLEMENTED()}", + f"{verb('take')} {art} {Configuration.pick(configuration, 'object', ['obj', 'singCat'])} {prep('fromLocPrep')} the {Configuration.pick(configuration, 'location', ['plcmtLoc'])} and {get_possible_sub_commands('hasObj')}", ) command( "findPrsInRoom", - f"{verb('find')} a (?:{gesture_person_list}|{pose_person_list}) {prep('inLocPrep')} the {Configuration.pick(configuration, 'location', ['room'])} and {SUB_COMMAND_TODO_UNIMPLEMENTED()}", + f"{verb('find')} a (?:{gesture_person_list}|{pose_person_list}) {prep('inLocPrep')} the {Configuration.pick(configuration, 'location', ['room'])} and {get_possible_sub_commands('foundPers')}", ) command( "findObjInRoom", - f"{verb('find')} {art} {Configuration.pick(configuration, 'object', ['obj', 'singCat'])} {prep('inLocPrep')} the {Configuration.pick(configuration, 'location', ['room'])} and {SUB_COMMAND_TODO_UNIMPLEMENTED()}", + f"{verb('find')} {art} {Configuration.pick(configuration, 'object', ['obj', 'singCat'])} {prep('inLocPrep')} the {Configuration.pick(configuration, 'location', ['room'])} then {get_possible_sub_commands('foundObj')}", ) command( "meetPrsAtBeac", - f"{verb('meet')} {Configuration.pick(configuration, 'name', ['name'])} {prep('inLocPrep')} the {Configuration.pick(configuration, 'location', ['room'])} and {SUB_COMMAND_TODO_UNIMPLEMENTED()}", + f"{verb('meet')} {Configuration.pick(configuration, 'name', ['name'])} {prep('inLocPrep')} the {Configuration.pick(configuration, 'location', ['room'])} and {get_possible_sub_commands('foundPers')}", ) command( "countObjOnPlcmt", @@ -301,11 +359,11 @@ def SUB_COMMAND_TODO_UNIMPLEMENTED(): ) command( "greetClothDscInRm", - f"{verb('greet')} the person wearing {art} {color_clothe_list} {prep('inLocPrep')} the {Configuration.pick(configuration, 'location', ['room'])} and {SUB_COMMAND_TODO_UNIMPLEMENTED()}", + f"{verb('greet')} the person wearing {art} {color_clothe_list} {prep('inLocPrep')} the {Configuration.pick(configuration, 'location', ['room'])} and {get_possible_sub_commands('foundPers')}", ) command( "greetNameInRm", - f"{verb('greet')} {Configuration.pick(configuration, 'name', ['name'])} {prep('inLocPrep')} the {Configuration.pick(configuration, 'location', ['room'])} and {SUB_COMMAND_TODO_UNIMPLEMENTED()}", + f"{verb('greet')} {Configuration.pick(configuration, 'name', ['name'])} {prep('inLocPrep')} the {Configuration.pick(configuration, 'location', ['room'])} and {get_possible_sub_commands('foundPers')}", ) command( "meetNameAtLocThenFindInRm", @@ -326,53 +384,121 @@ def SUB_COMMAND_TODO_UNIMPLEMENTED(): return "|".join(commands) -def gpsr_parse(matches: Dict[str, str]): - result = {} +def gpsr_parse(matches: Dict[str, str]) -> Dict[str, Any]: + result: dict[str, Any] = { + "commands": [], + "command_params": [], + } for key in matches.keys(): value = matches[key] if value is None: continue - - write_into = result - key = re.sub("uniq\d+_", "", key) - - while key.startswith("CMD"): - cmd, rest = key.split("_", 1) - cmd = cmd[3:] # remove CMD prefix - - if cmd not in write_into: - write_into[cmd] = {} - - write_into = write_into[cmd] - key = rest - - if "_" in key: - actual_key, value = key.split("_") - write_into[actual_key] = value + key_to_check = key.split("_")[-1] + while key_to_check.isnumeric(): + key = "_".join(key.split("_")[:-1]) + key_to_check = key.split("_")[-1] + if key_to_check == "verb": + result["commands"].append(reverse_translate_verb_dict(value)) + result["command_params"].append({}) + elif key_to_check in [ + "object", + "location", + "gesture", + "room", + "name", + "start", + "end", + "objectcomp", + "clothes", + "talk", + ]: + value_to_add = value + try: + result["command_params"][-1][key_to_check] = value_to_add + except: + continue else: - write_into[key] = value + print(f"Unhandled key: {key_to_check}") return result -def gpsr_compile_and_parse(config: Configuration, input: str) -> dict: +def gpsr_compile_and_parse(config: Configuration, input: str) -> Dict: input = input.lower() # remove punctuation input = re.sub(r"[^\w\s]", "", input) - print(input) if input[0] == " ": input = input[1:] - + print(f"Parsed input: {input}") regex_str = gpsr_regex(config) regex = re.compile(regex_str) matches = regex.match(input) matches = matches.groupdict() - return gpsr_parse(matches) + object_categories = ( + config["object_categories_singular"] + config["object_categories_plural"] + ) + return parse_result_dict(gpsr_parse(matches), object_categories) + + +def parse_result_dict(result: Dict, object_categories: List[str]) -> Dict: + """Parses the result dictionary output by the gpsr parse to + handle missing parameters. + + Args: + result (dict): _description_ + + Returns: + dict: _description_ + """ + + for i, command in enumerate(result["commands"]): + if "object" in result["command_params"][i]: + if result["command_params"][i]["object"] in object_categories: + # rename object to object category + result["command_params"][i]["object_category"] = result[ + "command_params" + ][i]["object"] + del result["command_params"][i]["object"] + # Update command params based on the previous commands params + if i > 0: + if "location" not in result["command_params"][i]: + if "location" in result["command_params"][i - 1]: + result["command_params"][i]["location"] = result["command_params"][ + i - 1 + ]["location"] + if "room" not in result["command_params"][i]: + if "room" in result["command_params"][i - 1]: + result["command_params"][i - 1]["room"] = result["command_params"][ + i - 1 + ]["room"] + del result["command_params"][i]["room"] + if "name" not in result["command_params"][i]: + if "name" in result["command_params"][i - 1]: + result["command_params"][i]["name"] = result["command_params"][ + i - 1 + ]["name"] + if "object" not in result["command_params"][i]: + if "object" in result["command_params"][i - 1]: + result["command_params"][i]["object"] = result["command_params"][ + i - 1 + ]["object"] + + return result + + +def reverse_translate_verb_dict(verb: str) -> str: + for master_verb, verbs in verb_dict.items(): + if verb in verbs: + return master_verb + return verb if __name__ == "__main__": + object_categories_plural = ["sticks"] + object_categories_singular = ["stick"] + object_categories = object_categories_singular + object_categories_plural config: Configuration = { "person_names": ["guest1", "guest2"], - "location_names": ["sofa", "piano"], + "location_names": ["sofa", "piano", "kitchen table"], "placement_location_names": ["kitchen table"], "room_names": ["living room", "kitchen"], "object_names": ["cup", "television"], @@ -384,9 +510,47 @@ def gpsr_compile_and_parse(config: Configuration, input: str) -> dict: regex = re.compile(regex_str) - def execute(input: str): + def execute(input: str, object_categories: List[str]): matches = regex.match(input).groupdict() - return gpsr_parse(matches) + return parse_result_dict(gpsr_parse(matches), object_categories) + + print( + execute( + "go to the kitchen then meet guest1 and tell the time", + object_categories, + ) + ) - # subcommands aren't implemented but are caught: - print(execute("go to the sofa then do something here")) + # print( + # execute( + # "navigate to the kitchen then find a cup and get it and bring it to the person pointing to the right in the kitchen", + # object_categories, + # ) + # ) + + # print( + # execute( + # "navigate to the kitchen then find a cup and get it and bring it to me", + # object_categories, + # ) + # ) + # print( + # execute( + # "navigate to the kitchen table then find a stick and fetch it and deliver it to guest1 in the living room", + # object_categories, + # ) + # ) + + # print( + # execute( + # "lead the person wearing a red shirt from the sofa to the living room", + # object_categories, + # ) + # ) + + # print( + # execute( + # "tell me what is the biggest stick on the kitchen table", + # object_categories, + # ) + # ) diff --git a/tasks/receptionist/CMakeLists.txt b/tasks/receptionist/CMakeLists.txt index 512f6a5e6..85d832a1b 100644 --- a/tasks/receptionist/CMakeLists.txt +++ b/tasks/receptionist/CMakeLists.txt @@ -161,10 +161,10 @@ include_directories( ## Mark executable scripts (Python etc.) for installation ## in contrast to setup.py, you can choose the destination -# catkin_install_python(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) +catkin_install_python(PROGRAMS + scripts/main.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) ## Mark executables for installation ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html diff --git a/tasks/receptionist/README.md b/tasks/receptionist/README.md deleted file mode 100644 index 54796a8cd..000000000 --- a/tasks/receptionist/README.md +++ /dev/null @@ -1,88 +0,0 @@ -# lift - -The lift package - -This package is maintained by: -- [zoe](mailto:zoe.a.evans@kcl.ac.uk) - -## Prerequisites - -This package depends on the following ROS packages: -- catkin (buildtool) -- geometry_msgs (build) -- message_generation (build) -- roscpp (build) -- rospy (build) -- std_msgs (build) -- geometry_msgs (exec) -- roscpp (exec) -- rospy (exec) -- std_msgs (exec) - -Ask the package maintainer to write or create a blank `doc/PREREQUISITES.md` for their package! - -## Usage - -Ask the package maintainer to write a `doc/USAGE.md` for their package! - -## Example - -Ask the package maintainer to write a `doc/EXAMPLE.md` for their package! - -## Technical Overview - -Ask the package maintainer to write a `doc/TECHNICAL.md` for their package! - -## ROS Definitions - -### Launch Files - -#### `no_rasa` - -No description provided. - -| Argument | Default | Description | -|:-:|:-:|---| -| is_sim | false | | -| plot_show | false | | -| plot_save | true | | -| debug_with_images | true | | -| publish_markers | true | | -| debug | 3 | | -| rasa | true | | - - -#### `demo` - -No description provided. - -#### `setup` - -No description provided. - -| Argument | Default | Description | -|:-:|:-:|---| -| is_sim | false | | -| plot_show | false | | -| plot_save | true | | -| debug_with_images | true | | -| publish_markers | true | | -| debug | 3 | | -| rasa | true | | -| whisper_matcher | by-index | | -| whisper_device_param | 13 | | -| rasa_model | $(find lasr_rasa)/assistants/lift/models | | - - - -### Messages - -This package has no messages. - -### Services - -This package has no services. - -### Actions - -This package has no actions. diff --git a/tasks/receptionist/config/lab.yaml b/tasks/receptionist/config/lab.yaml new file mode 100644 index 000000000..a3adc5c93 --- /dev/null +++ b/tasks/receptionist/config/lab.yaml @@ -0,0 +1,42 @@ +priors: + names: + - Adel + - Angel + - Axel + - Charlie + - Jane + - Jules + - Morgan + - Paris + - Robin + - Simone + drinks: + - cola + - iced tea + - juice pack + - milk + - orange juice + - red wine + - tropical juice +wait_pose: + position: + x: 2.4307581363168773 + y: -1.661594410669659 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.012769969339563213 + w: 0.9999184606171978 +wait_area: [[2.65, -0.61], [4.21, -0.33], [4.58, -2.27], [2.67, -2.66]] +seat_pose: + position: + x: 1.1034954065916212 + y: 0.17802904565746552 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.816644293927375 + w: 0.577141314753899 +seat_area: [[-0.39, 0.87], [-0.74, 2.18], [1.26, 2.64], [1.54, 1.26]] diff --git a/tasks/receptionist/config/motions.yaml b/tasks/receptionist/config/motions.yaml index de45b5d18..06a719ab6 100644 --- a/tasks/receptionist/config/motions.yaml +++ b/tasks/receptionist/config/motions.yaml @@ -5,7 +5,7 @@ play_motion: points: - positions: [0.5, 0.0] time_from_start: 0.0 - look_center: + look_centre: joints: [head_1_joint, head_2_joint] points: - positions: [0.0, 0.0] @@ -20,7 +20,7 @@ play_motion: points: - positions: [0.5, -0.25] time_from_start: 0.0 - look_down_center: + look_down_centre: joints: [head_1_joint, head_2_joint] points: - positions: [0.0, -0.25] diff --git a/tasks/receptionist/config/receptionist_demo.yaml b/tasks/receptionist/config/receptionist_demo.yaml deleted file mode 100644 index 55c48730c..000000000 --- a/tasks/receptionist/config/receptionist_demo.yaml +++ /dev/null @@ -1,42 +0,0 @@ -start: - pose: - position: {x: 0.869912857238, y: 1.07249069916, z: 0.0} - orientation: {x: 0.0, y: 0.0, z: -0.629267081124, w: 0.777189127957} - - -host: - name: "zoe" - -guest1: - name: "unknown" - drink: "unknown" - -guest2: - name: "unknown" - drink: "unknown" - - -guestcount: - count: 0 - -wait_area: - polygon: [[1.94, 0.15], [2.98, 0.28], [3.08, -0.68], [2.06, -0.84]] - - -wait_position: - pose: - position: {x: 0.576111426292, y: -0.688977508097, z: 0.0} - orientation: {x: 0.0, y: 0.0, z: 0.16777977598, w: 0.985824501} - - -#Hardcoded, TODO remove: -talk_position: - pose: - position: {x: 1.21588332458, y: -0.507325055265, z: 0.0} - orientation: {x: 0.0, y: 0.0, z: 0.121772525848, w: 0.992558034549} - - -seating_area_position: - pose: - position: {x: 2.12474401462, y: -2.2094874081, z: 0.0} - orientation: {x: 0.0, y: 0.0, z: -0.527834895731, w: 0.849346997904} diff --git a/tasks/receptionist/launch/demo.launch b/tasks/receptionist/launch/demo.launch deleted file mode 100644 index 450e8fa6c..000000000 --- a/tasks/receptionist/launch/demo.launch +++ /dev/null @@ -1,5 +0,0 @@ - - - - - diff --git a/tasks/receptionist/launch/setup.launch b/tasks/receptionist/launch/setup.launch index d2b7a96e9..1669854f4 100644 --- a/tasks/receptionist/launch/setup.launch +++ b/tasks/receptionist/launch/setup.launch @@ -1,67 +1,20 @@ - - - - - - - - - - + + - - - - - - + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - + + - - - - - - - - - - - - - - - - + diff --git a/tasks/receptionist/scripts/main.py b/tasks/receptionist/scripts/main.py new file mode 100755 index 000000000..808952737 --- /dev/null +++ b/tasks/receptionist/scripts/main.py @@ -0,0 +1,46 @@ +#!/usr/bin/env python3 +import rospy +from receptionist.state_machine import Receptionist + +from geometry_msgs.msg import Pose, Point, Quaternion +from shapely.geometry import Polygon + +if __name__ == "__main__": + rospy.init_node("receptionist_robocup") + wait_pose_param = rospy.get_param("/receptionist/wait_pose") + wait_pose = 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) + + seat_pose_param = rospy.get_param("/receptionist/seat_pose") + seat_pose = Pose( + position=Point(**seat_pose_param["position"]), + orientation=Quaternion(**seat_pose_param["orientation"]), + ) + + seat_area_param = rospy.get_param("/receptionist/seat_area") + seat_area = Polygon(seat_area_param) + + receptionist = Receptionist( + wait_pose, + wait_area, + seat_pose, + seat_area, + { + "name": "John", + "drink": "beer", + "attributes": { + "hair_colour": "strawberry blonde", + "glasses": False, + "hat": True, + "height": "tall", + }, + }, + ) + outcome = receptionist.execute() + rospy.loginfo(f"Receptionist finished with outcome: {outcome}") + rospy.spin() diff --git a/tasks/receptionist/src/receptionist/__init__.py b/tasks/receptionist/src/receptionist/__init__.py index 396df9212..e69de29bb 100644 --- a/tasks/receptionist/src/receptionist/__init__.py +++ b/tasks/receptionist/src/receptionist/__init__.py @@ -1 +0,0 @@ -from .default import Default \ No newline at end of file diff --git a/tasks/receptionist/src/receptionist/default.py b/tasks/receptionist/src/receptionist/default.py deleted file mode 100644 index 0e70c83fc..000000000 --- a/tasks/receptionist/src/receptionist/default.py +++ /dev/null @@ -1,57 +0,0 @@ -#!/usr/bin/env python3 -from tiago_controllers.controllers import Controllers -from lasr_voice.voice import Voice -from lasr_vision_msgs.srv import YoloDetection -import rospy, actionlib -from tiago_controllers.controllers.base_controller import CmdVelController -from interaction_module.srv import AudioAndTextInteraction -from play_motion_msgs.msg import PlayMotionGoal, PlayMotionAction -from lasr_speech.srv import Speech -from tf_module.srv import BaseTransformRequest, ApplyTransformRequest, LatestTransformRequest, BaseTransform, \ - LatestTransform, ApplyTransform, TfTransform, TfTransformRequest - -from cv_bridge3 import CvBridge -from lasr_shapely import LasrShapely -from std_msgs.msg import Int16, Empty - - -rasa = True - -class Default: - def __init__(self): - rospy.loginfo("YOLO is here") - self.yolo = rospy.ServiceProxy('/yolov8/detect', YoloDetection) - rospy.loginfo("PM is here") - self.pm = actionlib.SimpleActionClient('/play_motion', PlayMotionAction) - rospy.loginfo("Controllers is here") - self.voice = Voice() - self.bridge = CvBridge() - rospy.loginfo("CV Bridge") - self.shapely = LasrShapely() - rospy.loginfo("Got shapely") - self.controllers = Controllers() - rospy.loginfo("CMD is here") - self.cmd = CmdVelController() - rospy.loginfo("Voice is here") - - self.tf = rospy.ServiceProxy('tf_transform', TfTransform) - print("TF is here") - self.tf_base = rospy.ServiceProxy('base_transform', BaseTransform) - self.tf_latest = rospy.ServiceProxy('latest_transform', LatestTransform) - self.tf_apply = rospy.ServiceProxy('apply_transform', ApplyTransform) - if rasa: - rospy.wait_for_service("/lasr_speech/transcribe_and_parse") - rospy.loginfo("SPEECH RASA is here") - self.speech = rospy.ServiceProxy("/lasr_speech/transcribe_and_parse", Speech) - else: - pass - rospy.loginfo("SPEECH Dialogflow is here") - self.speech = rospy.ServiceProxy("/interaction_module", AudioAndTextInteraction) - - if not rospy.get_published_topics(namespace='/pal_head_manager'): - rospy.loginfo("Is SIM ---> True") - rospy.set_param("/is_simulation", True) - else: - rospy.loginfo("Is SIM ---> FALSE") - rospy.set_param("/is_simulation", False) - diff --git a/tasks/receptionist/src/receptionist/defaults.py b/tasks/receptionist/src/receptionist/defaults.py deleted file mode 100755 index 0b0a3579f..000000000 --- a/tasks/receptionist/src/receptionist/defaults.py +++ /dev/null @@ -1,35 +0,0 @@ -#!/usr/bin/env python3 -import os, rospkg, shutil, rospy, sys - -rospy.loginfo("setting debug") -DEBUG = rospy.get_param('debug') # 3 -rospy.loginfo("setting Debug with images ") -DEBUG_WITH_IMAGES = rospy.get_param('debug_with_images') - - -# for matplotlib -rospy.loginfo("setting Plot SHOW ") -PLOT_SHOW = rospy.get_param('plot_show') -rospy.loginfo("setting Plot Save ") -PLOT_SAVE = rospy.get_param('plot_save') -rospy.loginfo("setting Publish Markers ") -PUBLISH_MARKERS = rospy.get_param('publish_markers') -rospy.loginfo("setting Debug Path ") -DEBUG_PATH = os.getcwd() -rospy.loginfo("setting Rasa ") -RASA = rospy.get_param('rasa') - -rospy.logwarn("DEBUG: {DEBUG}, DEBUG_WITH_IMAGES: {DEBUG_WITH_IMAGES}, PLOT_SHOW: {PLOT_SHOW}, PLOT_SAVE: {PLOT_SAVE}".format(**locals())) - -if DEBUG_WITH_IMAGES: - if not os.path.exists(os.path.join(rospkg.RosPack().get_path("receptionist"), "debug_receptionist")): - os.mkdir(os.path.join(rospkg.RosPack().get_path("receptionist"), "debug_receptionist")) - DEBUG_PATH = os.path.join(rospkg.RosPack().get_path("receptionist"), "debug_receptionist",) - if os.path.exists(DEBUG_PATH): - shutil.rmtree(DEBUG_PATH) - os.mkdir(DEBUG_PATH) - - -# for plots in waypoitns -import random -TEST = random.randint(0, 1000) \ No newline at end of file diff --git a/tasks/receptionist/src/receptionist/main.py b/tasks/receptionist/src/receptionist/main.py deleted file mode 100755 index 4d7e81fbc..000000000 --- a/tasks/receptionist/src/receptionist/main.py +++ /dev/null @@ -1,12 +0,0 @@ -#!/usr/bin/env python3 -import rospy -from receptionist.sm import Receptionist - -if __name__ == "__main__": - rospy.init_node("receptionist_robocup") - receptionist = Receptionist() - outcome = receptionist.execute() - print(outcome) - rospy.spin() - - \ No newline at end of file diff --git a/tasks/receptionist/src/receptionist/sm.py b/tasks/receptionist/src/receptionist/sm.py deleted file mode 100755 index b9cfe1b2f..000000000 --- a/tasks/receptionist/src/receptionist/sm.py +++ /dev/null @@ -1,31 +0,0 @@ -#!/usr/bin/env python3 -import smach -from receptionist.states import Start -from receptionist.states import AskForName -from receptionist.states import AskForDrink -from receptionist.states import End -from receptionist import Default -from receptionist.states import GoToPerson -from receptionist.states import GoToSeatingArea -from receptionist.states import LookForSeats -from receptionist.states import GoToWaitForPerson -from lasr_skills import WaitForPersonInArea -#from receptionist.states.end import End - - -class Receptionist(smach.StateMachine): - def __init__(self): - smach.StateMachine.__init__(self, outcomes=['succeeded', 'failed']) - self.default = Default() - self.userdata.area_polygon = [[1.94, 0.15], [2.98, 0.28], [3.08, -0.68], [2.06, -0.84]] - self.userdata.depth_topic = "/xtion/depth_registered/points" - with self: - smach.StateMachine.add('START', Start(self.default), transitions={'succeeded' : 'GO_TO_WAIT_FOR_PERSON'}) - smach.StateMachine.add("GO_TO_WAIT_FOR_PERSON",GoToWaitForPerson(self.default), transitions={'succeeded': 'WAIT_FOR_PERSON'}) - smach.StateMachine.add('WAIT_FOR_PERSON', WaitForPersonInArea() ,transitions={'succeeded' : 'GO_TO_PERSON', 'failed' : 'failed'}) - smach.StateMachine.add('GO_TO_PERSON', GoToPerson(self.default),transitions={'succeeded':'ASK_FOR_NAME'}) - smach.StateMachine.add('ASK_FOR_NAME', AskForName(self.default),transitions={'failed':'ASK_FOR_NAME','succeeded':'ASK_FOR_DRINK'}) - smach.StateMachine.add('ASK_FOR_DRINK', AskForDrink(self.default),transitions={'failed':'ASK_FOR_DRINK','succeeded':'GO_TO_SEATING_AREA'}) - smach.StateMachine.add('GO_TO_SEATING_AREA', GoToSeatingArea(self.default), transitions={'succeeded' : 'LOOK_FOR_SEATS'}) - smach.StateMachine.add('LOOK_FOR_SEATS', LookForSeats(self.default), transitions={'succeeded' : 'GO_TO_WAIT_FOR_PERSON'}) - smach.StateMachine.add('END', End(self.default),transitions={'succeeded':'succeeded'}) diff --git a/tasks/receptionist/src/receptionist/speech_helper.py b/tasks/receptionist/src/receptionist/speech_helper.py deleted file mode 100755 index 40878e807..000000000 --- a/tasks/receptionist/src/receptionist/speech_helper.py +++ /dev/null @@ -1,49 +0,0 @@ -import rospy -import json - - -def listen(default): - print("trying to listen!") - resp = default.speech(True) - print("Resp success: ", resp.success) - if not resp.success: - default.voice.speak("Sorry, I didn't get that") - return listen(default) - resp = json.loads(resp.json_response) - rospy.loginfo(resp) - return resp - -def affirm(default): - resp = listen(default) - if resp['intent']['name'] != 'affirm': - default.voice.speak("Sorry, I didn't get that, please say yes or no") - return affirm(default) - choices = resp["entities"].get("choice", None) - if choices is None: - default.voice.speak("Sorry, I didn't get that") - return affirm(default) - choice = choices[0]["value"].lower() - if choice not in ["yes", "no"]: - default.voice.speak("Sorry, I didn't get that") - return affirm(default) - return choice - -def get_drink(default): - resp = listen(default) - # if resp['intent']['name'] != 'fav_drink': - # return "unknown" - drink = resp["entities"].get("drink",[]) - if drink is None or not drink: - return "unknown" - drink = drink[0]["value"].lower() - return str(drink) - -def get_name(default): - resp = listen(default) - # if resp['intent']['name'] != 'name': - # return "unknown" - name = resp["entities"].get("name",[]) - if name is None or not name: - return "unknown" - name = name[0]["value"].lower() - return str(name) \ No newline at end of file diff --git a/tasks/receptionist/src/receptionist/state_machine.py b/tasks/receptionist/src/receptionist/state_machine.py new file mode 100755 index 000000000..05e038bfb --- /dev/null +++ b/tasks/receptionist/src/receptionist/state_machine.py @@ -0,0 +1,284 @@ +import smach + +from geometry_msgs.msg import Pose +from shapely.geometry import Polygon +from lasr_skills import GoToLocation, WaitForPersonInArea, Say, AskAndListen +from receptionist.states import ( + ParseNameAndDrink, + GetGuestAttributes, + Introduce, + SeatGuest, +) + + +class Receptionist(smach.StateMachine): + + def __init__( + self, + wait_pose: Pose, + wait_area: Polygon, + seat_pose: Pose, + seat_area: Polygon, + host_data: dict, + ): + + smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"]) + + with self: + + self.userdata.guest_data = {"host": host_data, "guest1": {}, "guest2": {}} + + smach.StateMachine.add( + "GO_TO_WAIT_LOCATION_GUEST_1", + GoToLocation(wait_pose), + transitions={ + "succeeded": "SAY_WAITING_GUEST_1", + "failed": "failed", + }, + ) + + smach.StateMachine.add( + "SAY_WAITING_GUEST_1", + Say(text="I am waiting for a guest."), + transitions={ + "succeeded": "WAIT_FOR_PERSON_GUEST_1", + "aborted": "failed", + "preempted": "failed", + }, + ) + + smach.StateMachine.add( + "WAIT_FOR_PERSON_GUEST_1", + WaitForPersonInArea(wait_area), + transitions={ + "succeeded": "GET_NAME_AND_DRINK_GUEST_1", + "failed": "failed", + }, + ) + + smach.StateMachine.add( + "GET_NAME_AND_DRINK_GUEST_1", + AskAndListen("What is your name and favourite drink?"), + transitions={ + "succeeded": "PARSE_NAME_AND_DRINK_GUEST_1", + "failed": "failed", + }, + ) + + smach.StateMachine.add( + "PARSE_NAME_AND_DRINK_GUEST_1", + ParseNameAndDrink("guest1"), + transitions={ + "succeeded": "GET_GUEST_ATTRIBUTES_GUEST_1", + "failed": "failed", + }, + remapping={"guest_transcription": "transcribed_speech"}, + ) + + smach.StateMachine.add( + "GET_GUEST_ATTRIBUTES_GUEST_1", + GetGuestAttributes("guest1"), + transitions={ + "succeeded": "SAY_FOLLOW_GUEST_1", + "failed": "failed", + }, + ) + + smach.StateMachine.add( + "SAY_FOLLOW_GUEST_1", + Say(text="Please follow me, I will guide you to the other guests"), + transitions={ + "succeeded": "GO_TO_SEAT_LOCATION_GUEST_1", + "preempted": "failed", + "aborted": "failed", + }, + ) + + smach.StateMachine.add( + "GO_TO_SEAT_LOCATION_GUEST_1", + GoToLocation(seat_pose), + transitions={ + "succeeded": "SAY_WAIT_GUEST_1", + "failed": "failed", + }, + ) + + smach.StateMachine.add( + "SAY_WAIT_GUEST_1", + Say(text="Please wait here on my left"), + transitions={ + "succeeded": "INTRODUCE_GUEST_1_TO_HOST", + "preempted": "failed", + "aborted": "failed", + }, + ) + + smach.StateMachine.add( + "INTRODUCE_GUEST_1_TO_HOST", + Introduce(guest_to_introduce="guest1", guest_to_introduce_to="host"), + transitions={ + "succeeded": "INTRODUCE_HOST_TO_GUEST_1", + "failed": "failed", + }, + ) + + smach.StateMachine.add( + "INTRODUCE_HOST_TO_GUEST_1", + Introduce(guest_to_introduce="host", guest_to_introduce_to="guest1"), + transitions={ + "succeeded": "SEAT_GUEST_1", + "failed": "failed", + }, + ) + + smach.StateMachine.add( + "SEAT_GUEST_1", + SeatGuest(seat_area), + transitions={ + "succeeded": "GO_TO_WAIT_LOCATION_GUEST_2", + "failed": "failed", + }, + ) + + """ + Guest 2 + """ + + smach.StateMachine.add( + "GO_TO_WAIT_LOCATION_GUEST_2", + GoToLocation(wait_pose), + transitions={ + "succeeded": "SAY_WAITING_GUEST_2", + "failed": "failed", + }, + ) + + smach.StateMachine.add( + "SAY_WAITING_GUEST_2", + Say(text="I am waiting for a guest."), + transitions={ + "succeeded": "WAIT_FOR_PERSON_GUEST_2", + "aborted": "failed", + "preempted": "failed", + }, + ) + + smach.StateMachine.add( + "WAIT_FOR_PERSON_GUEST_2", + WaitForPersonInArea(wait_area), + transitions={ + "succeeded": "GET_NAME_AND_DRINK_GUEST_2", + "failed": "failed", + }, + ) + + smach.StateMachine.add( + "GET_NAME_AND_DRINK_GUEST_2", + AskAndListen("What is your name and favourite drink?"), + transitions={ + "succeeded": "PARSE_NAME_AND_DRINK_GUEST_2", + "failed": "failed", + }, + ) + + smach.StateMachine.add( + "PARSE_NAME_AND_DRINK_GUEST_2", + ParseNameAndDrink("guest2"), + transitions={ + "succeeded": "GET_GUEST_ATTRIBUTES_GUEST_2", + "failed": "failed", + }, + remapping={"guest_transcription": "transcribed_speech"}, + ) + + smach.StateMachine.add( + "GET_GUEST_ATTRIBUTES_GUEST_2", + GetGuestAttributes("guest2"), + transitions={ + "succeeded": "SAY_FOLLOW_GUEST_2", + "failed": "failed", + }, + ) + + smach.StateMachine.add( + "SAY_FOLLOW_GUEST_2", + Say(text="Please follow me, I will guide you to the other guests"), + transitions={ + "succeeded": "GO_TO_SEAT_LOCATION_GUEST_2", + "preempted": "failed", + "aborted": "failed", + }, + ) + + smach.StateMachine.add( + "GO_TO_SEAT_LOCATION_GUEST_2", + GoToLocation(seat_pose), + transitions={ + "succeeded": "SAY_WAIT_GUEST_2", + "failed": "failed", + }, + ) + + smach.StateMachine.add( + "SAY_WAIT_GUEST_2", + Say(text="Please wait here on my left"), + transitions={ + "succeeded": "INTRODUCE_GUEST_2_TO_EVERYONE", + "preempted": "failed", + "aborted": "failed", + }, + ) + + smach.StateMachine.add( + "INTRODUCE_GUEST_2_TO_EVERYONE", + Introduce(guest_to_introduce="guest2", everyone=True), + transitions={ + "succeeded": "INTRODUCE_HOST_TO_GUEST_2", + "failed": "failed", + }, + ) + + smach.StateMachine.add( + "INTRODUCE_HOST_TO_GUEST_2", + Introduce(guest_to_introduce="host", guest_to_introduce_to="guest2"), + transitions={ + "succeeded": "INTRODUCE_GUEST_1_TO_GUEST_2", + "failed": "failed", + }, + ) + + smach.StateMachine.add( + "INTRODUCE_GUEST_1_TO_GUEST_2", + Introduce(guest_to_introduce="guest1", guest_to_introduce_to="guest2"), + transitions={ + "succeeded": "SEAT_GUEST_2", + "failed": "failed", + }, + ) + + smach.StateMachine.add( + "SEAT_GUEST_2", + SeatGuest(seat_area), + transitions={"succeeded": "GO_TO_FINISH_LOCATION", "failed": "failed"}, + ) + + """ + Finish + """ + smach.StateMachine.add( + "GO_TO_FINISH_LOCATION", + GoToLocation(wait_pose), + transitions={ + "succeeded": "SAY_FINISHED", + "failed": "failed", + }, + ) + smach.StateMachine.add( + "SAY_FINISHED", + Say(text="I am done."), + transitions={ + "succeeded": "succeeded", + "aborted": "failed", + "preempted": "failed", + }, + ) diff --git a/tasks/receptionist/src/receptionist/states/__init__.py b/tasks/receptionist/src/receptionist/states/__init__.py index cc177733f..3a25c381c 100644 --- a/tasks/receptionist/src/receptionist/states/__init__.py +++ b/tasks/receptionist/src/receptionist/states/__init__.py @@ -1,8 +1,4 @@ -from .ask_for_name import AskForName -from .ask_for_drink import AskForDrink -from .end import End -from .go_to_person import GoToPerson -from .go_to_seating_area import GoToSeatingArea -from .go_to_wait_for_person import GoToWaitForPerson -from .look_for_seats import LookForSeats -from .start import Start \ No newline at end of file +from .get_name_and_drink import ParseNameAndDrink +from .get_attributes import GetGuestAttributes +from .introduce import Introduce +from .seat_guest import SeatGuest diff --git a/tasks/receptionist/src/receptionist/states/ask_for_drink.py b/tasks/receptionist/src/receptionist/states/ask_for_drink.py deleted file mode 100644 index e35f3a38d..000000000 --- a/tasks/receptionist/src/receptionist/states/ask_for_drink.py +++ /dev/null @@ -1,35 +0,0 @@ -import smach -import rospy -from receptionist.speech_helper import get_drink - -class AskForDrink(smach.State): - def __init__(self, default): - smach.State.__init__(self, outcomes=['succeeded','failed']) - self.default = default - - - def execute(self, userdata): - - guestcount = rospy.get_param("guestcount/count", 0) - - self.default.voice.speak("What is your favourite drink?") - - - for _ in range(3): - try: - drink = get_drink(self.default) - except: - drink = "unknown " - if drink == "unknown": - self.default.voice.speak("Sorry, I didn't get that. Could you repeat, please?") - else: - break - - if drink == "unknown": - self.default.voice.speak("Sadly, I couldn't understand your favourite drink. You might go thirsty this evening.") - else: - self.default.voice.speak(f"{drink}, what a great drink!") - - rospy.set_param(f"guest{guestcount+1}/drink", drink) - rospy.set_param("guestcount/count", guestcount+1) - return 'succeeded' diff --git a/tasks/receptionist/src/receptionist/states/ask_for_name.py b/tasks/receptionist/src/receptionist/states/ask_for_name.py deleted file mode 100644 index 3c2f1dcf4..000000000 --- a/tasks/receptionist/src/receptionist/states/ask_for_name.py +++ /dev/null @@ -1,33 +0,0 @@ -import smach -import rospy -from receptionist.speech_helper import get_name - -class AskForName(smach.State): - def __init__(self, default): - smach.State.__init__(self, outcomes=['succeeded','failed']) - self.default = default - - - def execute(self, userdata): - - guestcount = rospy.get_param("guestcount/count", 0) - - self.default.voice.speak("What is your name?") - - for _ in range(3): - try: - name = get_name(self.default) - except: - name = "unknown" - if name == "unknown": - self.default.voice.speak("Sorry, I didn't get that. Could you repeat, please?") - else: - break - - if name == "unknown": - self.default.voice.speak("I couldn't understand your name, but it's great to meet you!") - else: - self.default.voice.speak(f"It's great to meet you {name}!") - - rospy.set_param(f"guest{guestcount+1}/name", name) - return 'succeeded' diff --git a/tasks/receptionist/src/receptionist/states/end.py b/tasks/receptionist/src/receptionist/states/end.py deleted file mode 100644 index 3d350a00d..000000000 --- a/tasks/receptionist/src/receptionist/states/end.py +++ /dev/null @@ -1,20 +0,0 @@ -import smach -import rospy -from tiago_controllers.helpers.pose_helpers import get_pose_from_param - - -class End(smach.State): - def __init__(self, default): - smach.State.__init__(self, outcomes=['succeeded']) - self.default = default - - def execute(self, userdata): - guest1name = rospy.get_param('guest1/name') - guest1drink = rospy.get_param('guest1/drink') - guest2drink = rospy.get_param('guest2/drink') - guest2name = rospy.get_param('guest2/name') - - self.default.voice.speak(f"{guest1name}'s favourite drink was {guest1drink}") - self.default.voice.speak(f"{guest2name}'s favourite drink was {guest2drink}") - - return 'succeeded' \ No newline at end of file diff --git a/tasks/receptionist/src/receptionist/states/get_attributes.py b/tasks/receptionist/src/receptionist/states/get_attributes.py new file mode 100644 index 000000000..22538a267 --- /dev/null +++ b/tasks/receptionist/src/receptionist/states/get_attributes.py @@ -0,0 +1,103 @@ +""" +State for calling the service to get a set of guest attributes. +Currently incomplete. +""" + +import rospy +import smach +from smach import UserData +from typing import List, Any, Dict, Union +from lasr_vision_clip.srv import VqaRequest, VqaResponse, Vqa + + +class GetGuestAttributes(smach.State): + def __init__( + self, + guest_id: str, + attribute_service: Union[str, None] = None, + outcomes: List[str] = ["succeeded", "failed"], + input_keys: List[str] = ["guest_id", "guest_data"], + output_keys: List[str] = ["guest_data"], + ): + """Calls and parses the service that gets a set of guest attributes. + + Args: + attribute_service (str): Name of the service to call that returns the guest's attributes. + """ + + super().__init__( + outcomes=outcomes, + input_keys=input_keys, + output_keys=output_keys, + ) + self._service_proxy = rospy.ServiceProxy("/clip_vqa/query_service", Vqa) + self._guest_id: str = guest_id + self._attribute_service: Union[str, None] = attribute_service + + def _call_attribute_service(self): + # TODO + pass + + def _send_vqa_request(self, possible_answers: List[str]) -> str: + request = VqaRequest() + request.possible_answers = possible_answers + response = self._service_proxy(request) + return response.answer + + def execute(self, userdata: UserData) -> str: + if self._attribute_service: + attributes = self._call_attribute_service() + else: + glasses_answers = [ + "a person wearing glasses", + "a person not wearing glasses", + ] + hat_answers = ["a person wearing a hat", "a person not wearing a hat"] + height_answers = ["a tall person", "a short person"] + hair_colour_answers = [ + "a person with black hair", + "a person with blonde hair", + "a person with brown hair", + "a person with red hair", + "a person with grey hair", + "a person with white hair", + ] + + glasses_response = self._send_vqa_request(glasses_answers) + hat_response = self._send_vqa_request(hat_answers) + height_response = self._send_vqa_request(height_answers) + hair_colour_response = self._send_vqa_request(hair_colour_answers) + + if glasses_response == "a person wearing glasses": + glasses = True + else: + glasses = False + if hat_response == "a person wearing a hat": + hat = True + else: + hat = False + if height_response == "a tall person": + height = "tall" + else: + height = "short" + if hair_colour_response == "a person with black hair": + hair_colour = "black" + elif hair_colour_response == "a person with blonde hair": + hair_colour = "blonde" + elif hair_colour_response == "a person with brown hair": + hair_colour = "brown" + elif hair_colour_response == "a person with red hair": + hair_colour = "red" + elif hair_colour_response == "a person with grey hair": + hair_colour = "grey" + + attributes = { + "hair_colour": hair_colour, + "glasses": glasses, + "hat": hat, + "height": height, + } + + userdata.guest_data[self._guest_id]["attributes"] = attributes + + return "succeeded" diff --git a/tasks/receptionist/src/receptionist/states/get_name_and_drink.py b/tasks/receptionist/src/receptionist/states/get_name_and_drink.py new file mode 100644 index 000000000..e2da4fede --- /dev/null +++ b/tasks/receptionist/src/receptionist/states/get_name_and_drink.py @@ -0,0 +1,78 @@ +""" +State for parsing the transcription of the guests' name and favourite drink, and adding this +to the guest data userdata +""" + +import rospy +import smach +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__( + self, + outcomes=["succeeded", "failed"], + 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 + print(userdata) + print(type(userdata.guest_transcription)) + 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") + outcome = "failed" + if not drink_found: + rospy.loginfo("Drink not found in transcription") + outcome = "failed" + + return outcome diff --git a/tasks/receptionist/src/receptionist/states/go_to_person.py b/tasks/receptionist/src/receptionist/states/go_to_person.py deleted file mode 100644 index cd00aff0b..000000000 --- a/tasks/receptionist/src/receptionist/states/go_to_person.py +++ /dev/null @@ -1,15 +0,0 @@ -import smach -import rospy -from tiago_controllers.helpers.pose_helpers import get_pose_from_param - - -class GoToPerson(smach.State): - def __init__(self, default): - smach.State.__init__(self, outcomes=['succeeded']) - self.default = default - - def execute(self, userdata): - self.default.voice.speak("Going to person") - res = self.default.controllers.base_controller.ensure_sync_to_pose(get_pose_from_param('/talk_position/pose')) - rospy.logerr(res) - return 'succeeded' diff --git a/tasks/receptionist/src/receptionist/states/go_to_seating_area.py b/tasks/receptionist/src/receptionist/states/go_to_seating_area.py deleted file mode 100644 index 8e4cd87c2..000000000 --- a/tasks/receptionist/src/receptionist/states/go_to_seating_area.py +++ /dev/null @@ -1,16 +0,0 @@ -import smach -import rospy -from tiago_controllers.helpers.pose_helpers import get_pose_from_param - - -class GoToSeatingArea(smach.State): - def __init__(self, default): - smach.State.__init__(self, outcomes=['succeeded']) - self.default = default - - def execute(self, userdata): - self.default.voice.speak("Going to seating area") - res = self.default.controllers.base_controller.ensure_sync_to_pose(get_pose_from_param('/seating_area_position/pose')) - rospy.logerr(res) - - return 'succeeded' diff --git a/tasks/receptionist/src/receptionist/states/go_to_wait_for_person.py b/tasks/receptionist/src/receptionist/states/go_to_wait_for_person.py deleted file mode 100644 index 951015884..000000000 --- a/tasks/receptionist/src/receptionist/states/go_to_wait_for_person.py +++ /dev/null @@ -1,16 +0,0 @@ -import smach -import rospy -from tiago_controllers.helpers.pose_helpers import get_pose_from_param - - -class GoToWaitForPerson(smach.State): - def __init__(self, default): - smach.State.__init__(self, outcomes=['succeeded']) - self.default = default - - def execute(self, userdata): - self.default.voice.speak("I will wait for a person") - res = self.default.controllers.base_controller.ensure_sync_to_pose(get_pose_from_param('/wait_position/pose')) - rospy.logerr(res) - - return 'succeeded' diff --git a/tasks/receptionist/src/receptionist/states/introduce.py b/tasks/receptionist/src/receptionist/states/introduce.py new file mode 100644 index 000000000..cedb54740 --- /dev/null +++ b/tasks/receptionist/src/receptionist/states/introduce.py @@ -0,0 +1,149 @@ +""" +State machine that introduces the greeted guest to all other guests/host present in the +seating area. + +""" + +import rospy +import smach +from smach import UserData +from lasr_skills import Say +from typing import Dict, Any, Optional + + +def stringify_guest_data(guest_data: Dict[str, Any], guest_id: str) -> 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. + + Args: + guest_data (Dict[str, Any]): guest data dictionary. + guest_id (str): guest id key to use to get the guest data. + + Returns: + str: string representation of the guest data. + """ + + relevant_guest_data = guest_data[guest_id] + + guest_str = "" + + guest_str += f"{relevant_guest_data['name']}, their favourite drink is {relevant_guest_data['drink']}. " + guest_str += f"They have {relevant_guest_data['attributes']['hair_colour']} hair, their height is {relevant_guest_data['attributes']['height']}, " + guest_str += f"they are {'wearing glasses' if relevant_guest_data['attributes']['glasses'] else 'not wearing glasses'}, and they are " + guest_str += f"{'wearing a hat' if relevant_guest_data['attributes']['hat'] else 'not wearing a hat'}." + + return guest_str + + +class GetStrGuestData(smach.State): + def __init__(self, guest_id: str): + super().__init__( + outcomes=["succeeded"], + input_keys=["guest_data"], + output_keys=["guest_str"], + ) + self._guest_id = guest_id + + def execute(self, userdata: UserData) -> str: + guest_str = stringify_guest_data(userdata.guest_data, self._guest_id) + userdata.guest_str = guest_str + return "succeeded" + + +class GetGuestName(smach.State): + def __init__(self, guest_id: str): + super().__init__( + outcomes=["succeeded"], + input_keys=["guest_data"], + output_keys=["requested_name"], + ) + self._guest_id = guest_id + + def execute(self, userdata: UserData) -> str: + requested_name = userdata.guest_data[self._guest_id]["name"] + userdata.requested_name = requested_name + return "succeeded" + + +class GetIntroductionString(smach.State): + def __init__(self): + super().__init__( + outcomes=["succeeded"], + input_keys=["guest_str", "requested_name"], + output_keys=["introduction_str"], + ) + + def execute(self, userdata: UserData) -> str: + introduction_str = ( + f"Hello {userdata.requested_name}, this is {userdata.guest_str}." + ) + userdata.introduction_str = introduction_str + return "succeeded" + + +class Introduce(smach.StateMachine): + def __init__( + self, + guest_to_introduce: str, + guest_to_introduce_to: Optional[str] = None, + everyone: Optional[bool] = False, + ): + super().__init__( + outcomes=["succeeded", "failed"], + input_keys=["guest_data"], + ) + assert not (guest_to_introduce_to is None and not everyone) + + with self: + if everyone: + smach.StateMachine.add( + "GetStrGuestData", + GetStrGuestData(guest_id=guest_to_introduce), + transitions={"succeeded": "SayIntroduce"}, + ) + smach.StateMachine.add( + "SayIntroduce", + Say( + format_str="Hello everyone, this is {}.", + ), + transitions={ + "succeeded": "succeeded", + "preempted": "failed", + "aborted": "failed", + }, + remapping={"placeholders": "guest_str"}, + ) + + else: + smach.StateMachine.add( + "GetStrGuestData", + GetStrGuestData(guest_id=guest_to_introduce), + transitions={"succeeded": "GetGuestName"}, + ) + + smach.StateMachine.add( + "GetGuestName", + GetGuestName(guest_id=guest_to_introduce_to), + transitions={"succeeded": "GetIntroductionString"}, + ) + + smach.StateMachine.add( + "GetIntroductionString", + GetIntroductionString(), + transitions={"succeeded": "SayIntroduce"}, + remapping={ + "guest_str": "guest_str", + "requested_name": "requested_name", + }, + ) + + smach.StateMachine.add( + "SayIntroduce", + Say(), + transitions={ + "succeeded": "succeeded", + "preempted": "failed", + "aborted": "failed", + }, + remapping={"text": "introduction_str"}, + ) diff --git a/tasks/receptionist/src/receptionist/states/look_for_seats.py b/tasks/receptionist/src/receptionist/states/look_for_seats.py deleted file mode 100755 index aea147463..000000000 --- a/tasks/receptionist/src/receptionist/states/look_for_seats.py +++ /dev/null @@ -1,142 +0,0 @@ -#!/usr/bin/env python3 - -import smach -import rospy -import numpy as np -import math -from play_motion_msgs.msg import PlayMotionGoal -from geometry_msgs.msg import Point -from shapely.geometry import Polygon -from lasr_skills import DetectObjects3D, LookToPoint -from copy import copy - -class LookForSeats(smach.StateMachine): - - - class Look(smach.State): - - def __init__(self, default): - smach.State.__init__(self, outcomes=['done', 'not_done'], input_keys=['look_motion']) - self.default = default - self.motions = ['look_down_left', 'look_down_center', 'look_down_right'] - self.remaining_motions = copy(self.motions) - - def execute(self, userdata): - if not self.remaining_motions: - self.remaining_motions = copy(self.motions) - return 'done' - pm_goal = PlayMotionGoal(motion_name=self.motions.pop(0), skip_planning=True) - self.default.pm.send_goal_and_wait(pm_goal) - rospy.sleep(1.0) - return 'not_done' - - class ProcessDetections(smach.State): - - def __init__(self): - smach.State.__init__(self, outcomes=['succeeded'], input_keys=['detections_3d', 'bulk_people_detections_3d', 'bulk_seat_detections_3d'], output_keys=['bulk_people_detections_3d', 'bulk_seat_detections3d']) - - def execute(self, userdata): - for det in userdata.detections_3d: - if det[0].name == "person": - userdata.bulk_people_detections_3d.append(det) - else: - userdata.bulk_seat_detections_3d.append(det) - return 'succeeded' - - class CheckSeat(smach.State): - def __init__(self, default): - smach.State.__init__(self, outcomes=['not_done', 'succeeded', 'failed'],input_keys=['detections_3d', 'bulk_people_detections_3d', 'bulk_seat_detections_3d'], output_keys=['free_seat', 'bulk_people_detections_3d', 'bulk_seat_detections3d', 'point']) - self.default = default - - def is_person_sitting_in_chair(self, person_contours, chair_contours): - return Polygon(person_contours).intersects(Polygon(chair_contours)) - - def execute(self, userdata): - if not userdata.bulk_seat_detections_3d: - return 'failed' - det1, p1 = userdata.bulk_seat_detections_3d.pop(0) - if not userdata.bulk_people_detections_3d: - userdata.free_seat = [det1, p1] - userdata.point = Point(*p1) - print(f"Chair at {p1} is free!") - robot_x, robot_y, _ = self.default.controllers.base_controller.get_current_pose() - r = np.array([robot_x, robot_y]) - p = np.array([p1[0], p1[1]]) - theta = np.degrees(np.arccos(np.dot(r, p) / (np.linalg.norm(r) * np.linalg.norm(p)))) - print(theta) - if theta > 10.0: - self.default.voice.sync_tts("Please sit down on this chair to my right") - elif theta < -10.0: - self.default.voice.sync_tts("Please sit down on this chair to my left") - else: - self.default.voice.sync_tts("Please sit down on this chair") - self.remaining_motions = [] - return 'succeeded' - for (det2, _) in userdata.bulk_people_detections_3d: - if not self.is_person_sitting_in_chair(np.array(det2.xyseg).reshape(-1, 2), np.array(det1.xyseg).reshape(-1, 2)): - userdata.free_seat = [det1, p1] - userdata.point = Point(*p1) - print(f"Chair at {p1} is free!") - robot_x, robot_y, _ = self.default.controllers.base_controller.get_current_pose() - r = np.array([robot_x, robot_y]) - p = np.array([p1[0], p1[1]]) - theta = np.degrees(np.arccos(np.dot(r, p) / (np.linalg.norm(r) * np.linalg.norm(p)))) - print(theta) - if theta > 10.0: - self.default.voice.sync_tts("Please sit down on this chair to my right") - elif theta < -10.0: - self.default.voice.sync_tts("Please sit down on this chair to my left") - else: - self.default.voice.sync_tts("Please sit down on this chair") - self.remaining_motions = [] - return 'succeeded' - return 'not_done' - - class PointToChair(smach.State): - - def __init__(self, default): - smach.State.__init__(self, outcomes=['succeeded'], input_keys=['point']) - self.default = default - - def execute(self, userdata): - - pm_goal = PlayMotionGoal(motion_name="back_to_default_head", skip_planning=True) - self.default.pm.send_goal_and_wait(pm_goal) - - self.default.controllers.base_controller.sync_face_to(userdata.point.x, userdata.point.y) - - pm_goal = PlayMotionGoal(motion_name="raise_torso", skip_planning=True) - self.default.pm.send_goal_and_wait(pm_goal) - - pm_goal = PlayMotionGoal(motion_name="point", skip_planning=False) - self.default.pm.send_goal_and_wait(pm_goal) - - rospy.sleep(5.0) - - pm_goal = PlayMotionGoal(motion_name="home", skip_planning=False) - self.default.pm.send_goal_and_wait(pm_goal) - - return 'succeeded' - - - def __init__(self, default): - smach.StateMachine.__init__(self, outcomes=['succeeded', 'failed']) - self.default = default - self.userdata.filter = ["person", "chair"] - self.userdata.bulk_people_detections_3d = [] - self.userdata.bulk_seat_detections_3d = [] - self.userdata.depth_topic = "xtion/depth_registered/points" - - with self: - smach.StateMachine.add('LOOK', self.Look(self.default), transitions={'not_done' : 'DETECT_OBJECTS_3D', 'done' : 'CHECK_SEAT'}) - smach.StateMachine.add('DETECT_OBJECTS_3D', DetectObjects3D(), transitions={'succeeded' : 'PROCESS_DETECTIONS', 'failed' : 'failed'}) - smach.StateMachine.add('PROCESS_DETECTIONS', self.ProcessDetections(), transitions={'succeeded' : 'LOOK'}) - smach.StateMachine.add('CHECK_SEAT', self.CheckSeat(self.default), transitions={'succeeded' : 'FINALISE_SEAT', 'failed' : 'failed', 'not_done': 'CHECK_SEAT'}) - smach.StateMachine.add('FINALISE_SEAT', self.PointToChair(self.default), transitions={'succeeded' : 'succeeded'}) - -if __name__ == "__main__": - from receptionist import Default - rospy.init_node("test_look_for_seats") - default = Default() - sm = LookForSeats(default) - sm.execute() \ No newline at end of file diff --git a/tasks/receptionist/src/receptionist/states/seat_guest.py b/tasks/receptionist/src/receptionist/states/seat_guest.py new file mode 100755 index 000000000..4090ea318 --- /dev/null +++ b/tasks/receptionist/src/receptionist/states/seat_guest.py @@ -0,0 +1,148 @@ +import smach + +from typing import List +from shapely.geometry import Polygon + +import numpy as np + +from lasr_skills import PlayMotion, Detect3DInArea, LookToPoint, Say + + +class SeatGuest(smach.StateMachine): + + _motions: List[str] = ["look_down_left", "look_down_right", "look_down_centre"] + + class ProcessDetections(smach.State): + + def __init__(self): + smach.State.__init__( + self, + outcomes=["succeeded", "failed"], + input_keys=[ + "detections_3d", + ], + output_keys=["seat_position"], + ) + + def execute(self, userdata) -> str: + seat_detections = [ + det for det in userdata.detections_3d if det.name == "chair" + ] + person_detections = [ + det for det in userdata.detections_3d if det.name == "person" + ] + + person_polygons: List[Polygon] = [ + Polygon(np.array(person.xyseg).reshape(-1, 2)) + for person in person_detections + ] + + print( + f"There are {len(seat_detections)} seats and {len(person_detections)} people." + ) + + for seat in seat_detections: + + seat_polygon: Polygon = Polygon(np.array(seat.xyseg).reshape(-1, 2)) + seat_is_empty: bool = True + for person_polygon in person_polygons: + if person_polygon.intersects(seat_polygon): + seat_is_empty = False + print(person_polygon.intersection(seat_polygon)) + break + + if seat_is_empty: + userdata.seat_position = seat.point + print(seat.point) + return "succeeded" + + return "failed" + + def __init__( + self, + seat_area: Polygon, + ): + smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"]) + with self: + + # TODO: stop doing this + self.userdata.people_detections = [] + self.userdata.seat_detections = [] + + motion_iterator = smach.Iterator( + outcomes=["succeeded", "failed"], + it=self._motions, + it_label="motion", + input_keys=["people_detections", "seat_detections"], + output_keys=["seat_position"], + exhausted_outcome="failed", + ) + + with motion_iterator: + + container_sm = smach.StateMachine( + outcomes=["succeeded", "failed", "continue"], + input_keys=["motion", "people_detections", "seat_detections"], + output_keys=["seat_position"], + ) + + with container_sm: + smach.StateMachine.add( + "LOOK", + PlayMotion(), + transitions={ + "succeeded": "DETECT", + "aborted": "failed", + "preempted": "failed", + }, + remapping={"motion_name": "motion"}, + ) + smach.StateMachine.add( + "DETECT", + Detect3DInArea(seat_area, filter=["chair", "person"]), + transitions={"succeeded": "CHECK", "failed": "failed"}, + ) + smach.StateMachine.add( + "CHECK", + self.ProcessDetections(), + transitions={"succeeded": "succeeded", "failed": "continue"}, + ) + + smach.Iterator.set_contained_state( + "CONTAINER_SM", container_sm, loop_outcomes=["continue"] + ) + + smach.StateMachine.add( + "MOTION_ITERATOR", + motion_iterator, + transitions={"succeeded": "LOOK_TO_POINT", "failed": "failed"}, + ) + smach.StateMachine.add( + "LOOK_TO_POINT", + LookToPoint(), + transitions={ + "succeeded": "SAY_SIT", + "aborted": "failed", + "preempted": "failed", + }, + remapping={"point": "seat_position"}, + ) + smach.StateMachine.add( + "SAY_SIT", + Say("Please sit in the seat that I am looking at."), + transitions={ + "succeeded": "RESET_HEAD", + "aborted": "failed", + "preempted": "failed", + }, + ) # TODO: sleep after this. + + smach.StateMachine.add( + "RESET_HEAD", + PlayMotion("look_centre"), + transitions={ + "succeeded": "succeeded", + "aborted": "failed", + "preempted": "failed", + }, + ) diff --git a/tasks/receptionist/src/receptionist/states/start.py b/tasks/receptionist/src/receptionist/states/start.py deleted file mode 100644 index 3e5f0eed9..000000000 --- a/tasks/receptionist/src/receptionist/states/start.py +++ /dev/null @@ -1,20 +0,0 @@ -import smach -import rospy -from tiago_controllers.helpers.pose_helpers import get_pose_from_param - - -class Start(smach.State): - def __init__(self, default): - smach.State.__init__(self, outcomes=['succeeded']) - self.default = default - - def execute(self, userdata): - self.default.voice.speak("Start of task.") - rospy.set_param("guest2/drink","unknown") - rospy.set_param("guest1/drink","unknown") - rospy.set_param("guestcount/count",0) - - res = self.default.controllers.base_controller.ensure_sync_to_pose(get_pose_from_param('/start/pose')) - rospy.logerr(res) - - return 'succeeded'