Autonomy state management #223
Description
Figure out a good way to manage different states and transitions for this year's autonomy system.
A behavior tree library would probably be the simplest way to go, and the same is used for the standard nav2 project.
py_trees seems promising. If performance becomes a big concern, BehaviorTree.cpp looks like a great alternative.