NavFn overflow check allows gradient extrapolation

[madsciencetist]

In navfn.cpp calcPath(), the gradient is bilinearly interpolated between the cells (x,y), (x+1,y), (x,y+1), and (x+1,y+1). To stay within this range, (dx,dy) must be bounded between 0 and 1. The overflow check, however, bounds them between -1 and 1, meaning dx can move by two cells before the cell index increments once. The result is that negative (dx,dy) values cause gradient extrapolation, which in practice can cause squirrely paths near the path goal (the robot) where the gradient changes sharply.

Simply changing the bounds check to [0, 1] seems to fix the issue and the resulting paths appear smoother.

[madsciencetist]

Also applies to global_planner/gradient_path.cpp