From 9be0d06fadff12dfa5d414778d66b5d5fba26f5f Mon Sep 17 00:00:00 2001 From: Khaled Jalloul Date: Tue, 3 Dec 2024 11:36:42 +0100 Subject: [PATCH] Fix cpplint and uncrustify --- grid_map_core/src/GridMap.cpp | 3 +- grid_map_core/test/GridMapTest.cpp | 108 ++++++++++++++--------------- 2 files changed, 55 insertions(+), 56 deletions(-) diff --git a/grid_map_core/src/GridMap.cpp b/grid_map_core/src/GridMap.cpp index 224da85a5..c65687325 100644 --- a/grid_map_core/src/GridMap.cpp +++ b/grid_map_core/src/GridMap.cpp @@ -820,7 +820,8 @@ Position GridMap::getClosestPositionInMap(const Position & position) const const double minY = bottomRightCorner.y(); // Clip to box constraints and correct for indexing precision. - // Points on the border can lead to invalid indices because the cells represent half open intervals, i.e. [...). + // Points on the border can lead to invalid indices because the cells represent half + // open intervals, i.e. [...). positionInMap.x() = std::fmin(positionInMap.x(), maxX - std::numeric_limits::epsilon()); positionInMap.y() = std::fmin(positionInMap.y(), maxY - std::numeric_limits::epsilon()); diff --git a/grid_map_core/test/GridMapTest.cpp b/grid_map_core/test/GridMapTest.cpp index e621c0b17..ccc5261de 100644 --- a/grid_map_core/test/GridMapTest.cpp +++ b/grid_map_core/test/GridMapTest.cpp @@ -154,12 +154,10 @@ TEST(GridMap, ClipToMap) EXPECT_TRUE(map.isInside(clippedPositionOutMap)); } - - TEST(GridMap, ClipToMap2) { - GridMap map({"types"}); - map.setGeometry(Length(1.0, 1.0), 0.05, Position(0.0, 0.0)); + grid_map::GridMap map({"types"}); + map.setGeometry(grid_map::Length(1.0, 1.0), 0.05, grid_map::Position(0.0, 0.0)); // Test 8 points outside of map. /* @@ -173,187 +171,187 @@ TEST(GridMap, ClipToMap2) * * Note: Position to index alignment is an half open interval. * An example position of 0.5 is assigned to the upper index. - * The interval in the current example is: + * The interval in the current example is: * Position: [...)[0.485 ... 0.5)[0.5 ... 0.505)[...) * Index: 8 9 10 11 */ - Index insideIndex; - Position outsidePosition; + grid_map::Index insideIndex; + grid_map::Position outsidePosition; // Point A - outsidePosition = Position(1.0, 1.0); + outsidePosition = grid_map::Position(1.0, 1.0); auto closestInsidePosition = map.getClosestPositionInMap(outsidePosition); bool isInside = map.getIndex(closestInsidePosition, insideIndex); - auto expectedPosition = Position(0.5, 0.5); - auto expectedIndex = Index(0, 0); + auto expectedPosition = grid_map::Position(0.5, 0.5); + auto expectedIndex = grid_map::Index(0, 0); // Check position. EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x()); EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y()); - + // Check index. EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition; EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition; - + // Check if index is inside. EXPECT_TRUE(isInside) << "position is: " << std::endl - << closestInsidePosition << std::endl + << closestInsidePosition << std::endl << " index is: " << std::endl << insideIndex << std::endl; // Point B - outsidePosition = Position(1.0, 0.0); + outsidePosition = grid_map::Position(1.0, 0.0); closestInsidePosition = map.getClosestPositionInMap(outsidePosition); isInside = map.getIndex(closestInsidePosition, insideIndex); - expectedPosition = Position(0.5, 0.0); - expectedIndex = Index(0, 10); + expectedPosition = grid_map::Position(0.5, 0.0); + expectedIndex = grid_map::Index(0, 10); // Check position. EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x()); EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y()); - + // Check index. EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition; EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition; - + // Check if index is inside. EXPECT_TRUE(isInside) << "position is: " << std::endl - << closestInsidePosition << std::endl + << closestInsidePosition << std::endl << " index is: " << std::endl << insideIndex << std::endl; // Point C - outsidePosition = Position(1.0, -1.0); + outsidePosition = grid_map::Position(1.0, -1.0); closestInsidePosition = map.getClosestPositionInMap(outsidePosition); isInside = map.getIndex(closestInsidePosition, insideIndex); - expectedPosition = Position(0.5, -0.5); - expectedIndex = Index(0, 19); + expectedPosition = grid_map::Position(0.5, -0.5); + expectedIndex = grid_map::Index(0, 19); // Check position. EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x()); EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y()); - + // Check index. EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition; EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition; - + // Check if index is inside. EXPECT_TRUE(isInside) << "position is: " << std::endl - << closestInsidePosition << std::endl + << closestInsidePosition << std::endl << " index is: " << std::endl << insideIndex << std::endl; // Point D - outsidePosition = Position(0.0, 1.0); + outsidePosition = grid_map::Position(0.0, 1.0); closestInsidePosition = map.getClosestPositionInMap(outsidePosition); isInside = map.getIndex(closestInsidePosition, insideIndex); - expectedPosition = Position(0.0, 0.5); - expectedIndex = Index(10, 0); + expectedPosition = grid_map::Position(0.0, 0.5); + expectedIndex = grid_map::Index(10, 0); // Check position. EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x()); EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y()); - + // Check index. EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition; EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition; - + // Check if index is inside. EXPECT_TRUE(isInside) << "position is: " << std::endl - << closestInsidePosition << std::endl + << closestInsidePosition << std::endl << " index is: " << std::endl << insideIndex << std::endl; // Point E - outsidePosition = Position(0.0, -1.0); + outsidePosition = grid_map::Position(0.0, -1.0); closestInsidePosition = map.getClosestPositionInMap(outsidePosition); isInside = map.getIndex(closestInsidePosition, insideIndex); - expectedPosition = Position(0.0, -0.5); - expectedIndex = Index(10, 19); + expectedPosition = grid_map::Position(0.0, -0.5); + expectedIndex = grid_map::Index(10, 19); // Check position. EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x()); EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y()); - + // Check index. EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition; EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition; - + // Check if index is inside. EXPECT_TRUE(isInside) << "position is: " << std::endl - << closestInsidePosition << std::endl + << closestInsidePosition << std::endl << " index is: " << std::endl << insideIndex << std::endl; // Point F - outsidePosition = Position(-1.0, 1.0); + outsidePosition = grid_map::Position(-1.0, 1.0); closestInsidePosition = map.getClosestPositionInMap(outsidePosition); isInside = map.getIndex(closestInsidePosition, insideIndex); - expectedPosition = Position(-0.5, 0.5); - expectedIndex = Index(19, 0); + expectedPosition = grid_map::Position(-0.5, 0.5); + expectedIndex = grid_map::Index(19, 0); // Check position. EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x()); EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y()); - + // Check index. EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition; EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition; - + // Check if index is inside. EXPECT_TRUE(isInside) << "position is: " << std::endl - << closestInsidePosition << std::endl + << closestInsidePosition << std::endl << " index is: " << std::endl << insideIndex << std::endl; // Point G - outsidePosition = Position(-1.0, 0.0); + outsidePosition = grid_map::Position(-1.0, 0.0); closestInsidePosition = map.getClosestPositionInMap(outsidePosition); isInside = map.getIndex(closestInsidePosition, insideIndex); - expectedPosition = Position(-0.5, 0.0); - expectedIndex = Index(19, 10); + expectedPosition = grid_map::Position(-0.5, 0.0); + expectedIndex = grid_map::Index(19, 10); // Check position. EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x()); EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y()); - + // Check index. EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition; EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition; - + // Check if index is inside. EXPECT_TRUE(isInside) << "position is: " << std::endl - << closestInsidePosition << std::endl + << closestInsidePosition << std::endl << " index is: " << std::endl << insideIndex << std::endl; // Point H - outsidePosition = Position(-1.0, -1.0); + outsidePosition = grid_map::Position(-1.0, -1.0); closestInsidePosition = map.getClosestPositionInMap(outsidePosition); isInside = map.getIndex(closestInsidePosition, insideIndex); - expectedPosition = Position(-0.5, -0.5); - expectedIndex = Index(19, 19); + expectedPosition = grid_map::Position(-0.5, -0.5); + expectedIndex = grid_map::Index(19, 19); // Check position. EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x()); EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y()); - + // Check index. EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition; EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition; - + // Check if index is inside. EXPECT_TRUE(isInside) << "position is: " << std::endl - << closestInsidePosition << std::endl + << closestInsidePosition << std::endl << " index is: " << std::endl << insideIndex << std::endl; }