4.3 Planner Tutorial
For this tutorial, you will learn what the Planner does, how it works, what state machines are, and apply this knowledge to implement a simple mission for the AUV!
Read this.
A state machine is a computational model used in computer science and engineering to describe the behavior of a system by defining a finite set of states, transitions between these states, and the actions associated with each state or transition. It is an effective way to model complex systems with discrete behavior.
In a state machine:
- State: Represents a mode that the system can be in. (e.g. executing a specific task)
- Transition: Specifies how the system can move from one state to another. (in the context of AUV missions, all states have one incoming transition, and two outgoing transitions for "failure" or "success")
State machines typically start in an initial state and transition through different states in response to external events or conditions. These transitions can be triggered by events, timer expirations, or certain conditions being met. States can have associated actions that are executed when the state is entered, exited, or during its execution. In the context of the AUV's missions, each state is responsible for completing a certain small task, and executes one of two transitions based on whether that task fails or succeeds.
Nested state machines are a powerful extension of traditional state machines. They allow you to organize complex systems by breaking them down into smaller, more manageable state machines. In nested state machines, each state can operate either as a normal state or as its own state machine, making it easier to design and understand large-scale systems. For the AUV missions, this allows us to have a high-level state machine that transitions from task to task, where each task is its own state machine. This abstraction keeps things clean and makes for a state machine which is easier to read but can still handle complex tasks.
For a more concrete example:
We could have a top-level state machine defined as:
mission = submerge -> search for object -> pick up object -> emerge.
But, since the pick up object is quite a complex task, we could define it as its own state machine:
pick up object = open gripper -> move above object -> close gripper -> check that object was picked up
In the context of Python and ROS (Robot Operating System), you can implement state machines, including nested state machines, using the smach library. smach provides a framework for defining and executing state machines in Python, making it particularly useful for robot control and automation tasks.
Here's a simplified example of how to create a nested state machine with smach:
import rospy
import smach
# Define some states
class State1(smach.State):
def __init__(self):
smach.State.__init__(self, outcomes=['fail', 'success'])
def execute(self, userdata):
# State logic here
return 'success'
class State2(smach.State):
def __init__(self):
smach.State.__init__(self, outcomes=['fail', 'success'])
def execute(self, userdata):
# State logic here
return 'success'
class State3(smach.State):
def __init__(self):
smach.State.__init__(self, outcomes=['fail', 'success'])
def execute(self, userdata):
# State logic here
return 'success'
rospy.init_node('state_machine_node')
# Create a state machine for some complex task
complex_task_sm = smach.StateMachine(outcomes=['fail', 'success']) # defines state machine possible final states
# Create and add states (these can be states or state machines)
with complex_task_sm:
smach.StateMachine.add('STATE1', State1(), transitions={'success':'STATE2', 'fail':'fail'})
smach.StateMachine.add('STATE2', State2(), transitions={'success':'success', 'fail':'STATE1'})
# Create the top-level state machine
top_level_sm = smach.StateMachine(outcomes=['fail', 'success']) # defines state machine possible final states
# Create and add states (these can be states or state machines)
with top_level_sm:
smach.StateMachine.add('complex_task', complex_task_sm, transitions={'success':'STATE3', 'fail':'fail'}) # this adds the complex task state machine as the initial state for the top-level state machine
smach.StateMachine.add('STATE3', State3(), transitions={'success':'success', 'fail':'fail'})
# Execute and print the result of the top-level state machine
outcome = top_level_sm.execute()
print(outcome)
In this example, State1, State2 and State3 are simple states, and the state machine complex_task_sm is created that uses State1 and State2. Then, the top-level state machine top_level_sm is created that uses the complex task state machine as its initial state, and when that task has succeeded, executes State3. The add method is used to define transitions between states for the state machines.
More complex systems can be built by adding additional states, transitions, and actions to suit the specific requirements of a mission. Check out the full documentation on smach here.
To keep things as simple as possible when writing missions, the Planner has utility classes for the Controls, Vision and State Estimation packages that provide a simplified interface for missions to use. These classes are defined in the substates/utility folder in the Planner package. To use them, simply import them into your mission's python file (or have them passed in as arguments from a higher-level state machine, as is done in these substates), and call the class functions (or, for the State Estimation utility, read from the class fields directly) you need for your mission.
Take a look at the Planner (Utility Classes) section of this wiki for more information on the utility classes, specifically what functionality they offer. It will be very important for the exercise.
For this exercise, you will write a simple mission which is able to complete the qualification task for Robosub! This task involves submerging the AUV, moving it forward, going around a pole which is at an arbitrary distance from where the AUV started, and returning to the AUV's starting position, before floating back to the surface. You can assume that the AUV will be facing the pole when it starts (i.e. an orientation of X=0, Y=0, Z=0 degrees in Euler representation or W=1, X=0, Y=0, Z=0 as a quaternion).
For clear information on reference frames go here.
Pro tip: Assuming the AUV is at a rotation of X=0, Y=0, Z=0 in Euler, the X axis points "forward" with respect to the AUV, the Y axis points "left", and the Z axis points "up". To visualize the X, Y, and Z axes that the AUV/Controller class follows, use the right hand rule and point your index finger forward:
Then, to know which direction around an axis is the positive rotation axis, point the thumb from your right hand towards the positive direction of that axis. Whichever direction your fingers are curled towards is the positive direction for rotations on that axis!
Write a mission for the AUV that does the following, in order:
- Submerges the AUV to -2 meters on the z axis.
- Moves the AUV forward by some arbitrary distance (make this a parameter to play with if you want!).
- Turn the AUV towards the left by 90 degrees.
- Moves the AUV towards the left (note "left" here means with respect to the world frame. In the AUV frame, this is a "forward" movement) by some arbitrary distance (make this small, 1-3 meters).
- Turn the AUV towards the left by 90 degrees.
- Moves the AUV backward by some arbitrary distance (make this a parameter to play with if you want!). (again, "backward" here means with respect to the world frame. In the AUV frame, this is a "forward" movement)
- Stop all movement and have the AUV float to the surface.
You can either write this mission as a class, in which case you should use the GoToOctagon class as a starting point to understand the structure of class missions and how to run the mission. You can also write the task as a standalone Python file (this is definitely the easier option), in which case you can base yourself off of any of the Python files in this folder (except for mission.py).
If at any point you are stuck or confused, don't hesitate to send a message to the software chat on Discord! The leads or other members will happily help you out.
Once you have completed the mission, and have successfully tested it using the Gazebo simulation, make a Pull Request containing your new changes on the repository. If possible, please include a short screen recording of the mission running in the simulation. We will not merge the code, but this indicates to us that you have finished the tutorial.
**NOTE: **If your computer is too slow/has problems running the Gazebo simulation, reach out to the Software team through Discord so one of the members can run it and debug it with you. Otherwise, you can always come to the in-person software meetings and ask a member or lead for help either running the mission or debugging the simulation.