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 index d4d44bda3..4c0e15f39 100644 --- a/tasks/gpsr/scripts/main.py +++ b/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__": 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/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(