Merge branch 'fix/get_transformed_map' into 'master'

[grid_map] Fix for getIndex() function in cases of suboptimal position arguments Closes #UNKNOWN GitOrigin-RevId: 740443e4f3aad9fe495a565b557b3e46cd22e417
BruceChanJianLe · Aug 13, 2020 · f0dd780 · f0dd780
1 parent 3517962
commit f0dd780
Show file tree

Hide file tree

Showing 2 changed files with 55 additions and 2 deletions.
diff --git a/grid_map_core/src/GridMapMath.cpp b/grid_map_core/src/GridMapMath.cpp
@@ -138,8 +138,7 @@ bool getIndexFromPosition(Index& index,
   getVectorToOrigin(offset, mapLength);
   Vector indexVector = ((position - offset - mapPosition).array() / resolution).matrix();
   index = getIndexFromIndexVector(indexVector, bufferSize, bufferStartIndex);
-  if (!checkIfPositionWithinMap(position, mapLength, mapPosition)) return false;
-  return true;
+  return checkIfPositionWithinMap(position, mapLength, mapPosition) && checkIfIndexInRange(index, bufferSize);
 }
 
 bool checkIfPositionWithinMap(const Position& position,

diff --git a/grid_map_core/test/GridMapMathTest.cpp b/grid_map_core/test/GridMapMathTest.cpp
@@ -184,7 +184,61 @@ TEST(checkIfPositionWithinMap, EdgeCases)
   Length mapLength(2.0, 3.0);
   Position mapPosition(0.0, 0.0);
 
+  /*
+  *  
+  *  A (is inside)             B (is not inside)
+  *   +-----------------------+
+  *   |                       |
+  *   |                       |
+  *   |              X        |
+  *   |             ^         |
+  *   |             |         |
+  *   |             |         |
+  *   |       <-----+         |
+  *   |      Y                |
+  *   |                       |
+  *   |                       |
+  *   |                       |
+  *   +-----------------------+
+  *  C (is not inside)         D (is not inside)
+  *
+  * Resulting coordinates are:
+  *  A: (1.0, 1.5)
+  *  B: (1.0, -1.5)
+  *  C: (-1.0, 1.5)
+  *  D: (-1.0, -1.5)
+  *
+  */
+
+  // Noise around A.
+  EXPECT_TRUE(checkIfPositionWithinMap(Position(1.0, 1.5), mapLength, mapPosition));
+  EXPECT_FALSE(checkIfPositionWithinMap(Position(1.0 + DBL_EPSILON, 1.5), mapLength, mapPosition));
+  EXPECT_TRUE(checkIfPositionWithinMap(Position(1.0 - DBL_EPSILON, 1.5), mapLength, mapPosition));
+  EXPECT_FALSE(checkIfPositionWithinMap(Position(1.0, 1.5 + DBL_EPSILON), mapLength, mapPosition));
+  EXPECT_TRUE(checkIfPositionWithinMap(Position(1.0, 1.5 - DBL_EPSILON), mapLength, mapPosition));
+
+  // Noise around B.
   EXPECT_FALSE(checkIfPositionWithinMap(Position(1.0, -1.5), mapLength, mapPosition));
+  EXPECT_FALSE(checkIfPositionWithinMap(Position(1.0 + DBL_EPSILON, - 1.5), mapLength, mapPosition));
+  EXPECT_FALSE(checkIfPositionWithinMap(Position(1.0 - DBL_EPSILON, - 1.5), mapLength, mapPosition));
+  EXPECT_FALSE(checkIfPositionWithinMap(Position(1.0, - 1.5 + DBL_EPSILON), mapLength, mapPosition));
+  EXPECT_FALSE(checkIfPositionWithinMap(Position(1.0, - 1.5 - DBL_EPSILON), mapLength, mapPosition));
+
+  // Noise around C.
+  EXPECT_FALSE(checkIfPositionWithinMap(Position(-1.0, 1.5), mapLength, mapPosition));
+  EXPECT_TRUE(checkIfPositionWithinMap(Position(-1.0 + DBL_EPSILON, 1.5), mapLength, mapPosition));
+  EXPECT_FALSE(checkIfPositionWithinMap(Position(-1.0 - DBL_EPSILON, 1.5), mapLength, mapPosition));
+  EXPECT_FALSE(checkIfPositionWithinMap(Position(-1.0, 1.5 + DBL_EPSILON), mapLength, mapPosition));
+  EXPECT_FALSE(checkIfPositionWithinMap(Position(-1.0, 1.5 - DBL_EPSILON), mapLength, mapPosition));
+
+  // Noise around D.
+  EXPECT_FALSE(checkIfPositionWithinMap(Position(-1.0, -1.5), mapLength, mapPosition));
+  EXPECT_FALSE(checkIfPositionWithinMap(Position(-1.0 + DBL_EPSILON, -1.5), mapLength, mapPosition));
+  EXPECT_FALSE(checkIfPositionWithinMap(Position(-1.0 - DBL_EPSILON, -1.5), mapLength, mapPosition));
+  EXPECT_FALSE(checkIfPositionWithinMap(Position(-1.0, -1.5 + DBL_EPSILON), mapLength, mapPosition));
+  EXPECT_FALSE(checkIfPositionWithinMap(Position(-1.0, -1.5 - DBL_EPSILON), mapLength, mapPosition));
+
+  // Extra tests.
   EXPECT_FALSE(checkIfPositionWithinMap(Position(-1.0, 1.5), mapLength, mapPosition));
   EXPECT_FALSE(checkIfPositionWithinMap(Position(1.0 + DBL_EPSILON, 1.0), mapLength, mapPosition));
   EXPECT_TRUE(checkIfPositionWithinMap(Position((2.0 + DBL_EPSILON) / 2.0, 1.0), mapLength, mapPosition));