Hey @shoemakerlevy9, cool project 
In terms of the state machine, here are some resources that might be of interest:
- SMACH tutorials: http://wiki.ros.org/action/show/smach/Tutorials
- My fork of their hard-to-find source code: https://github.com/spmaniato/executive_smach_tutorials
- FlexBE (Flexible Behavior Engine): flexbe.github.io
- Source code of its core: https://github.com/team-vigir/flexbe_behavior_engine
- GitHub organization (states and GUI app): https://github.com/FlexBE
- FlexBE is an extension of SMACH, but may or may not fit your use case.
- I was somewhat involved in the development of FlexBE, so feel free to ask follow-up questions.
- Boost Statechart (C++): http://www.boost.org/doc/libs/1_64_0/libs/statechart/doc/index.html
- Steep learning curve but much faster and more expressive.