 Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
Writing a simple SMACH for Move Base Flex
Description: In this tutorial you learn how to set up a SMACH to use Move Base Flex for flexible and more specific navigation tasks. Using a 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.Keywords: smach, move_base_flex, navigation, planning, state machine
Tutorial Level: ADVANCED
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 state machine using 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 and published on a specific topic.
SMACH State Machine
The implemented state machine visualized by the smach_viewer
SMACH States
In the following four 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 Monitor State as a simple wrapper state for subscribing topics. The navigation goal is published as a geometry_msgs/PoseStamped on the topic /move_base_simple/goal. For implementing the Monitor State, add the following lines to your code.
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.
GetPath
The following lines add the GetPath action as a Simple Action State to the state machine.
1 smach.StateMachine.add(
2 'GET_PATH',
3 smach_ros.SimpleActionState(
4 '/move_base_flex/get_path',
5 GetPathAction,
6 goal_slots=['target_pose'],
7 result_slots=['path']
8 ),
9 transitions={
10 'succeeded': 'EXE_PATH',
11 'aborted': 'WAIT_FOR_GOAL',
12 'preempted': 'preempted'
13 },
14 remapping={
15 'target_pose': 'goal'
16 }
17 )
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.
Here, we only have a goal path but no results. Do not forget to change the namespace.
Recovery
1 smach.StateMachine.add(
2 'RECOVERY',
3 smach_ros.SimpleActionState(
4 'move_base_flex/recovery',
5 RecoveryAction,
6 goal_cb=recovery_path_goal_cb,
7 input_keys=["error", "clear_costmap_flag"],
8 output_keys = ["error_status", 'clear_costmap_flag']
9 ),
10 transitions={
11 'succeeded': 'GET_PATH',
12 'aborted': 'aborted',
13 'preempted': 'preempted'
14 }
15 )
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.
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
1 import rospy
2 import smach
3 import smach_ros
4
5 from geometry_msgs.msg import PoseStamped
6 from nav_msgs.msg import Path
7
8 from mbf_msgs.msg import ExePathAction
9 from mbf_msgs.msg import GetPathAction
10 from mbf_msgs.msg import RecoveryAction
11
12
13 def main():
14 rospy.init_node('mbf_state_machine')
15
16 # Create SMACH state machine
17 sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
18
19 # Define userdata
20 sm.userdata.goal = None
21 sm.userdata.path = None
22 sm.userdata.error = None
23 sm.userdata.clear_costmap_flag = False
24 sm.userdata.error_status = None
25
26 with sm:
27 # Goal callback for state WAIT_FOR_GOAL
28 def goal_cb(userdata, msg):
29 userdata.goal = msg
30 return False
31
32 # Monitor topic to get MeshGoal from RViz plugin
33 smach.StateMachine.add(
34 'WAIT_FOR_GOAL',
35 smach_ros.MonitorState(
36 '/move_base_simple/goal',
37 PoseStamped,
38 goal_cb,
39 output_keys=['goal']
40 ),
41 transitions={
42 'invalid': 'GET_PATH',
43 'valid': 'WAIT_FOR_GOAL',
44 'preempted': 'preempted'
45 }
46 )
47
48 # Get path
49 smach.StateMachine.add(
50 'GET_PATH',
51 smach_ros.SimpleActionState(
52 '/move_base_flex/get_path',
53 GetPathAction,
54 goal_slots=['target_pose'],
55 result_slots=['path']
56 ),
57 transitions={
58 'succeeded': 'EXE_PATH',
59 'aborted': 'WAIT_FOR_GOAL',
60 'preempted': 'preempted'
61 },
62 remapping={
63 'target_pose': 'goal'
64 }
65 )
66
67 # Execute path
68 smach.StateMachine.add(
69 'EXE_PATH',
70 smach_ros.SimpleActionState(
71 '/move_base_flex/exe_path',
72 ExePathAction,
73 goal_slots=['path']
74 ),
75 transitions={
76 'succeeded': 'succeeded',
77 'aborted': 'RECOVERY',
78 'preempted': 'preempted'
79 }
80 )
81
82 # Goal callback for state RECOVERY
83 def recovery_path_goal_cb(userdata, goal):
84 if userdata.clear_costmap_flag == False:
85 goal.behavior = 'clear_costmap'
86 userdata.clear_costmap_flag = True
87 else:
88 goal.behavior = 'straf_recovery'
89 userdata.clear_costmap_flag = False
90
91 # Recovery
92 smach.StateMachine.add(
93 'RECOVERY',
94 smach_ros.SimpleActionState(
95 'move_base_flex/recovery',
96 RecoveryAction,
97 goal_cb=recovery_path_goal_cb,
98 input_keys=["error", "clear_costmap_flag"],
99 output_keys = ["error_status", 'clear_costmap_flag']
100 ),
101 transitions={
102 'succeeded': 'GET_PATH',
103 'aborted': 'aborted',
104 'preempted': 'preempted'
105 }
106 )
107
108 # Create and start introspection server
109 sis = smach_ros.IntrospectionServer('smach_server', sm, '/SM_ROOT')
110 sis.start()
111
112 # Execute SMACH plan
113 sm.execute()
114
115 # Wait for interrupt and stop introspection server
116 rospy.spin()
117 sis.stop()
118
119 if __name__=="__main__":
120 main()
To display the state machine in the smach_viewer, run rqt_smach in your terminal.
rosrun rqt_smach rqt_smach
Following instructions will help you to install rqt_smach in your workspace.
