4.3 Planner Tutorial

What is the Planner?

The Planner is responsible for high-level decision making/mission planning for the AUV. It uses simplified interfaces with the Vision and State Estimation packages to decide what the AUV should do next, and an interface with the Controls package to execute changes in position/orientation accordingly. Missions are built as state machines, i.e. a series of states, where each state represents a small task (for example searching for a specific object on the floor of the pool, going through a gate, etc.)

State Machines

What is a State Machine?

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")

How Does a State Machine Work?

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

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

Programming State Machines with smach (Python/ROS)

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:

python
import rospy
import smach
import smach_ros

# 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 'outcome1'

class State2(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['fail', 'success'])

    def execute(self, userdata):
        # State logic here
        return 'outcome3'

# Create a top-level state machine
def main():
    rospy.init_node('state_machine_node')
    sm = smach.StateMachine(outcomes=['fail', 'success']) # defines state machine possible final states

    # Create and add states (these can be states or state machines)
    with sm:
        smach.StateMachine.add('STATE1', State1(), transitions={'success':'STATE2', 'fail':'fail'})
        smach.StateMachine.add('STATE2', State2(), transitions={'success':'success', 'fail':'STATE1'})

    # Execute the state machine
    outcome = sm.execute()

if __name__ == '__main__':
    main()

In this example, State1 and State2 are two simple states, and a top-level state machine sm is created to coordinate their execution. The add method is used to define transitions between states for the top-level state machine. When executed, this state machine will transition between State1 and State2 based on their defined outcomes.

This is just a basic overview of state machines in Python/ROS using the smach library. More complex systems can be built by adding additional states, transitions, and actions to suit the specific requirements of your application. Check out the details here.

Utility classes

Exercise