Initial implementation using splines.

It does not work moving backward.
It often fails because the goal is outside the local costmap.