Perhaps provide some basic behaviors like - [x] rotate to sp. heading - [x] Drive on heading for X distance (recommission backup plugin) - [x] assisted teleop - [ ] Wiggle (or concat 3 spins) -N deg, +2N deg, -N deg to wiggle back to the original orientation - [ ] Back out of recent buffer of commands bc bad situation as recovery, then retry with another algo - or backward potential field