Skip to content
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.

Sign up for GitHub

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

Already on GitHub? Sign in to your account

Update padFootprint function #108

Merged
merged 8 commits into from
Jan 6, 2025
Merged

Update padFootprint function #108

Show file tree
Hide file tree
Changes from 5 commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
f8cd41f
scale wrt centroid
Nisarg236 Dec 25, 2024
f7443af
minor fix
Nisarg236 Dec 25, 2024
1eac1c9
update unit tests
Nisarg236 Dec 25, 2024
e47bca8
update unit tests
Nisarg236 Dec 25, 2024
7f99c9c
scale by meters not percentage
Nisarg236 Dec 26, 2024
b0f6775
separate function for centroid and use std::hypot
Nisarg236 Jan 6, 2025
2b65725
update test case
Nisarg236 Jan 6, 2025
c646145
update test case
Nisarg236 Jan 6, 2025
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
27 changes: 22 additions & 5 deletions costmap_2d/src/footprint.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -137,12 +137,29 @@ void transformFootprint(double x, double y, double theta, const std::vector<geom

void padFootprint(std::vector<geometry_msgs::Point>& footprint, double padding)
{
// pad footprint in place
for (unsigned int i = 0; i < footprint.size(); i++)
geometry_msgs::Point footprint_center;
double sum_x = 0.0, sum_y = 0.0;

for (const auto& point : footprint)
{
sum_x += point.x;
sum_y += point.y;
}
footprint_center.x = sum_x / footprint.size();
footprint_center.y = sum_y / footprint.size();
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

S: I think it makes sense to create a method centroid that returns geometry_msgs::Point


for (auto& point : footprint)
{
geometry_msgs::Point& pt = footprint[ i ];
pt.x += sign0(pt.x) * padding;
pt.y += sign0(pt.y) * padding;
double dx = point.x - footprint_center.x;
double dy = point.y - footprint_center.y;

double distance = std::sqrt(dx * dx + dy * dy);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

S: std::hypot


if (distance > 0.0)
{
point.x += padding * (dx / distance);
point.y += padding * (dy / distance);
}
}
}

Expand Down
20 changes: 10 additions & 10 deletions costmap_2d/test/footprint_tests.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -70,17 +70,17 @@ TEST( Costmap2DROS, padded_footprint_from_string_param )
std::vector<geometry_msgs::Point> footprint = cm.getRobotFootprint();
EXPECT_EQ( 3, footprint.size() );

EXPECT_EQ( 1.5f, footprint[ 0 ].x );
EXPECT_EQ( 1.5f, footprint[ 0 ].y );
EXPECT_EQ( 0.0f, footprint[ 0 ].z );
EXPECT_EQ( 1.66667f, footprint[0].x );
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Q: do we really need to change round numbers?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

yes because otherwise it fails

EXPECT_EQ( 1.33333f, footprint[0].y );
EXPECT_EQ( 0.0f, footprint[0].z );

EXPECT_EQ( -1.5f, footprint[ 1 ].x );
EXPECT_EQ( 1.5f, footprint[ 1 ].y );
EXPECT_EQ( 0.0f, footprint[ 1 ].z );
EXPECT_EQ( -1.33333f, footprint[1].x );
EXPECT_EQ( 1.33333f, footprint[1].y );
EXPECT_EQ( 0.0f, footprint[1].z );

EXPECT_EQ( -1.5f, footprint[ 2 ].x );
EXPECT_EQ( -1.5f, footprint[ 2 ].y );
EXPECT_EQ( 0.0f, footprint[ 2 ].z );
EXPECT_EQ( -1.33333f, footprint[2].x );
EXPECT_EQ( -1.66667f, footprint[2].y );
EXPECT_EQ( 0.0f, footprint[2].z );
}

TEST( Costmap2DROS, radius_param )
Expand Down Expand Up @@ -155,7 +155,7 @@ TEST( Costmap2DROS, footprint_empty )
// With no specification of footprint or radius, defaults to 0.46 meter radius plus 0.01 meter padding.
EXPECT_EQ( 16, footprint.size() );

EXPECT_NEAR( 0.47f, footprint[ 0 ].x, 0.0001 );
EXPECT_NEAR( 0.47f, footprint[ 0 ].x, 0.001 );
EXPECT_NEAR( 0.0f, footprint[ 0 ].y, 0.0001 );
EXPECT_EQ( 0.0f, footprint[ 0 ].z );
}
Expand Down
Loading