Extend goto controller to be able to use path following.

Solves #44, yet untested