[TASK] FSM for docking task using YASMIN

[Andeshog]

Description of task

Continue to explore and learn how the YASMIN library works. Do so by developing a demo for the docking task at TACC. The demo should be similar to how we (auto) will handle the task in a real run, but since we cant yet rely on data from Perception you can use timers in the demo. The flow of the demo can be as follows:

• Start by looking for the dock. This can be done by performing a search maneuver like the rectangular search for a set amount of seconds (for example 15).
• When the dock is found go into a converging state where the drone will get to a certain distance to the dock. Starting off this can also be done with timer, but in the actual demo you will use the Velocity Controller for this state.
• When close enough to the dock go into a setup state which will align the drone over the dock, such that it is ready to dock. This maneuver will be done by the DP Controller.
• When the setup is right, go into docking state where the drone will perform the docking maneuver.
• When the drone has landed on the dock go into the docked state.
• (Bonus) Add a returning home state which will take the drone back to the starting point.

The demo will be tested in the simulator at the office.

Suggested Workflow

• Outline the whole process with all necessary states with a drawing/figure. Figure out what actions to be done in each state and requirement for transition.
• Discuss with the team on how to communicate states and transitions for the demo.
• Implement and test that the state transitions work as intended.

Specifications

Specification for the demo

• The starting position for the drone is (x ,y, z) = (29, 6, 0)
• The dock is located at (x, y, z) = (5, 5, 10)
• The distance to the dock to trigger transition from converging to setup can be set to 3 meters.
• The dimensions for the pool in the simulator is 30x12x10 (L x W x H)

Contacts

@Andeshog

Code Quality

• [ ] Every function in header files are documented (inputs/returns/exceptions)
• [ ] The project has automated tests that cover MOST of the functions and branches in functions (pytest/gtest)
• [ ] The code is documented on the wiki (provide link)

[chrstrom]

Just some food for thought after some of the things we learned when working on Beluga Mk2 and setting up the state machines for robosub:

More states usually lead to more clutter. Finding the "right" amount of states and what these should be is hard, while concluding on "lets just add another state" is easy. As an example, could "moving_to_dock", "returning_home", and "docking" all be captured in the state "moving_to_point"? (I don't know if that's feasible for this specific task, but it's something to think about).

In addition to being critical about what states you add, you might want to think about what events and effects you define. Ideally, all state transitions are triggered by external "events", i.e. the dp controller letting you know you've reached a setpoint. The output for a given state are called "effects", which could f.ex be firing up a dp controller to move to a specific point. If you were to check if you've reached the checkpoint internally, you suddenly need access to odometry, and end up "disconnecting" the fsm from the dp controller.

It is also very common to see an "error" state, but be wary of the implications there. Transitioning to an "error" state is equivalent to panicking / terminating a program and should act as a sink in the fsm, while transitioning to different states if something goes wrong is equivalent to a try-catch, and is a perfectly reasonable thing to have. The effects of the "error" state could be to shut down all non-essential systems and alert the user. The problem occurs when the "error" state is implemented in a way that allows the fsm to transition back out of the error state. In such a state machine, you shouldn't have transitioned into the "error" state to begin with - or maybe you don't need the "error" state at all.

If you model states/effects/events properly, you end up with guarantees like "only 1 controller is running at any given time". You also end up with robustness against errors (i.e. the only failure mode is fall over and die, look into exception handling vs fault tolerant design for more info)