Implemented TPIK and behavior tree

[evan-palmer]

Checklist

• [x] I have performed a thorough review of my code
• [x] I have sufficiently commented my code
• [x] The implementation follows the project style conventions
• [x] All project unit tests are passing
• [x] If relevant, documentation has been provided or updated to discuss the changes made
• [x] System integration tests were performed successfully

Changes Made

This is an initial version of the project which implements support for TPIK and a behavior tree for system-level autonomy.

Testing

There are still plenty of things to work on and to improve, but this will get an initial version ready.