First pass of the Smoooooth local planner