feat: initial state machine factory

tasks/gpsr/data/mock_data/names.json

@@ -4,7 +4,7 @@
         "angel",
         "axel",
         "charlie",
-        "janes",
+        "jane",
         "jules",
         "morgan",
         "paris",

tasks/gpsr/scripts/main.py

@@ -4,6 +4,7 @@
 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
 
@@ -33,6 +34,8 @@ def main():
     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__":

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

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(