GetPath fails after "succesfull" Recovery

[kJh19]

-ROS noetic -Smach

Using a smach setup similar to: http://wiki.ros.org/move_base_flex/Tutorials/SimpleSmachForMoveBaseFlex I move the robot to an impossible location, inside an obstacle, as due to dynamic object this scenario can sometimes occur for us.

• The getPath predictably fails, so we move to recovery.
• Recovery moves the robot to a viable location. In another test if we start here the GetPath succeeds
• We return to getPath.
• Getpath fails?
• goes to recovery, repeats indefinitely

I do not understand why getPath keeps aborting ones moved to in other scenarios a fine position.

I have a monitor state (WAIT_CANCEL) running concurrently to the getpath, recovery, exePath state-machine that on an empty message ends the concurrent state and moves to a wait_goal state. This state on receiving a goal starts the concurrent state machine with the WAIT_CANCEL state and [getpath, recovery, exePath] state-machine.

If I cancel it by sending to WAIT_CANCEL and then send the exact same goal Getpath succeeds. if I have the concurrent state end after recovery and go to wait_goal with the same goal on it's own, GetPath fails.

It keeps telling me the planner patience is exceeded. If I send to the WAIT_CANCEL, it tells me it canceled the planner. but not otherwise.

Below the state_machine

 with self.sm:

        # WAIT_MSG Concurrence 1
        #Concurrence terminiation when
        def wait_msg_child_term_cb(outcome_map):
            if outcome_map['WAIT_WAYPOINT'] == 'invalid':
                return True
            elif outcome_map['WAIT_RUN'] == 'invalid':
                return True
            elif outcome_map['WAIT_RESET'] == 'invalid':
                return True
            else:
                return False

        #Concurrence output
        def wait_msg_out_cb(outcome_map):
            if outcome_map['WAIT_WAYPOINT'] == 'invalid':
                return 'new_waypoint'
            elif outcome_map['WAIT_RUN'] == 'invalid':
                return 'run'
            elif outcome_map['WAIT_RESET'] == 'invalid':
                return 'reset'
            else:
                return 'nothing'

        Wait_message = smach.Concurrence(outcomes=['new_waypoint', 'run', 'reset', 'nothing'],
                                            default_outcome='nothing',
                                            input_keys = ['goals'],
                                            output_keys = ['goals'],
                                            child_termination_cb=wait_msg_child_term_cb,
                                            outcome_cb=wait_msg_out_cb)

        with Wait_message:

            def goal_cb(userdata, msg):
                userdata.goals.append(msg)
                return False

            def reset_cb(userdata, msg):
                userdata.goals = []
                return False

            def monitor_cb(ud, msg):
                return False

            smach.Concurrence.add('WAIT_WAYPOINT', smach_ros.MonitorState(
                    GOAL_TOPIC,
                    PoseStamped,
                    goal_cb,
                    input_keys=['goals'],
                    output_keys=['goals']
                ))

            smach.Concurrence.add('WAIT_RUN', smach_ros.MonitorState(
                    RUN_TOPIC,
                    Empty,
                    monitor_cb
                ))

            smach.Concurrence.add('WAIT_RESET', smach_ros.MonitorState(
                    RESET_TOPIC,
                    Empty,
                    reset_cb,
                    input_keys=['goals'],
                    output_keys=['goals']
                ))

        smach.StateMachine.add(
            'WAIT_MSG',
            Wait_message,
            transitions={
                'new_waypoint': 'WAIT_MSG',
                'run': 'RUN_PATH',
                'reset': 'WAIT_MSG',
                'nothing': 'WAIT_MSG'
            }
        )

        #RUN_PATH Concurrence 2
        #Concurrence termination when 
        def run_path_child_term_cb(outcome_map):
            if outcome_map['WAIT_CANCEL'] == 'invalid':
                return True
            elif outcome_map['WAIT_RESTART'] == 'invalid':
                return True
            elif outcome_map['EXE_SUB'] == 'succeeded' or outcome_map['EXE_SUB'] == 'aborted' or outcome_map['EXE_SUB'] == 'preempted':
                return True
            else:
                return False

        #Concurrence output
        def run_path_out_cb(outcome_map):
            if outcome_map['WAIT_CANCEL'] == 'invalid':
                return 'aborted'
            elif outcome_map['EXE_SUB'] == 'succeeded':
                return 'succeeded'
            elif outcome_map['EXE_SUB'] == 'aborted':
                return 'aborted'
            elif outcome_map['EXE_SUB'] == 'preempted':
                return 'preempted'
            else:
                return 'succeeded'

        run_path = smach.Concurrence(outcomes=['succeeded', 'aborted', 'preempted'],
                                            default_outcome='succeeded',
                                            input_keys = ['goals'],
                                            output_keys = ['goals'],
                                            child_termination_cb=run_path_child_term_cb,
                                            outcome_cb=run_path_out_cb)

        with run_path:

            def cancel_cb(ud, msg):
                return False

            smach.Concurrence.add('WAIT_CANCEL', smach_ros.MonitorState(
                    CANCEL_TOPIC,
                    Empty,
                    cancel_cb
                ))

            # Create the sub SMACH state machine 
            exe_sub = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'],
                                                input_keys = ['goals'],
                                                output_keys = ['goals'])

            exe_sub.userdata.goal = None
            exe_sub.userdata.path = None
            exe_sub.userdata.error = None
            exe_sub.userdata.error_status = None

            # Open the container 
            with exe_sub:

                class set_goal(smach.State):
                    def __init__(self):
                        smach.State.__init__(self, outcomes=['success','aborted'])

                    def execute(self, userdata):
                        #Set new goal
                        if len(exe_sub.userdata.goals)==0:
                            print('no_waypoints')
                            return 'aborted'
                        else:
                            exe_sub.userdata.goal = exe_sub.userdata.goals[0]
                            return 'success'

                # Set goal from waypoint list
                smach.StateMachine.add(
                    'SET_GOAL',
                    set_goal(),
                    transitions={
                        'success': 'GET_PATH'
                    }
                )

                # Get path
                smach.StateMachine.add(
                    'GET_PATH',
                    smach_ros.SimpleActionState(
                        '/move_base_flex/get_path',
                        GetPathAction,
                        goal_slots=['target_pose'],
                        result_slots=['path']
                    ),
                    transitions={
                        'succeeded': 'EXE_PATH',
                        'aborted': 'RECOVERY', 
                        'preempted': 'preempted'
                    },
                    remapping={
                        'target_pose': 'goal'
                    }
                )

                # EXE_PATH
                smach.StateMachine.add(
                    'EXE_PATH',
                    smach_ros.SimpleActionState(
                        '/move_base_flex/exe_path',
                        ExePathAction,
                        goal_slots=['path']
                    ),
                    transitions={
                        'succeeded': 'CHECK_GOALS',
                        'aborted': 'RECOVERY',
                        'preempted': 'preempted'
                    }
                )

                class check_goals(smach.State):
                    def __init__(self):
                        smach.State.__init__(self, outcomes=['success','continue'])

                    def execute(self, userdata):
                        del exe_sub.userdata.goals[0]
                        if len(exe_sub.userdata.goals)==0:
                            return 'success'
                        else:
                            return 'continue'

                # move waypoint list along
                smach.StateMachine.add(
                    'CHECK_GOALS',
                    check_goals(),
                    transitions={
                        'success': 'succeeded',
                        'continue': 'SET_GOAL'
                    }
                )

                # Goal callback for state RECOVERY
                def recovery_path_goal_cb(userdata, goal):
                    if self.fdist > self.bdist and self.fdist > 1.0:
                        goal.behavior = 'twist_recovery_forwards'
                    elif self.bdist > 1.0:
                        goal.behavior = 'twist_recovery_backwards'
                    else:
                        goal.behavior = 'do_nothing_5_seconds'
                    return

                smach.StateMachine.add(
                    'RECOVERY',
                    smach_ros.SimpleActionState(
                        'move_base_flex/recovery',
                        RecoveryAction,
                        goal_cb=recovery_path_goal_cb,
                        input_keys=["error"],
                        output_keys = ["error_status"]
                    ),
                    transitions={
                        'succeeded': 'SET_GOAL',
                        'aborted': 'aborted',
                        'preempted': 'preempted'
                    }
                )

            smach.Concurrence.add('EXE_SUB', exe_sub)

        # Execute path
        smach.StateMachine.add('RUN_PATH',
            run_path,
            transitions={
                'succeeded': 'WAIT_MSG',
                'aborted': 'ABORTED_GOALS',
                'preempted': 'preempted'
            }
        )

        class aborted_goals(smach.State):
            def __init__(self):
                smach.State.__init__(self, outcomes=['success'])

            def execute(self, userdata):
                msg = String()
                msg.data = "failed_reaching_waypoint"
                pubGUI.publish(msg.data)
                return 'success'

        smach.StateMachine.add(
                    'ABORTED_GOALS',
                    aborted_goals(),
                    transitions={
                        'success': 'WAIT_MSG'
                    }
                )