ROS Waypoint Selection Node

[EpicWolverine]

[EpicWolverine]

I have the waypoint loading done here but it doesn't do anything with it yet.

Should we change this to a service and just have Navigation PID handle checking if we're there and just ask this node for the next point each time? This also means Auto Control Logic would to get drive speed and direction from Navigation PID instead.