Added forward obstacle checking