feat: add command parser state machine

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

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",
+                },
+            )