From 41e47ff2e9157dd3c19c0d23f731afc8ed63bdc3 Mon Sep 17 00:00:00 2001 From: Matt Barker <105945282+m-barker@users.noreply.github.com> Date: Tue, 23 Apr 2024 14:29:38 +0100 Subject: [PATCH] feat: GPSR state machine factory 01 (#164) * fix: incorrect folder for command similarity states * feat: add command parser state machine * feat: working command parser sm * feat: initial state machine factory --- tasks/gpsr/CMakeLists.txt | 1 + tasks/gpsr/data/mock_data/names.json | 2 +- tasks/gpsr/scripts/main.py | 44 ++++++++++ tasks/gpsr/src/gpsr/state_machine_factory.py | 76 +++++++++++++++++ tasks/gpsr/src/gpsr/states/__init__.py | 2 + tasks/gpsr/src/gpsr/states/command_parser.py | 82 +++++++++++++++++++ .../states/command_similarity_matcher.py | 0 tasks/gpsr/src/gpsr/states/talk.py | 14 ++-- 8 files changed, 213 insertions(+), 8 deletions(-) create mode 100644 tasks/gpsr/scripts/main.py create mode 100644 tasks/gpsr/src/gpsr/state_machine_factory.py create mode 100644 tasks/gpsr/src/gpsr/states/command_parser.py rename tasks/gpsr/{ => src/gpsr}/states/command_similarity_matcher.py (100%) diff --git a/tasks/gpsr/CMakeLists.txt b/tasks/gpsr/CMakeLists.txt index af1f6c908..a1c9b8bc6 100644 --- a/tasks/gpsr/CMakeLists.txt +++ b/tasks/gpsr/CMakeLists.txt @@ -155,6 +155,7 @@ include_directories( ## in contrast to setup.py, you can choose the destination catkin_install_python(PROGRAMS scripts/parse_gpsr_xmls.py + scripts/main.py nodes/commands/question_answer nodes/command_parser DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} diff --git a/tasks/gpsr/data/mock_data/names.json b/tasks/gpsr/data/mock_data/names.json index 7dbbe563f..0882a43c4 100644 --- a/tasks/gpsr/data/mock_data/names.json +++ b/tasks/gpsr/data/mock_data/names.json @@ -4,7 +4,7 @@ "angel", "axel", "charlie", - "janes", + "jane", "jules", "morgan", "paris", diff --git a/tasks/gpsr/scripts/main.py b/tasks/gpsr/scripts/main.py new file mode 100644 index 000000000..4c0e15f39 --- /dev/null +++ b/tasks/gpsr/scripts/main.py @@ -0,0 +1,44 @@ +#!/usr/bin/env python3 +import smach +import rospy +import sys +from typing import Dict +from gpsr.load_known_data import GPSRDataLoader +from gpsr.state_machine_factory import build_state_machine +from gpsr.regex_command_parser import Configuration +from gpsr.states import CommandParserStateMachine + + +def load_gpsr_configuration() -> Configuration: + gpsr_data_dir = sys.argv[1] + """Loads the configuration for the GPSR command parser""" + data_loader = GPSRDataLoader(data_dir=gpsr_data_dir) + gpsr_known_data: Dict = data_loader.load_data() + config = Configuration( + { + "person_names": gpsr_known_data["names"], + "location_names": gpsr_known_data["non_placeable_locations"], + "placement_location_names": gpsr_known_data["placeable_locations"], + "room_names": gpsr_known_data["rooms"], + "object_names": gpsr_known_data["objects"], + "object_categories_plural": gpsr_known_data["categories_plural"], + "object_categories_singular": gpsr_known_data["categories_singular"], + } + ) + return config + + +def main(): + config = load_gpsr_configuration() + command_parser_sm = CommandParserStateMachine(data_config=config) + command_parser_sm.execute() + parsed_command: Dict = command_parser_sm.userdata.parsed_command + rospy.loginfo(f"Parsed command: {parsed_command}") + sm = build_state_machine(parsed_command) + sm.execute() + + +if __name__ == "__main__": + rospy.init_node("gpsr_main") + main() + rospy.spin() diff --git a/tasks/gpsr/src/gpsr/state_machine_factory.py b/tasks/gpsr/src/gpsr/state_machine_factory.py new file mode 100644 index 000000000..1618aa415 --- /dev/null +++ b/tasks/gpsr/src/gpsr/state_machine_factory.py @@ -0,0 +1,76 @@ +#!/usr/bin/env python3 +import rospy +import smach +from smach_ros import ServiceState +from typing import Dict, List +from lasr_skills import GoToLocation, FindNamedPerson +from gpsr.states import Talk + +STATE_COUNT = 0 + + +def increment_state_count() -> int: + global STATE_COUNT + STATE_COUNT += 1 + return STATE_COUNT + + +def build_state_machine(parsed_command: Dict) -> smach.StateMachine: + """Constructs the parameterized state machine for the GPSR task, + given the parsed command. + + Args: + parsed_command (Dict): parsed command. + + Returns: + smach.StateMachine: paramaterized state machine ready to be executed. + """ + command_verbs: List[str] = parsed_command["commands"] + command_params: List[Dict] = parsed_command["params"] + sm = smach.StateMachine(outcomes=["succeeded", "failed"]) + with sm: + for command_verb, command_param in zip(command_verbs, command_params): + if command_verb == "greet": + if "name" in command_param: + location_param = ( + f"/gpsr/arena/rooms/{command_param['location']}/pose" + ) + sm.add( + f"STATE_{increment_state_count()}", + GoToLocation(location_param=location_param), + transitions={ + "succeeded": f"STATE_{STATE_COUNT + 1}", + "failed": "failed", + }, + ) + sm.add( + f"STATE_{increment_state_count()}", + FindNamedPerson( + name=command_param["name"], location_param=location_param + ), + transitions={ + "succeeded": f"STATE_{STATE_COUNT + 1}", + "failed": "failed", + }, + ) + elif "clothes" in command_param: + pass + else: + raise ValueError( + "Greet command received with no name or clothes in command parameters" + ) + elif command_verb == "talk": + if "gesture" in command_param: + pass + elif "talk" in command_param: + sm.add( + f"STATE_{increment_state_count()}", + Talk(command_param["talk"]), + transitions={"succeeded": "succeded", "failed": "failed"}, + ) + else: + raise ValueError( + "Talk command received with no gesture or talk in command parameters" + ) + + return sm diff --git a/tasks/gpsr/src/gpsr/states/__init__.py b/tasks/gpsr/src/gpsr/states/__init__.py index 74a382e32..84504f968 100644 --- a/tasks/gpsr/src/gpsr/states/__init__.py +++ b/tasks/gpsr/src/gpsr/states/__init__.py @@ -1 +1,3 @@ from .talk import Talk +from .command_similarity_matcher import CommandSimilarityMatcher +from .command_parser import ParseCommand, CommandParserStateMachine diff --git a/tasks/gpsr/src/gpsr/states/command_parser.py b/tasks/gpsr/src/gpsr/states/command_parser.py new file mode 100644 index 000000000..f0345bdcb --- /dev/null +++ b/tasks/gpsr/src/gpsr/states/command_parser.py @@ -0,0 +1,82 @@ +#!/usr/bin/env python3 +import smach +import rospy + +from gpsr.regex_command_parser import Configuration, gpsr_compile_and_parse +from gpsr.states import CommandSimilarityMatcher +from lasr_skills import AskAndListen + + +class ParseCommand(smach.State): + def __init__(self, data_config: Configuration): + """Takes in a string containing the command and runs the command parser + that outputs a dictionary of parameters for the command. + + Args: + data_config (Configuration): Configuration object containing the regex patterns + """ + smach.State.__init__( + self, + outcomes=["succeeded", "failed"], + input_keys=["raw_command"], + output_keys=["parsed_command"], + ) + self.data_config = data_config + + def execute(self, userdata): + rospy.loginfo(f"Received command : {userdata.raw_command.lower()}") + try: + userdata.parsed_command = gpsr_compile_and_parse( + self.data_config, userdata.raw_command.lower() + ) + except Exception as e: + rospy.logerr(e) + return "failed" + return "succeeded" + + +class CommandParserStateMachine(smach.StateMachine): + def __init__( + self, + data_config: Configuration, + n_vecs_per_txt_file: int = 1177943, + total_txt_files: int = 10, + ): + """State machine that takes in a command, matches it to a known command, and + outputs the parsed command. + + Args: + data_config (Configuration): Configuration object containing the regex patterns + n_vecs_per_txt_file (int, optional): number of vectors in each gpsr txt + file. Defaults to 100. + total_txt_files (int, optional): total number of gpsr txt files. Defaults to 10. + """ + smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"]) + + with self: + smach.StateMachine.add( + "ASK_FOR_COMMAND", + AskAndListen(tts_phrase="Hello, please tell me your command."), + transitions={"succeeded": "PARSE_COMMAND", "failed": "failed"}, + remapping={"transcribed_speech": "raw_command"}, + ) + + smach.StateMachine.add( + "PARSE_COMMAND", + ParseCommand(data_config), + transitions={ + "succeeded": "succeeded", + "failed": "COMMAND_SIMILARITY_MATCHER", + }, + remapping={"parsed_command": "parsed_command"}, + ) + + smach.StateMachine.add( + "COMMAND_SIMILARITY_MATCHER", + CommandSimilarityMatcher([n_vecs_per_txt_file] * total_txt_files), + transitions={"succeeded": "PARSE_COMMAND", "failed": "failed"}, + remapping={ + "command": "raw_command", + "matched_command": "raw_command", + }, + ) diff --git a/tasks/gpsr/states/command_similarity_matcher.py b/tasks/gpsr/src/gpsr/states/command_similarity_matcher.py similarity index 100% rename from tasks/gpsr/states/command_similarity_matcher.py rename to tasks/gpsr/src/gpsr/states/command_similarity_matcher.py diff --git a/tasks/gpsr/src/gpsr/states/talk.py b/tasks/gpsr/src/gpsr/states/talk.py index ea22ff951..29204c573 100644 --- a/tasks/gpsr/src/gpsr/states/talk.py +++ b/tasks/gpsr/src/gpsr/states/talk.py @@ -8,13 +8,13 @@ # In future we might want to add looking at person talking to the state machine. class Talk(smach.StateMachine): class GenerateResponse(smach.State): - def __init__(self): + def __init__(self, talk_phrase: str): smach.State.__init__( self, outcomes=["succeeded", "failed"], - input_keys=["talk_phrase"], output_keys=["response"], ) + self._talk_phrase = talk_phrase def _create_responses(self) -> Dict[str, str]: response = {} @@ -43,23 +43,23 @@ def _create_responses(self) -> Dict[str, str]: def execute(self, userdata): try: - userdata.response = self._create_responses()[userdata.talk_phrase] + userdata.response = self._create_responses()[self._talk_phrase] except KeyError: rospy.loginfo( - f"Failed to generate response for {userdata.talk_phrase} as it is not in the list of possible questions." + f"Failed to generate response for {self._talk_phrase} as it is not in the list of possible questions." ) return "failed" return "succeeded" - def __init__(self): + def __init__(self, talk_phrase: str): smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"]) with self: smach.StateMachine.add( "GENERATE_RESPONSE", - self.GenerateResponse(), + self.GenerateResponse(talk_phrase), transitions={"succeeded": "SAY_RESPONSE", "failed": "failed"}, - remapping={"talk_phrase": "talk_phrase", "response": "response"}, + remapping={"response": "response"}, ) smach.StateMachine.add(