Update padFootprint function #108
Conversation
This comment was marked as duplicate.
This comment was marked as duplicate.
Sorry, something went wrong.
There was a problem hiding this comment.
3 things
The 1st example looks suspicious: doesn't look like padded 10 cm but 2 or 3 🤔
This looks similar to this commit
Do both provide the same result? If so, this is simpler
Keep the formatting of the files you modify. i.e. never mix stiles
Hi you are right it wasnt 0.1 meters because it was scaling by percentage, I changed it to scale by meters and updated the image. |
So the outcome is now the same as with this commit? |
@corot I think you are missing the part he calculates the centroid of the polygon. |
costmap_2d/src/footprint.cpp
Outdated
| 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(); |
There was a problem hiding this comment.
S: I think it makes sense to create a method centroid that returns geometry_msgs::Point
costmap_2d/src/footprint.cpp
Outdated
| double dx = point.x - footprint_center.x; | ||
| double dy = point.y - footprint_center.y; | ||
|
|
||
| double distance = std::sqrt(dx * dx + dy * dy); |
|
I think it makes sense to make a PR upstream: |
costmap_2d/test/footprint_tests.cpp
Outdated
| 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 ); |
There was a problem hiding this comment.
Q: do we really need to change round numbers?
There was a problem hiding this comment.
yes because otherwise it fails
There was a problem hiding this comment.
ok, as @renan028 explained, this implementation is superior to the one I mention
@Nisarg236 , you should consider open a PR to nav2, as this is a nice contribution
AB#44902
Previously this function was scaling the footprint with respect to the coordinate frame 0,0 which could result in distortion. Also when the centroid is having large offset with 0,0 then it will just displace the footprint instead of padding.
Now it scales with respect to the centroid of the footprint.
another case (10,10), (10,12), (12,12) , (12,10)
another case (0,0), (0,2), (2,2), (2,0)
I have also tested the behaviour with negative values of padding (shrinking the footprint)