Skip to content

Commit

Permalink
Fix cpplint and uncrustify
Browse files Browse the repository at this point in the history
  • Loading branch information
@kjalloul-anybotics
kjalloul-anybotics committed Dec 3, 2024
1 parent 3be5358 commit 9be0d06
Show file tree
Hide file tree
Showing 2 changed files with 55 additions and 56 deletions.
3 changes: 2 additions & 1 deletion grid_map_core/src/GridMap.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<double>::epsilon());
positionInMap.y() = std::fmin(positionInMap.y(), maxY - std::numeric_limits<double>::epsilon());

Expand Down
108 changes: 53 additions & 55 deletions grid_map_core/test/GridMapTest.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
/*
Expand All @@ -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;
}
Expand Down

0 comments on commit 9be0d06

Please sign in to comment.