diff --git a/tasks/gpsr/config/floor_6.yaml b/tasks/gpsr/config/floor_6.yaml index dc5f14a3d..94ec2249c 100644 --- a/tasks/gpsr/config/floor_6.yaml +++ b/tasks/gpsr/config/floor_6.yaml @@ -222,7 +222,7 @@ arena: - x: 2.8841753005981445 y: -1.464163780212402 z: 0.5 - living_room: + living room: pose: position: x: 1.5507337906536043 diff --git a/tasks/gpsr/src/gpsr/states/command_parser.py b/tasks/gpsr/src/gpsr/states/command_parser.py index d572655bf..a1c69afb0 100644 --- a/tasks/gpsr/src/gpsr/states/command_parser.py +++ b/tasks/gpsr/src/gpsr/states/command_parser.py @@ -51,6 +51,9 @@ def __init__( file. Defaults to 1177943. total_txt_files (int, optional): total number of gpsr txt files. Defaults to 10. """ + self._parse_command_counter = 0 + self._parse_command_counter_threshold = 3 + smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"]) with self: @@ -66,11 +69,20 @@ def __init__( ParseCommand(data_config), transitions={ "succeeded": "succeeded", - "failed": "COMMAND_SIMILARITY_MATCHER", + "failed": "CHECK_PARSE_COMMAND_COUNTER", }, remapping={"parsed_command": "parsed_command"}, ) + smach.StateMachine.add( + "CHECK_PARSE_COMMAND_COUNTER", + smach.CBState(self._increment_counter, outcomes=["continue", "exceeded"]), + transitions={ + "continue": "COMMAND_SIMILARITY_MATCHER", + "exceeded": "ASK_FOR_COMMAND" + } + ) + smach.StateMachine.add( "COMMAND_SIMILARITY_MATCHER", CommandSimilarityMatcher([n_vecs_per_txt_file] * total_txt_files), @@ -80,3 +92,13 @@ def __init__( "matched_command": "raw_command", }, ) + + def _increment_counter(self, userdata): + """Increment counter and determine whether command has been parsed over the number of times + set by the threshold. + """ + self._parse_command_counter += 1 + if self._parse_command_counter >= self._parse_command_counter_threshold: + self._parse_command_counter = 0 + return "exceeded" + return "continue" \ No newline at end of file