Minor fix in getClosestPosition to avoid wrong indices

[mmattamala]

Hi, this is a minor fix when using getClosestPosition() with positions outside the grid map.

The actual implementation used the std::epsilon value to avoid this but I still observed some issues when calling getIndex(). I used the same factor of 10 for epsilon used here and seems to do the trick (I guess that just using epsilon is still too tight)

[maximilianwulf]

Thank you very much. Could you check if you could add this scenario as a unit test as well? This test should give some inspiration.

[mmattamala]

Thanks @maximilianwulf. I'll add the unit test and will ask your re-review.

[mmattamala]

Hi @maximilianwulf, thanks for asking for the test, it made me realize the solution I was proposing is not solving the real problem.

I pushed a test (ClipToMap3) that replicates the same settings that caused the original failure on my code. It is a 4x4m map with 0.02m resolution (200x200 cells).

It breaks when trying to clip a point to the lower bounds and clips the indices to 200 instead of 199 (I attached the output below). It doesn't break with the settings in ClipToMap2 which uses a 1x1m, 0.05 map (20x20 cells).

I ran out of places to look at, but if you have any suggestions I can go further.

Output:

/home/matias/catkin_ws/src/grid_map/grid_map_core/test/GridMapTest.cpp:447: Failure
Expected equality of these values:
  expectedIndex.y()
    Which is: 199
  insideIndex.y()
    Which is: 200
closestInsidePosition 1.9999999999999998
-1.9999999999999998
/home/matias/catkin_ws/src/grid_map/grid_map_core/test/GridMapTest.cpp:450: Failure
Value of: isInside
  Actual: false
Expected: true
position is: 
 1.9999999999999998
-1.9999999999999998
 index is: 
  0
200

/home/matias/catkin_ws/src/grid_map/grid_map_core/test/GridMapTest.cpp:493: Failure
Expected equality of these values:
  expectedIndex.y()
    Which is: 199
  insideIndex.y()
    Which is: 200
closestInsidePosition                  0
-1.9999999999999998
/home/matias/catkin_ws/src/grid_map/grid_map_core/test/GridMapTest.cpp:496: Failure
Value of: isInside
  Actual: false
Expected: true
position is: 
                  0
-1.9999999999999998
 index is: 
100
200

/home/matias/catkin_ws/src/grid_map/grid_map_core/test/GridMapTest.cpp:516: Failure
Expected equality of these values:
  expectedIndex.x()
    Which is: 199
  insideIndex.x()
    Which is: 200
closestInsidePosition-1.9999999999999998
 1.9999999999999998
/home/matias/catkin_ws/src/grid_map/grid_map_core/test/GridMapTest.cpp:520: Failure
Value of: isInside
  Actual: false
Expected: true
position is: 
-1.9999999999999998
 1.9999999999999998
 index is: 
200
  0

/home/matias/catkin_ws/src/grid_map/grid_map_core/test/GridMapTest.cpp:539: Failure
Expected equality of these values:
  expectedIndex.x()
    Which is: 199
  insideIndex.x()
    Which is: 200
closestInsidePosition-1.9999999999999998
                  0
/home/matias/catkin_ws/src/grid_map/grid_map_core/test/GridMapTest.cpp:543: Failure
Value of: isInside
  Actual: false
Expected: true
position is: 
-1.9999999999999998
                  0
 index is: 
200
100

/home/matias/catkin_ws/src/grid_map/grid_map_core/test/GridMapTest.cpp:563: Failure
Expected equality of these values:
  expectedIndex.x()
    Which is: 199
  insideIndex.x()
    Which is: 200
closestInsidePosition-1.9999999999999998
-1.9999999999999998
/home/matias/catkin_ws/src/grid_map/grid_map_core/test/GridMapTest.cpp:564: Failure
Expected equality of these values:
  expectedIndex.y()
    Which is: 199
  insideIndex.y()
    Which is: 200
closestInsidePosition-1.9999999999999998
-1.9999999999999998
/home/matias/catkin_ws/src/grid_map/grid_map_core/test/GridMapTest.cpp:567: Failure
Value of: isInside
  Actual: false
Expected: true
position is: 
-1.9999999999999998
-1.9999999999999998
 index is: 
200
200

[maximilianwulf]

Thank you, I will import it.