diff --git a/tasks/gpsr/scripts/main.py b/tasks/gpsr/scripts/main.py new file mode 100644 index 000000000..e69de29bb diff --git a/tasks/gpsr/src/gpsr/states/__init__.py b/tasks/gpsr/src/gpsr/states/__init__.py index 74a382e32..fcf034f64 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_parser import ParseCommand, CommandParserStateMachine +from .command_similarity_matcher import CommandSimilarityMatcher 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..60dfb510d --- /dev/null +++ b/tasks/gpsr/src/gpsr/states/command_parser.py @@ -0,0 +1,84 @@ +#!/usr/bin/env python3 +import argparse +import smach +import rospy + +from gpsr.load_known_data import GPSRDataLoader +from gpsr.regex_command_parser import Configuration, gpsr_compile_and_parse +from gpsr.states import CommandSimilarityMatcher +from lasr_skills import AskAndListen, Say + + +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.transcribed_speech.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(), + 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": "parsed_command", + "matched_command": "matched_command", + }, + )