Fix obstacles in direct planing

introduced a param for the speed ramp to build up speed smother

added a distance to the waypoint reached calculation to fix the pillar error we had earlier