Update obstacle layer usage of max ranges by sd-kudan · Pull Request #5697 · ros-navigation/navigation2

sd-kudan · 2025-11-20T10:07:10Z

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 · 2025-11-20T11:27:26Z

@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 · 2025-11-24T22:04:05Z

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>

nav2_costmap_2d/plugins/obstacle_layer.cpp

Signed-off-by: Simon Dathan <simon.dathan@kudan.eu>

codecov · 2025-11-28T11:11:44Z

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

nav2_costmap_2d/plugins/obstacle_layer.cpp

Signed-off-by: Simon Dathan <simon.dathan@kudan.eu>

…acle-layer-usage-of-max-range

nav2_costmap_2d/plugins/obstacle_layer.cpp

SteveMacenski · 2025-12-04T15:47:20Z

nav2_costmap_2d/plugins/obstacle_layer.cpp

+    // Reduce the observation distance by 1 cell so that the cell containing the
+    // observation is not cleared
+    cell_raytrace_max_range =
+      std::min(cell_raytrace_max_range, observation_dist - 1);


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

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.

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:

Mark after clear, so if marking is possible, it should mark even if cleared previously

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

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

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.

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.

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)

nav2_costmap_2d/plugins/obstacle_layer.cpp

SteveMacenski · 2025-12-04T15:51:41Z

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

Signed-off-by: Simon Dathan <simon.dathan@kudan.eu>

sd-kudan · 2025-12-04T17:56:11Z

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 · 2025-12-05T09:15:59Z

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

…acle-layer-usage-of-max-range

SteveMacenski · 2025-12-05T18:25:35Z

@mini-1235 see the failure now

mini-1235 · 2025-12-05T18:36:22Z

@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 😉

nav2_costmap_2d/plugins/obstacle_layer.cpp

Signed-off-by: Simon Dathan <simon.dathan@kudan.eu>

SteveMacenski · 2025-12-08T19:14:06Z

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

Signed-off-by: Simon Dathan <simon.dathan@kudan.eu>

…acle-layer-usage-of-max-range

Signed-off-by: Simon Dathan <simon.dathan@kudan.eu>

SteveMacenski · 2025-12-09T18:57:55Z

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 · 2025-12-10T09:51:47Z

Again, sorry about the death by a thousand cuts.

No problem, got there in the end 😄

* Use cell distance for obstacle marking max range Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Don't raytrace clear the cell containing the current observation Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Add tests for max marking and clearing ranges Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Fix cpplint failures Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Fix distance calculation Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * fix casting, formatting Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * check origin is in map, update CMakeLists Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * use hypot instead of squared dist Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Move origin calc out of loop Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Revert don't raytrace observation cell Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Revert don't raytrace origin if it is observation cell Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Remove new line Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> --------- Signed-off-by: Simon Dathan <simon.dathan@kudan.eu>

sd-kudan · 2025-12-11T09:12:23Z

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

mini-1235 · 2025-12-11T10:28:58Z

@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

* Use cell distance for obstacle marking max range Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Don't raytrace clear the cell containing the current observation Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Add tests for max marking and clearing ranges Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Fix cpplint failures Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Fix distance calculation Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * fix casting, formatting Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * check origin is in map, update CMakeLists Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * use hypot instead of squared dist Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Move origin calc out of loop Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Revert don't raytrace observation cell Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Revert don't raytrace origin if it is observation cell Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Remove new line Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> --------- Signed-off-by: Simon Dathan <simon.dathan@kudan.eu>

* Use cell distance for obstacle marking max range Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Don't raytrace clear the cell containing the current observation Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Add tests for max marking and clearing ranges Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Fix cpplint failures Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Fix distance calculation Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * fix casting, formatting Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * check origin is in map, update CMakeLists Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * use hypot instead of squared dist Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Move origin calc out of loop Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Revert don't raytrace observation cell Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Revert don't raytrace origin if it is observation cell Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Remove new line Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> --------- Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> Signed-off-by: Decwest <fumiyaonishi1016@gmail.com>

* Update obstacle layer usage of max ranges (#5697) * Use cell distance for obstacle marking max range Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Don't raytrace clear the cell containing the current observation Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Add tests for max marking and clearing ranges Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Fix cpplint failures Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Fix distance calculation Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * fix casting, formatting Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * check origin is in map, update CMakeLists Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * use hypot instead of squared dist Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Move origin calc out of loop Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Revert don't raytrace observation cell Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Revert don't raytrace origin if it is observation cell Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Remove new line Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> --------- Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Fix tests for jazzy Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> --------- Signed-off-by: Simon Dathan <simon.dathan@kudan.eu>

* Use cell distance for obstacle marking max range Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Don't raytrace clear the cell containing the current observation Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Add tests for max marking and clearing ranges Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Fix cpplint failures Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Fix distance calculation Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * fix casting, formatting Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * check origin is in map, update CMakeLists Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * use hypot instead of squared dist Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Move origin calc out of loop Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Revert don't raytrace observation cell Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Revert don't raytrace origin if it is observation cell Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Remove new line Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> --------- Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> Signed-off-by: Christopher Thompson <cthompson@metalsharkboats.com>

* Use cell distance for obstacle marking max range Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Don't raytrace clear the cell containing the current observation Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Add tests for max marking and clearing ranges Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Fix cpplint failures Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Fix distance calculation Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * fix casting, formatting Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * check origin is in map, update CMakeLists Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * use hypot instead of squared dist Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Move origin calc out of loop Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Revert don't raytrace observation cell Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Revert don't raytrace origin if it is observation cell Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Remove new line Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> --------- Signed-off-by: Simon Dathan <simon.dathan@kudan.eu>

* No need to spam the logs (#5674) Signed-off-by: Tim Clephas <tim.clephas@nobleo.nl> * Conditionally call onLoop based on node status (#5700) Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Allow for no progress checkers to be configured (#5701) Signed-off-by: SteveMacenski <stevenmacenski@gmail.com> * Fix: wait for drive_on_heading_client instead of backup_client (#5724) The basic navigator was waiting for the backup_client in the driveOnHeading function. Signed-off-by: agennart <antoine.gennart@quimesis.be> Co-authored-by: agennart <antoine.gennart@quimesis.be> * Fix: reset controller_server loop_rate when missed desired rate #5712 (#5723) When a single iteration takes longer than the expected period, this impacts the next iterations behavior since they will have less time to perform the control logic, thus increasing the actual controller_frequency. By resetting the loop_rate on sleep() failure, this ensures that each iteration will have a full period to exectue its logic. Signed-off-by: agennart <antoine.gennart@quimesis.be> Co-authored-by: agennart <antoine.gennart@quimesis.be> * temporary fix bug null pointer (#5749) * temporary fix bug null pointer Signed-off-by: suifengersan123 <yangabc810@gmail.com> * add return Signed-off-by: suifengersan123 <yangabc810@gmail.com> * remove return Signed-off-by: suifengersan123 <yangabc810@gmail.com> --------- Signed-off-by: suifengersan123 <yangabc810@gmail.com> * fix: change warning to exception when goal poses cannot be transformed (#5759) - Replace RCLCPP_WARN with std::runtime_error exception in onPreempt method - Remove bt_action_server_->terminatePendingGoal() call after accepting goal - Add required stdexcept include for exception handling Fixes issue where navigators accept pending goal but are not properly terminated if rejected during goal pose transformation. Co-authored-by: claude[bot] <209825114+claude[bot]@users.noreply.github.com> * Update obstacle layer usage of max ranges (#5697) * Use cell distance for obstacle marking max range Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Don't raytrace clear the cell containing the current observation Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Add tests for max marking and clearing ranges Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Fix cpplint failures Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Fix distance calculation Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * fix casting, formatting Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * check origin is in map, update CMakeLists Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * use hypot instead of squared dist Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Move origin calc out of loop Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Revert don't raytrace observation cell Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Revert don't raytrace origin if it is observation cell Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Remove new line Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> --------- Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * [mppi] Don't reset zone-based speed limits (#5768) * [mppi] Don't reset zone-based speed limits Signed-off-by: Adi Vardi <adi.vardi@enway.ai> * [mppi] update motion model params from speed_limit Signed-off-by: Adi Vardi <adi.vardi@enway.ai> * fix linting issue Signed-off-by: Adi Vardi <adi.vardi@enway.ai> * Revert: reset speed limits after param change Signed-off-by: Adi Vardi <adi.vardi@enway.ai> --------- Signed-off-by: Adi Vardi <adi.vardi@enway.ai> * Bugfix inactive publishers (#5748) * Do not publish costmap unless active Signed-off-by: Christopher Thompson <cthompson@metalsharkboats.com> * Fix style Signed-off-by: Christopher Thompson <cthompson@metalsharkboats.com> * Fix typo Signed-off-by: Christopher Thompson <cthompson@metalsharkboats.com> * Use layers isEnabled() to prevent publishing Signed-off-by: Christopher Thompson <cthompson@metalsharkboats.com> * Fix style Signed-off-by: Christopher Thompson <cthompson@metalsharkboats.com> * Add missing include for CostmapLayer type Signed-off-by: Christopher Thompson <cthompson@metalsharkboats.com> * Remove extra scoping Signed-off-by: Christopher Thompson <cthompson@metalsharkboats.com> --------- Signed-off-by: Christopher Thompson <cthompson@metalsharkboats.com> Co-authored-by: Christopher Thompson <cthompson@metalsharkboats.com> * Fix MAX_NON_OBSTACLE_COST in theta star planner (#5865) Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Bump jazzy to 1.3.11 for release Signed-off-by: SteveMacenski <stevenmacenski@gmail.com> --------- Signed-off-by: Tim Clephas <tim.clephas@nobleo.nl> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: SteveMacenski <stevenmacenski@gmail.com> Signed-off-by: agennart <antoine.gennart@quimesis.be> Signed-off-by: suifengersan123 <yangabc810@gmail.com> Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> Signed-off-by: Adi Vardi <adi.vardi@enway.ai> Signed-off-by: Christopher Thompson <cthompson@metalsharkboats.com> Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> Co-authored-by: Tim Clephas <tim.clephas@nobleo.nl> Co-authored-by: Saitama <gennartan@users.noreply.github.com> Co-authored-by: agennart <antoine.gennart@quimesis.be> Co-authored-by: suifengersan123 <136066397+suifengersan123@users.noreply.github.com> Co-authored-by: claude[bot] <209825114+claude[bot]@users.noreply.github.com> Co-authored-by: Simon Dathan <simon.dathan@kudan.eu> Co-authored-by: Adi Vardi <57910756+adivardi@users.noreply.github.com> Co-authored-by: Christopher Thompson <pele1410@gmail.com> Co-authored-by: Christopher Thompson <cthompson@metalsharkboats.com> Co-authored-by: Maurice Alexander Purnawan <mauricepurnawan@gmail.com>

…ion#5775) * Update obstacle layer usage of max ranges (ros-navigation#5697) * Use cell distance for obstacle marking max range Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Don't raytrace clear the cell containing the current observation Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Add tests for max marking and clearing ranges Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Fix cpplint failures Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Fix distance calculation Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * fix casting, formatting Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * check origin is in map, update CMakeLists Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * use hypot instead of squared dist Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Move origin calc out of loop Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Revert don't raytrace observation cell Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Revert don't raytrace origin if it is observation cell Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Remove new line Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> --------- Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Fix tests for jazzy Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> --------- Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

) * Backport jazzy: Update obstacle layer usage of max range (#5775) * Update obstacle layer usage of max ranges (#5697) * Use cell distance for obstacle marking max range Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Don't raytrace clear the cell containing the current observation Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Add tests for max marking and clearing ranges Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Fix cpplint failures Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Fix distance calculation Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * fix casting, formatting Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * check origin is in map, update CMakeLists Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * use hypot instead of squared dist Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Move origin calc out of loop Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Revert don't raytrace observation cell Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Revert don't raytrace origin if it is observation cell Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Remove new line Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> --------- Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Fix tests for jazzy Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> --------- Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Remove unrelated changes Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Lint Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> --------- Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> Co-authored-by: Simon Dathan <simon.dathan@kudan.eu>

…ion#5775) * Update obstacle layer usage of max ranges (ros-navigation#5697) * Use cell distance for obstacle marking max range Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Don't raytrace clear the cell containing the current observation Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Add tests for max marking and clearing ranges Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Fix cpplint failures Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Fix distance calculation Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * fix casting, formatting Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * check origin is in map, update CMakeLists Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * use hypot instead of squared dist Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Move origin calc out of loop Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Revert don't raytrace observation cell Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Revert don't raytrace origin if it is observation cell Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Remove new line Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> --------- Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Fix tests for jazzy Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> --------- Signed-off-by: Simon Dathan <simon.dathan@kudan.eu>

* No need to spam the logs (ros-navigation#5674) Signed-off-by: Tim Clephas <tim.clephas@nobleo.nl> * Conditionally call onLoop based on node status (ros-navigation#5700) Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Allow for no progress checkers to be configured (ros-navigation#5701) Signed-off-by: SteveMacenski <stevenmacenski@gmail.com> * Fix: wait for drive_on_heading_client instead of backup_client (ros-navigation#5724) The basic navigator was waiting for the backup_client in the driveOnHeading function. Signed-off-by: agennart <antoine.gennart@quimesis.be> Co-authored-by: agennart <antoine.gennart@quimesis.be> * Fix: reset controller_server loop_rate when missed desired rate ros-navigation#5712 (ros-navigation#5723) When a single iteration takes longer than the expected period, this impacts the next iterations behavior since they will have less time to perform the control logic, thus increasing the actual controller_frequency. By resetting the loop_rate on sleep() failure, this ensures that each iteration will have a full period to exectue its logic. Signed-off-by: agennart <antoine.gennart@quimesis.be> Co-authored-by: agennart <antoine.gennart@quimesis.be> * temporary fix bug null pointer (ros-navigation#5749) * temporary fix bug null pointer Signed-off-by: suifengersan123 <yangabc810@gmail.com> * add return Signed-off-by: suifengersan123 <yangabc810@gmail.com> * remove return Signed-off-by: suifengersan123 <yangabc810@gmail.com> --------- Signed-off-by: suifengersan123 <yangabc810@gmail.com> * fix: change warning to exception when goal poses cannot be transformed (ros-navigation#5759) - Replace RCLCPP_WARN with std::runtime_error exception in onPreempt method - Remove bt_action_server_->terminatePendingGoal() call after accepting goal - Add required stdexcept include for exception handling Fixes issue where navigators accept pending goal but are not properly terminated if rejected during goal pose transformation. Co-authored-by: claude[bot] <209825114+claude[bot]@users.noreply.github.com> * Update obstacle layer usage of max ranges (ros-navigation#5697) * Use cell distance for obstacle marking max range Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Don't raytrace clear the cell containing the current observation Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Add tests for max marking and clearing ranges Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Fix cpplint failures Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Fix distance calculation Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * fix casting, formatting Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * check origin is in map, update CMakeLists Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * use hypot instead of squared dist Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Move origin calc out of loop Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Revert don't raytrace observation cell Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Revert don't raytrace origin if it is observation cell Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * Remove new line Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> --------- Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> * [mppi] Don't reset zone-based speed limits (ros-navigation#5768) * [mppi] Don't reset zone-based speed limits Signed-off-by: Adi Vardi <adi.vardi@enway.ai> * [mppi] update motion model params from speed_limit Signed-off-by: Adi Vardi <adi.vardi@enway.ai> * fix linting issue Signed-off-by: Adi Vardi <adi.vardi@enway.ai> * Revert: reset speed limits after param change Signed-off-by: Adi Vardi <adi.vardi@enway.ai> --------- Signed-off-by: Adi Vardi <adi.vardi@enway.ai> * Bugfix inactive publishers (ros-navigation#5748) * Do not publish costmap unless active Signed-off-by: Christopher Thompson <cthompson@metalsharkboats.com> * Fix style Signed-off-by: Christopher Thompson <cthompson@metalsharkboats.com> * Fix typo Signed-off-by: Christopher Thompson <cthompson@metalsharkboats.com> * Use layers isEnabled() to prevent publishing Signed-off-by: Christopher Thompson <cthompson@metalsharkboats.com> * Fix style Signed-off-by: Christopher Thompson <cthompson@metalsharkboats.com> * Add missing include for CostmapLayer type Signed-off-by: Christopher Thompson <cthompson@metalsharkboats.com> * Remove extra scoping Signed-off-by: Christopher Thompson <cthompson@metalsharkboats.com> --------- Signed-off-by: Christopher Thompson <cthompson@metalsharkboats.com> Co-authored-by: Christopher Thompson <cthompson@metalsharkboats.com> * Fix MAX_NON_OBSTACLE_COST in theta star planner (ros-navigation#5865) Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Bump jazzy to 1.3.11 for release Signed-off-by: SteveMacenski <stevenmacenski@gmail.com> --------- Signed-off-by: Tim Clephas <tim.clephas@nobleo.nl> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: SteveMacenski <stevenmacenski@gmail.com> Signed-off-by: agennart <antoine.gennart@quimesis.be> Signed-off-by: suifengersan123 <yangabc810@gmail.com> Signed-off-by: Simon Dathan <simon.dathan@kudan.eu> Signed-off-by: Adi Vardi <adi.vardi@enway.ai> Signed-off-by: Christopher Thompson <cthompson@metalsharkboats.com> Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> Co-authored-by: Tim Clephas <tim.clephas@nobleo.nl> Co-authored-by: Saitama <gennartan@users.noreply.github.com> Co-authored-by: agennart <antoine.gennart@quimesis.be> Co-authored-by: suifengersan123 <136066397+suifengersan123@users.noreply.github.com> Co-authored-by: claude[bot] <209825114+claude[bot]@users.noreply.github.com> Co-authored-by: Simon Dathan <simon.dathan@kudan.eu> Co-authored-by: Adi Vardi <57910756+adivardi@users.noreply.github.com> Co-authored-by: Christopher Thompson <pele1410@gmail.com> Co-authored-by: Christopher Thompson <cthompson@metalsharkboats.com> Co-authored-by: Maurice Alexander Purnawan <mauricepurnawan@gmail.com>

sd-kudan force-pushed the sdathan/update-obstacle-layer-usage-of-max-range branch from 845dced to be10620 Compare November 20, 2025 10:24

zach-kudan mentioned this pull request Nov 20, 2025

Costmap2D Obstacle Layer - Obstacles cleared when reversing away from them #5476

Closed

sd-kudan added 4 commits November 25, 2025 09:14

Use cell distance for obstacle marking max range

b3e960e

Signed-off-by: Simon Dathan <simon.dathan@kudan.eu>

Don't raytrace clear the cell containing the current observation

de9eb60

Signed-off-by: Simon Dathan <simon.dathan@kudan.eu>

Add tests for max marking and clearing ranges

bd0b69f

Signed-off-by: Simon Dathan <simon.dathan@kudan.eu>

Fix cpplint failures

cea3222

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 November 25, 2025 09:25

SteveMacenski requested changes Nov 25, 2025

View reviewed changes

nav2_costmap_2d/plugins/obstacle_layer.cpp Outdated Show resolved Hide resolved

nav2_costmap_2d/plugins/obstacle_layer.cpp Outdated Show resolved Hide resolved

SteveMacenski linked an issue Nov 25, 2025 that may be closed by this pull request

Costmap2D Obstacle Layer - Obstacles cleared when reversing away from them #5476

Closed

Fix distance calculation

126214c

Signed-off-by: Simon Dathan <simon.dathan@kudan.eu>

SteveMacenski requested changes Dec 1, 2025

View reviewed changes

sd-kudan added 3 commits December 4, 2025 10:26

fix casting, formatting

f3c3d60

Signed-off-by: Simon Dathan <simon.dathan@kudan.eu>

check origin is in map, update CMakeLists

841b1dc

Signed-off-by: Simon Dathan <simon.dathan@kudan.eu>

Merge remote-tracking branch 'upstream/main' into sdathan/update-obst…

d07a629

…acle-layer-usage-of-max-range

SteveMacenski reviewed Dec 4, 2025

View reviewed changes

use hypot instead of squared dist

c0b3e9a

Signed-off-by: Simon Dathan <simon.dathan@kudan.eu>

Merge remote-tracking branch 'upstream/main' into sdathan/update-obst…

72e7fa6

…acle-layer-usage-of-max-range

SteveMacenski requested changes Dec 5, 2025

View reviewed changes

nav2_costmap_2d/plugins/obstacle_layer.cpp Outdated Show resolved Hide resolved

nav2_costmap_2d/plugins/obstacle_layer.cpp Outdated Show resolved Hide resolved

nav2_costmap_2d/plugins/obstacle_layer.cpp Outdated Show resolved Hide resolved

Move origin calc out of loop

4a6db66

Signed-off-by: Simon Dathan <simon.dathan@kudan.eu>

SteveMacenski approved these changes Dec 8, 2025

View reviewed changes

Revert don't raytrace observation cell

835befd

Signed-off-by: Simon Dathan <simon.dathan@kudan.eu>

sd-kudan added 3 commits December 9, 2025 09:40

Revert don't raytrace origin if it is observation cell

080bad2

Signed-off-by: Simon Dathan <simon.dathan@kudan.eu>

Merge remote-tracking branch 'upstream/main' into sdathan/update-obst…

404daec

…acle-layer-usage-of-max-range

Remove new line

9083ec6

Signed-off-by: Simon Dathan <simon.dathan@kudan.eu>

SteveMacenski approved these changes Dec 9, 2025

View reviewed changes

SteveMacenski merged commit 80b72d2 into ros-navigation:main Dec 9, 2025
19 of 20 checks passed

sd-kudan mentioned this pull request Dec 11, 2025

Backport jazzy: Update obstacle layer usage of max range #5775

Merged

8 tasks

Conversation

sd-kudan commented Nov 20, 2025

Basic Info

Description of contribution in a few bullet points

Description of documentation updates required from your changes

Description of how this change was tested

Demo

Future work that may be required in bullet points

For Maintainers:

Uh oh!

mergify bot commented Nov 20, 2025

Uh oh!

SteveMacenski commented Nov 24, 2025

Uh oh!

Uh oh!

Uh oh!

codecov bot commented Nov 28, 2025

Codecov Report

Uh oh!

SteveMacenski left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

SteveMacenski Dec 4, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

sd-kudan Dec 4, 2025

Choose a reason for hiding this comment

Uh oh!

SteveMacenski Dec 5, 2025

Choose a reason for hiding this comment

Uh oh!

sd-kudan Dec 8, 2025

Choose a reason for hiding this comment

Uh oh!

SteveMacenski Dec 8, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

sd-kudan Dec 9, 2025

Choose a reason for hiding this comment

Uh oh!

Uh oh!

SteveMacenski commented Dec 4, 2025

Uh oh!

sd-kudan commented Dec 4, 2025

Uh oh!

mini-1235 commented Dec 5, 2025

Uh oh!

SteveMacenski commented Dec 5, 2025

Uh oh!

mini-1235 commented Dec 5, 2025

Uh oh!

SteveMacenski left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

SteveMacenski commented Dec 8, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

Uh oh!

SteveMacenski commented Dec 9, 2025

Uh oh!

sd-kudan commented Dec 10, 2025

Uh oh!

sd-kudan commented Dec 11, 2025

Uh oh!

mini-1235 commented Dec 11, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

SteveMacenski Dec 4, 2025 •

edited

Loading

SteveMacenski Dec 8, 2025 •

edited

Loading

SteveMacenski commented Dec 8, 2025 •

edited

Loading

mini-1235 commented Dec 11, 2025 •

edited

Loading