Need a test harness for algorithms

We need to find a modular way to be able to take in a cost map and position/pose and then output navigation goals for the robot.

Probably will require lots of white boarding to get it right the first time.

The idea is to have everything set up for different algorithms to be drop-in replacements for each other and for them to be independent of the robots (v2 or v3).