## page was renamed from move_base_flex/Tutorials/Simple SMACH for Move Base Flex
## For instruction on writing tutorials
## http://www.ros.org/wiki/WritingTutorials
####################################
##FILL ME IN
####################################
## for a custom note with links:
## note =
## for the canned note of "This tutorial assumes that you have completed the previous tutorials:" just add the links
## note.0=
## descriptive title for the tutorial
## title = Writing a simple SMACH for Move Base Flex
## multi-line description to be displayed in search
## description = In this tutorial you learn how to set up a [[smach|SMACH]] to use Move Base Flex for flexible and more specific navigation tasks. Using a [[smach|SMACH]] lets you easily include your navigation tasks into your global robot behavior state machine. We will use RViz to receive a goal pose as the first input for the [[smach|SMACH]].
## the next tutorial description (optional)
## next =
## links to next tutorial (optional)
## next.0.link=
## next.1.link=
## what level user is this tutorial for
## level=AdvancedCategory
## keywords = smach, move_base_flex, navigation, planning, state machine
####################################
<<IncludeCSTemplate(TutorialCSHeaderTemplate)>>

## AUTOGENERATED DO NOT DELETE
## TutorialCategory
## FILL IN THE STACK TUTORIAL CATEGORY HERE
== Description ==
In this tutorial we address the actions ''GetPath'', ''ExePath'' and ''Recovery'' provided by Move Base Flex. While ''GetPath'' runs the global planner searching for a path to the target pose, ''ExePath'' runs the local planner executing the planned path. The ''Recovery'' action can be used to execute various behaviors for error handling during planning and controlling. We connect these actions by setting up a [[smach|SMACH]] state machine using [[smach/Tutorials/SimpleActionState|Simple Action States]]. In addition to the actions described above, the implementation of a state that receives a navigation goal by the user is required. The target pose can be easily set via the visualization tool [[rviz|RViz]] and published on a specific topic.

=== SMACH State Machine ===

{{attachment:mbf_state_machine.png||width="500"}}

The implemented state machine visualized by the [[smach_viewer]]


== SMACH States ==
In the following four [[smach|SMACH]] states are described: ''WaitForGoal'', ''GetPath'', ''ExePath'' and ''Recovery''. Furthermore, the connection between these states is shown and explained in the manner of transferring data from one state to another, e.g. the goal pose or the computed path.

=== WaitForGoal ===
The smach_ros package provides the [[smach/Tutorials/MonitorState|Monitor State]] as a simple wrapper state for subscribing topics. The navigation goal is published as a [[geometry_msgs|geometry_msgs/PoseStamped]] on the topic /move_base_simple/goal. For implementing the Monitor State, add the following lines to your code.

{{{#!python
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'
    }
)
}}}

There is a callback function ''goal_cb'' which is invoked when a new goal is published on the subscribed topic. The callback passes the target pose to the userdata ''goal'' defined as an output key to transfer it to the next state.

{{{#!python
def goal_cb(userdata, msg):
    userdata.goal = msg
    return False
}}}

=== GetPath ===
The following lines add the ''GetPath'' action as a Simple Action State to the state machine.

{{{#!python
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': 'WAIT_FOR_GOAL',
        'preempted': 'preempted'
    },
    remapping={
	'target_pose': 'goal'
    }
)
}}}

Note that you declare the correct namespace for the action. While the goal_slots define the input values, result_slots indicate the output values. To ensure the correct input and output keys, refer to the GetPath.action in the [[mbf_msgs]] package. The slots transfer the goals and results of the action automatically to the correlated userdata. If goals/results and userdata are named differently, you can change them with the remapping argument.

=== ExePath ===
The ExePath action is similar to GetPath. The state is added to the state machine as follows.

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

Here, we only have a goal ''path'' but no results. Do not forget to change the namespace.

=== Recovery ===
{{{#!python
smach.StateMachine.add(
    'RECOVERY',
    smach_ros.SimpleActionState(
        'move_base_flex/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',
        'aborted': 'aborted',
        'preempted': 'preempted'
    }
)
}}}

For the input, the recovery could react to different errors, which we not implemented yet and it takes the clear costmap_flag into account, which I'll explain in reference to the callback functions. If the recovery is successfull, it transitions to a new planning, if even the recovery failed the user must specify a new goal.

{{{#!python
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
}}}

The Recovery has only a goal_cb where at the moment per default by turns [[clear_costmap_recovery]] and [[straf_recovery]] are written into the goal and therefore executed. This is determined by the clear_costmap flag which changes alternately from True to false to ensure the turnwise calls.

== Complete State Machine ==
{{{#!python
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


def main():
    rospy.init_node('mbf_state_machine')

    # 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_flex/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_flex/exe_path',
		ExePathAction,
		goal_slots=['path']
	    ),
	    transitions={
		'succeeded': 'succeeded',
		'aborted': 'RECOVERY',
		'preempted': 'preempted'
	    }
	)

	# 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_flex/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',
	        'aborted': 'aborted',
	        'preempted': 'preempted'
	    }
        )

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

    # Execute SMACH plan
    sm.execute()

    # Wait for interrupt and stop introspection server
    rospy.spin()
    sis.stop()

if __name__=="__main__":
    main()
}}}

To display the state machine in the [[smach_viewer]], run [[rqt_smach]] in your terminal.

{{{
rosrun rqt_smach rqt_smach
}}}

Following [[https://gist.github.com/matt3o/88bced95dba37a8932a51904d0734dff|instructions]] will help you to install rqt_smach in your workspace.