Does move_base_flex global replanning constantly require GetPathAction state calls? [closed]

edit

This method works. Run the tutorial smacc program twice, so there will be two seperate statemachine node. And whenever you publish a new goal, one of them will work to reach the goal and the other one will be preempted! I reviesed the program slightly as below:

  #!/usr/bin/env python3
import rospy
import smach
import smach_ros

from geometry_msgs.msg import PoseStamped
from nav_msgs.msg import Path

from mbf_msgs.msg import ExePathAction
from mbf_msgs.msg import GetPathAction
from mbf_msgs.msg import RecoveryAction

import threading

def main():
    rospy.init_node('mbf_state_machine',anonymous=True)

    # Create SMACH state machine
    sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])

    # Define userdata
    sm.userdata.goal = None
    sm.userdata.path = None
    sm.userdata.error = None
    sm.userdata.clear_costmap_flag = False
    sm.userdata.error_status = None

    with sm:
        # Goal callback for state WAIT_FOR_GOAL
        def goal_cb(userdata, msg):
            userdata.goal = msg
            return False

        # Monitor topic to get MeshGoal from RViz plugin
        smach.StateMachine.add(
            'WAIT_FOR_GOAL',
            smach_ros.MonitorState(
                '/move_base_simple/goal',
                PoseStamped,
                goal_cb,
                output_keys=['goal']
            ),
            transitions={
                'invalid': 'GET_PATH',
                'valid': 'WAIT_FOR_GOAL',
                'preempted': 'preempted'
            }
        )

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

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

        # Goal callback for state RECOVERY
        def recovery_path_goal_cb(userdata, goal):
            if userdata.clear_costmap_flag == False:
                goal.behavior = 'clear_costmap'
                userdata.clear_costmap_flag = True
            else:
                goal.behavior = 'straf_recovery'
                userdata.clear_costmap_flag = False

        # Recovery
        smach.StateMachine.add(
            'RECOVERY',
             smach_ros.SimpleActionState(
                'move_base/recovery',
                RecoveryAction,
                goal_cb=recovery_path_goal_cb,
                input_keys=["error", "clear_costmap_flag"],
                output_keys = ["error_status", 'clear_costmap_flag']
            ),
            transitions={
                #'succeeded': 'GET_PATH',
                'succeeded': 'WAIT_FOR_GOAL',
                #'aborted': 'aborted',
                'aborted': 'WAIT_FOR_GOAL',
                #'preempted': 'preempted'
                'preempted': 'WAIT_FOR_GOAL'
            }
        )

    # Create and start introspection server
    sis = smach_ros.IntrospectionServer('smach_server', sm, '/SM_ROOT')
    sis.start()

    # Execute SMACH plan
    sm_t = threading.Thread(target=sm.execute)
    sm_t.start()
    # sm.execute()
    # Wait for interrupt and stop introspection server
    rospy.spin()
    sis.stop()
    sm.preempt_requested()
    rospy.loginfo("wait sm exit!")
    sm.request_preempt()
    sm_t.join()

if __name__=="__main__":
    main()