Updates to move to using a custom robot state space. Cleaning the headers that were in the src directory out, moving them to include directory.