Added part of ompl vehicle example