Movatterモバイル変換

[0]ホーム
URL:

Skip to content

Navigation Menu

Sign in
Appearance settings

Search code, repositories, users, issues, pull requests...

Provide feedback

We read every piece of feedback, and take your input very seriously.

Saved searches

Use saved searches to filter your results more quickly

 Sign up
Appearance settings

[ROS 2][grid_map_core] Fix point clipping f1344bd#504

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to ourterms of service andprivacy statement. We’ll occasionally send you account related emails.

Already on GitHub?Sign in to your account

Open
kjalloul-anybotics wants to merge2 commits intoANYbotics:rolling
base:rolling
Choose a base branch
Loading
fromkjalloul-anybotics:ros2_update/fix_point_clipping
Open
Show file tree
Hide file tree
Changes from1 commit
Commits
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
PrevPrevious commit
Fix cpplint and uncrustify
  • Loading branch information
@kjalloul-anybotics
kjalloul-anybotics committedDec 3, 2024
commit9be0d06fadff12dfa5d414778d66b5d5fba26f5f
3 changes: 2 additions & 1 deletiongrid_map_core/src/GridMap.cpp
View file
Open in desktop
Original file line numberDiff line numberDiff 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 deletionsgrid_map_core/test/GridMapTest.cpp
View file
Open in desktop
Original file line numberDiff line numberDiff 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
Loading
[8]ページ先頭
©2009-2025 Movatter.jp