Tests & Cleanup

- A variety of more thorough tests bump coverage up to ~86% as well as add more rigor to existing tests
- Finish cleaning up and documenting all core files (in pr2_pbd_interaction/src/)