Update obstacle layer usage of max ranges

[sd-kudan]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#5476
Primary OS tested on	Ubuntu
Robotic platform tested on	gazebo simulation
Does this PR contain AI generated software?	No
Was this PR description generated by AI software?	No

---

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:

• When marking obstacles from new observations, the check against obstacle_max_range is done in grid cell space for consistency with the ray tracing free space check against raytrace_max_range
• When ray tracing, excluded the cell containing the observation point so that an obstacle is not cleared if it is present in the current measurement but raytrace_max_range > obstacle_max_range

Description of documentation updates required from your changes

Description of how this change was tested

• Unit tests added for obstacle layer marking and clearing
• Tested in gazebo simulation

Demo

Before:

obstacle_layer_clearing.mp4

After:

obstacle_layer.mp4

---

Future work that may be required in bullet points

For Maintainers:

• Check that any new parameters added are updated in docs.nav2.org
• Check that any significant change is added to the migration guide
• Check that any new features OR changes to existing behaviors are reflected in the tuning guide
• Check that any new functions have Doxygen added
• Check that any new features have test coverage
• Check that any new plugins is added to the plugins page
• If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists
• Should this be backported to current distributions? If so, tag with backport-*.

[mergify]

@sd-kudan, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

[SteveMacenski]

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 :-)

[codecov]

Codecov Report

✅ All modified and coverable lines are covered by tests.

Files with missing lines	Coverage Δ
nav2_costmap_2d/plugins/obstacle_layer.cpp	82.67% <100.00%> (+0.43%)	⬆️

... and 5 files with indirect coverage changes

🚀 New features to boost your workflow:

• ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

[SteveMacenski]

I think you have some functional problems here - as well as a few stylistic thing we should clean up that linters / ROS styling doesn't support

[SteveMacenski]

Please remove - I'm not sure this is important considering the marking observation is triggered after the clearing.

[sd-kudan]

I just wanted to check - I had added this to handle the case where raytrace_max_range > obstacle_max_range. In that case the cell might not be subsequently marked. Setting raytrace_max_range > obstacle_max_range helps avoid ghost obstacles lingering in the map when an obstacle is moving away from the sensor.
If that's not the behavior you'd expect with this configuration I'll remove.

[SteveMacenski]

That does beg a question, I agree. If the marking reading doesn't meet obstacle_max_range, then its not going to be marked no matter what. I don't think it makes sense to cull clearing when marking was set not to cover this spectrum.

I agree that I would be concerned about if they were equal and some small casting error resulted in marking being OOB but clearing was able to happen, but I think that as something in this PR we can make sure we compute the same as to make sure this doesn't happen.

We do marking after clearing, so I think there's not a validly possible case that this should cover:

1. Mark after clear, so if marking is possible, it should mark even if cleared previously
2. If the ranges are the same, we should ensure in this PR that there's no floating point / casting error that would result in a clearing operation that marking doesn't also cover, as a point of technical correctness
3. If the obstacle range is smaller, then we know that we're going to clearing marked things 1-N cells further, so that seems like an intentional choice of settings

But, please, push back if I'm missing a key detail that you're looking out for

[sd-kudan]

I think it comes down to interpretation of what should happen when raytracing and what a "clearing observation" means. My expectation is to clear up to but excluding the cell containing the observed point, since we have no evidence that the cell is empty. So for point 3, if we have an observation beyond obstacle_max_range I wouldn't expect it to be cleared, even though we have decided not to mark it. Happy to remove if you disagree or if you think it could cause problems.

[SteveMacenski]

I think we should remove in this context. I generally agree with you, but this is 10+ year old code and I'd rather not disrupt things too much in such a sensitive location in the codebase. Unless there's a bug, I'd like to avoid potentially creating one for some subset of users that use the clearing / marking in unique ways.

One way I think this could fail is when the raymarching is aliased due to the ray not being aligned with the cell axes. If we back out one full cell, that could actually be multiple cells that are hit. I think our raycasting uses full 1 cell increments so it wouldn't change, but if we did it in say 0.5 cells (or later changed it to another GPU accelerated library with finer checks), then we'd have an obstacle that should have been cleared because we didn't raycast far enough.

[sd-kudan]

Understood - I have removed this. I have also removed skipping raytracing the origin cell when observation_dist == 0 as it's just handling a special case of this change. See #5697 (comment)

[SteveMacenski]

Last question: casting values always truncates the number, so for observation_dist, is it intended to always be the shortest and not round?

[sd-kudan]

Last question: casting values always truncates the number, so for observation_dist, is it intended to always be the shortest and not round?

Yes - I think truncation is consistent as that's what is done in raytraceLine. However I think this highlights a problem with my usage of squared distance - discritization is different in squared cell space. I've updated to use std::hypot instead, let me know if you think this is more appropriate.

[mini-1235]

@sd-kudan, please pull in / rebase main, we had some CI issues earlier

[SteveMacenski]

@mini-1235 see the failure now

[mini-1235]

@mini-1235 see the failure now

I'm aware, but still unable to reproduce this locally 🫠

[SteveMacenski]

Otherwise LGTM

Sorry for these death-by-a-thousand-cuts reviews. I really do appreciate this! I promise other less low-level things will be easier in the future 😉

[SteveMacenski]

See #5697 (comment) and then we can merge :-)

[SteveMacenski]

Merged!

Again, sorry about the death by a thousand cuts. That is really not the experience I try to have with developers. I appreicate your willingness to iterate. I expect future contributions to be easier 😉

[sd-kudan]

Again, sorry about the death by a thousand cuts.

No problem, got there in the end 😄

[sd-kudan]

Hi @SteveMacenski what is the process for backporting? Should I create a PR for jazzy?

[mini-1235]

@sd-kudan Yes, please open a manual backport PR to Jazzy. I can see there are some conflicts when cherry-picking, so we can't use the Mergify bot for this one