Update obstacle layer usage of max ranges #5697
Conversation
sd-kudan
force-pushed
the
sdathan/update-obstacle-layer-usage-of-max-range
branch
from
845dced to
be10620
Compare
|
@sd-kudan, your PR has failed to build. Please check CI outputs and resolve issues. |
|
Can you rebase / pull in main (we squash merge anyways) to get CI to turn over? I want to see the tests pass before I dig in. I'm going to need to really dive into code surrounding this for a full review since its a big change with subtle potential issues, so before I do that I want to make sure automated testing doesn't already flag issues that makes my time unnecessary :-) |
Signed-off-by: Simon Dathan <simon.dathan@kudan.eu>
Signed-off-by: Simon Dathan <simon.dathan@kudan.eu>
Signed-off-by: Simon Dathan <simon.dathan@kudan.eu>
Signed-off-by: Simon Dathan <simon.dathan@kudan.eu>
sd-kudan
force-pushed
the
sdathan/update-obstacle-layer-usage-of-max-range
branch
from
be10620 to
cea3222
Compare
| unsigned int origin_cell_x; | ||
| unsigned int origin_cell_y; | ||
| worldToMap(obs.origin_.x, obs.origin_.y, origin_cell_x, origin_cell_y); | ||
| unsigned int point_cell_x; | ||
| unsigned int point_cell_y; | ||
| worldToMap(px, py, point_cell_x, point_cell_y); | ||
| int delta_x = point_cell_x - origin_cell_x; | ||
| int delta_y = point_cell_y - origin_cell_y; | ||
| unsigned int max_delta = std::max(std::abs(delta_x), std::abs(delta_y)); |
There was a problem hiding this comment.
This needs to compute the L2 distance (squared or unsquared) - just takign the maximum of X or Y is not a valid distance calculation for range. I think you're misunderstanding the raytraceLine / bresenham2D algorithm. We are marching in the X or Y direction but we do so scaled from the total length to march the L2 distance.
I think essentially the previous calculation should remain, but based on cell length rather than world coordinates.
There was a problem hiding this comment.
Thanks for taking a look - have updated to use squared distance.
| // Calculate the distance to the endpoint in cells to limit the maximum length of the raytrace | ||
| // and avoid clearing the endpoint cell. | ||
| int delta_x = x1 - x0; | ||
| int delta_y = y1 - y0; | ||
| unsigned int cell_dist_to_endpoint = std::max(std::abs(delta_x), std::abs(delta_y)); | ||
| if (cell_dist_to_endpoint < 1) { | ||
| continue; | ||
| } | ||
| cell_raytrace_max_range = std::min(cell_raytrace_max_range, cell_dist_to_endpoint - 1); |
There was a problem hiding this comment.
All marking is done after all clearing, so this shouldn't be necessary.
navigation2/nav2_costmap_2d/plugins/obstacle_layer.cpp
Lines 520 to 526 in cea3222
There was a problem hiding this comment.
This allows setting raytrace_max_range > obstacle_max_range which can help clear obstacles which are moving away from the sensor, without clearing stationary obstacles while moving away from them for example.
Signed-off-by: Simon Dathan <simon.dathan@kudan.eu>
Codecov Report✅ All modified and coverable lines are covered by tests.
... and 5 files with indirect coverage changes 🚀 New features to boost your workflow:
|
2 participants
Basic Info
Description of contribution in a few bullet points
To address issue reported in #5476 of obstacles being cleared when reversing away from them, the following changes are made:
obstacle_max_rangeis done in grid cell space for consistency with the ray tracing free space check againstraytrace_max_rangeraytrace_max_range > obstacle_max_rangeDescription of documentation updates required from your changes
Description of how this change was tested
Demo
Before:
obstacle_layer_clearing.mp4
After:
obstacle_layer.mp4
Future work that may be required in bullet points
For Maintainers:
backport-*.