Merge pull request #275 from BruceChanJianLe:master

GitOrigin-RevId: 2ccf404f896965a2d512814befae59fb1f9b0d45
ANYbotics · Jan 15, 2021 · efd69a8 · efd69a8
2 parents 381b30e + 555c5ee
commit efd69a8
Showing 1 changed file with 26 additions and 0 deletions.
diff --git a/grid_map_core/test/GridMapTest.cpp b/grid_map_core/test/GridMapTest.cpp
@@ -489,3 +489,29 @@ TEST(ValueAtPosition, LinearInterpolated)
   value = map.atPosition("types", Position(0.69,0.38), InterpolationMethods::INTER_LINEAR);
   EXPECT_NEAR(2.1963200, value, 0.0000001);
 }
+
+TEST(GridMap, PositiveClosestPositionInMap)
+{
+  GridMap map({"types"});
+  map.setGeometry(Length(1.0, 1.0), 0.05, Position(0.0, 0.0));
+
+  Position max_positive_position (1.0, 1.0);
+
+  auto closest_positive_position = map.getClosestPositionInMap(max_positive_position);
+  Index idx;
+
+  EXPECT_TRUE(map.getIndex(closest_positive_position, idx));
+}
+
+TEST(GridMap, NegativeClosestPositionInMap)
+{
+  GridMap map({"types"});
+  map.setGeometry(Length(1.0, 1.0), 0.05, Position(0.0, 0.0));
+
+  Position max_negative_position (-1.0, -1.0);
+
+  auto closest_negative_position = map.getClosestPositionInMap(max_negative_position);
+  Index idx;
+
+  EXPECT_TRUE(map.getIndex(closest_negative_position, idx));
+}