Currently the SplinedRelaxedAStar planner is working but still in the shape the ROS plugin demanded. Change that to be more standalone