 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 SMACH for parallel planning for Move Base Flex
Description: The goal of this tutorial is to create a SMACH for move base flex to execute two global planners simultaneously.Keywords: smach, move_base_flex, navigation, planning, state machine, parallel
Tutorial Level: ADVANCED
Contents
Description
The goal of this tutorial is to create a SMACH for move base flex to execute two global planners simultaneously. To do this we need to implement two state machines S1, S2 nested into one concurrency block. Both consist of two states, one for planning with a dedicated global planner (usually different ones in S1 and S2) and one to execute a successfully made plan so that the robot follows the planned path.
Because S1 and S2 are being executed simultaneously, the robot will start to follow the earlier terminated planner which will be overwritten by the other once it itself finished planning because both use the same concurrency slot.
If both S1 and S2 fail to plan or to execute the state machine changes into recovery state, if they terminate with outcome 'aborted' the state machine also terminates.
If both S1 and S2 succeed to plan and to execute the robot reached its goal and the SM changes into wait-for-goal state.
Interface
CBInterface and cb_interface provide decorators for describing data of a state including outcomes, input_keys and output_keys. We used an extention to CBInterface and cb_interface to bind the self-parameter. That provides the possibility to use methods.
State Machine
The State Machine contains the WaitForGoal and Recovery states and the PLAN_EXEC state machine in a concurrency block.
The implemented state machine visualized by the smach_viewer.
WaitForGoal
Creating the WaitForGoal state that entering the concurrency block by succeed:
Recovery
Creating the Recovery state that entering the concurrency block by succeed:
Concurrency State Machine
Create a new state machine and add it to the state machine. Add the outcome invalid. The state machine terminated in invalid if the start or goal is invalid, no path is found and if the planner take to much time.
Creating this state machine in a concurrency block to execute the global planner state machines simultaneously.
Concurrency State Machine initialized the planner state machines:
1 smach.Concurrence.__init__(
2 self,
3 outcomes=['failure', 'invalid', 'succeeded', 'preempted', 'aborted'],
4 default_outcome='succeeded',
5 outcome_map=outcome_map,
6 input_keys=['target_pose'],
7 output_keys=['outcome', 'message', 'path'],
8 child_termination_cb=self.plan_exec_sm_child_termination_cb,
9 outcome_cb=self.plan_exec_sm_outcome_cb)
10 with self:
11 for index, planner in enumerate(self._planners):
12 planner_sm = PlannerStateMachine(index, planner, 'eband')
13 smach.Concurrence.add(planner.upper(), planner_sm)
plan_exec_sm_child_termination_cb() gets called every time a child state machine terminates. If one child succeeded or all children terminated with outcome aborted or preempted, the whole concurrent SM terminates.
1 def plan_exec_sm_child_termination_cb(self, outcome_map):
2 # check for success
3 for planner in self._planners:
4 if outcome_map[planner.upper()] == 'succeeded':
5 return True # terminate concurrent statemachine, if one execution succeeded
6
7 # check, if all planners are aborted, preempted or invalid
8 aborted_preempted_invalid = True
9 for planner in self._planners:
10 print "plan_exec child terminated with mode " + str(outcome_map[planner.upper()])
11 if outcome_map[planner.upper()] not in ('preempted', 'aborted', 'invalid'):
12 aborted_preempted_invalid = False
13 if aborted_preempted_invalid:
14 return True # every planner was aborted or preempted. Cancel this statemachine
15
16 # Do not terminate the concurrent statemachine, if at least one child is running.
17 return False
In plan_exec_sm_outcome_cb() happens something quite similar. This time the concurrent state machines outcome is returned. Checks the outcome of the planners if one planner execution succeeded terminate the concurrent state machine. If both planners terminate but do not succeed return the corresponding outcome of the planner state machine.
1 # gets called when ALL child states are terminated
2 def plan_exec_sm_outcome_cb(self, outcome_map):
3 for planner in self._planners:
4 if outcome_map[planner.upper()] == 'succeeded':
5 return 'succeeded'
6
7 # Check, if every planner is invalid
8 invalid = True
9 for planner in self._planners:
10 if outcome_map[planner.upper()] != 'invalid':
11 invalid = False
12 if invalid:
13 print 'No path was found.'
14 return 'invalid'
15
16 aborted_or_preempted = True
17 aborted_found = False
18 for planner in self._planners:
19 if outcome_map[planner.upper()] not in ('preempted', 'aborted'):
20 aborted_or_preempted = False
21 if outcome_map[planner.upper()] == 'aborted':
22 aborted_found = True
23
24 if aborted_or_preempted:
25 if aborted_found:
26 return 'aborted'
27 else:
28 return 'preempted'
29
30 return 'failure'
Planner State Machine
The planner state machine contains a Planner and an Execution state for each global planner so that the planner is ready to execute when it finish planning. When the slower planner is done planning it overwrites the execution state.
Planner
Creating the Planner state:
1 state = smach_ros.SimpleActionState(
2 'move_base_flex/get_path',
3 GetPathAction,
4 goal_cb=self.get_path_goal_cb,
5 result_cb=self.get_path_result_cb)
6 smach.StateMachine.add(
7 planner_name.upper(),
8 state,
9 transitions={
10 'succeeded': planner_name.upper()+'_EXEC',
11 'failure': 'failure',
12 'invalid': 'invalid',
13 'preempted': 'preempted'})
If the global planner implements the mbf_costmap_core::CostmapPlanner interface you could use the following mapping. In get_path_goal_cb the result outcome is modified to the cancel method for move base flex.
1 if result.outcome == GetPathResult.SUCCESS:
2 return 'succeeded'
3 elif result.outcome == GetPathResult.CANCELED:
4 return 'preempted'
5 elif result.outcome in (GetPathResult.INVALID_START, GetPathResult.INVALID_GOAL, GetPathResult.NO_PATH_FOUND, GetPathResult.PAT_EXCEEDED):
6 print 'Planning with %s could not get any path %s:\n%s' % (self._planner_name, str(result.outcome), result.message)
7 return 'invalid'
8 else:
9 print 'Planning with %s terminated with non-success status code %s:\n%s' % (self._planner_name, str(result.outcome), result.message)
10 return 'failure'
Execution
Creating the Execution state:
1 state = smach_ros.SimpleActionState(
2 'move_base_flex/exe_path',
3 ExePathAction,
4 goal_cb=self.exe_path_goal_cb,
5 result_cb=self.exe_path_result_cb)
6 smach.StateMachine.add(
7 planner_name.upper() + '_EXEC',
8 state,
9 transitions={
10 'succeeded': 'succeeded',
11 'failure': 'failure',
12 'preempted': 'preempted'})
If the global planner implements the mbf_costmap_core::CostmapPlanner interface you could use the following mapping. In exe_path_goal_cb the result outcome is modified to the cancel method for move base flex.
