Full-area footprint check

[tonynajjar]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	foundation for #5329
Primary OS tested on	Ubuntu
Robotic platform tested on	private simulation
Does this PR contain AI generated software?	Yes but I reviewed and iterated on it. should I still mark it?
Was this PR description generated by AI software?	Nope

---

Description of contribution in a few bullet points

• I added the option of a full-area check in footprintcost. The option is off by default, it will be used in future PRs using a full-area footprint check for more accurate collision-checking

Description of documentation updates required from your changes

Description of how this change was tested

• I wrote additional unit tests
• I've tested it in simulation

---

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]

@tonynajjar, 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).

[mergify]

@tonynajjar, 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).

[tonynajjar]

note to self, should be consistent with the one above (LETHAL_OBSTACLE)

[mergify]

@tonynajjar, 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).

[tonynajjar]

@SteveMacenski I replaced the opencv with a custom implementation - I compared them using a performance test (included) and they were similar.

[mergify]

@tonynajjar, 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]

I will take a look at this tomorrow probably - but can you highlight the metrics you see different? How much 'worse' is this?

[tonynajjar]

I will take a look at this tomorrow probably - but can you highlight the metrics you see different? How much 'worse' is this?

This is the output of the performance test for this PR, it should be self-explanatory:

[==========] Running 1 test from 1 test suite.
[----------] Global test environment set-up.
[----------] 1 test from collision_footprint
[ RUN      ] collision_footprint.test_performance_benchmark

=== FOOTPRINT COLLISION CHECKER PERFORMANCE BENCHMARK ===
Test configuration:
  - Costmap size: 500x500 cells (25m x 25m @ 0.05m resolution)
  - Footprint: Rectangle 2m x 1m
  - Test position: (12.5, 12.5, 0)
  - Iterations per scenario: 1000
  - Obstacle cost: 254

Scenario: Obstacle at vertex
  Perimeter checking:
    - Result: 254
    - Average time: 0.08 microseconds
  Full-area checking:
    - Result: 254.00
    - Average time: 0.05 microseconds
  Performance ratio (full-area / perimeter): 0.68x

Scenario: Obstacle on edge
  Perimeter checking:
    - Result: 254.00
    - Average time: 0.58 microseconds
  Full-area checking:
    - Result: 254.00
    - Average time: 0.58 microseconds
  Performance ratio (full-area / perimeter): 1.00x

Scenario: Obstacle inside
  Perimeter checking:
    - Result: 0.00
    - Average time: 0.65 microseconds
  Full-area checking:
    - Result: 254.00
    - Average time: 5.19 microseconds
  Performance ratio (full-area / perimeter): 7.99x

Scenario: No obstacles
  Perimeter checking:
    - Result: 0.00
    - Average time: 0.65 microseconds
  Full-area checking:
    - Result: 0.00
    - Average time: 9.64 microseconds
  Performance ratio (full-area / perimeter): 14.83x

Early exit performance verification (full-area checking):
  Obstacle at vertex: 0.05 μs (fastest)
  Obstacle on edge: 0.58 μs
  Obstacle inside: 5.19 μs
  No obstacles: 9.64 μs (slowest)
  ✓ Early exit performance ordering verified
============================================================

[       OK ] collision_footprint.test_performance_benchmark (21 ms)
[----------] 1 test from collision_footprint (21 ms total)

[----------] Global test environment tear-down
[==========] 1 test from 1 test suite ran. (21 ms total)
[  PASSED  ] 1 test.

I also compared the perimeter-only check before and after the changes and no degradation of performance (modified the test here for perimeter-only)

Before

[==========] Running 1 test from 1 test suite.
[----------] Global test environment set-up.
[----------] 1 test from collision_footprint
[ RUN      ] collision_footprint.test_performance_benchmark

=== FOOTPRINT COLLISION CHECKER PERFORMANCE BENCHMARK ===
Test configuration:
  - Costmap size: 500x500 cells (25m x 25m @ 0.05m resolution)
  - Footprint: Rectangle 2m x 1m
  - Test position: (12.5, 12.5, 0)
  - Iterations per scenario: 1000
  - Obstacle cost: 254

Scenario: Obstacle at vertex
  Perimeter checking:
    - Result: 254
    - Average time: 0.05 microseconds

Scenario: Obstacle on edge
  Perimeter checking:
    - Result: 254.00
    - Average time: 0.28 microseconds

Scenario: Obstacle inside
  Perimeter checking:
    - Result: 0.00
    - Average time: 0.37 microseconds

Scenario: No obstacles
  Perimeter checking:
    - Result: 0.00
    - Average time: 0.36 microseconds
============================================================

[       OK ] collision_footprint.test_performance_benchmark (5 ms)
[----------] 1 test from collision_footprint (5 ms total)

[----------] Global test environment tear-down
[==========] 1 test from 1 test suite ran. (5 ms total)
[  PASSED  ] 1 test.

After

[==========] Running 1 test from 1 test suite.
[----------] Global test environment set-up.
[----------] 1 test from collision_footprint
[ RUN      ] collision_footprint.test_performance_benchmark

=== FOOTPRINT COLLISION CHECKER PERFORMANCE BENCHMARK ===
Test configuration:
  - Costmap size: 500x500 cells (25m x 25m @ 0.05m resolution)
  - Footprint: Rectangle 2m x 1m
  - Test position: (12.5, 12.5, 0)
  - Iterations per scenario: 1000
  - Obstacle cost: 254

Scenario: Obstacle at vertex
  Perimeter checking:
    - Result: 254
    - Average time: 0.07 microseconds

Scenario: Obstacle on edge
  Perimeter checking:
    - Result: 254.00
    - Average time: 0.65 microseconds

Scenario: Obstacle inside
  Perimeter checking:
    - Result: 0.00
    - Average time: 0.63 microseconds

Scenario: No obstacles
  Perimeter checking:
    - Result: 0.00
    - Average time: 0.63 microseconds
============================================================

[       OK ] collision_footprint.test_performance_benchmark (5 ms)
[----------] 1 test from collision_footprint (5 ms total)

[----------] Global test environment tear-down
[==========] 1 test from 1 test suite ran. (5 ms total)
[  PASSED  ] 1 test

[SteveMacenski]

So this look reasonable from the initial review perspective. Is there a good way to parameterize this for those that want it + only do the checks when its important to do so (i.e. max cost is INFLATED, else we're not near an obstacle anyway to require it). That should give you speed ups back to normal collision checking when possible.

I think this is good to keep in our pocket as a working example and everything else is optimizations to see if we can do better. For example, doing the swept area from interpolated perimeter footprints instead -- which in my head should be better in terms of compute because we should be checking less cells and accounts for the outer-most edge's sweeping between search jumps in case it 'knicks'. That is what https://github.com/ros-navigation/navigation2/tree/h4pvbk-codex/add-optimized-collision-checking-method starts (I know you know; for others reading / posterity).

I don't love how heavy this is and doesn't solve an element of your problem with jumps while rotating / turning where a long robot can have is corners overlap that isn't caught if its not in a parent or child polygon. But, I'm also not someone that blocks 'better/good' in the name of 'perfect'

[tonynajjar]

Is there a good way to parameterize this for those that want it

Probably it should be a parameter at the level of the planners and controllers, like consider_footprint, but I think in this PR there's nothing to be parameterized, the check_full_area argument will be set according to the parameter upstream in SMAC, MPPI, etc...

• only do the checks when its important to do so (i.e. max cost is INFLATED, else we're not near an obstacle anyway to require it)

You mean if the perimeter cost is less than INFLATED then no need to do full area check? Counter-example:

doesn't solve an element of your problem with jumps while rotating / turning where a long robot can have is corners overlap that isn't caught if its not in a parent or child polygon

True but it does solve another problem -> RemoveInCollisionGoals currently also misses small lethal cells inside the footprint (see same image above)

[SteveMacenski]

Counter-example:

Got it 👍 Counter then is to do what we do in MPPI/Smac and find the cost in which anything is possibly circumscribed and use that rather than INFLATED.

RemoveInCollisionGoals

I think this uses the IsPathValid service, if so, I think the full collision checking should be a field of that as well possibly.

[tonynajjar]

I think this uses the IsPathValid service, if so, I think the full collision checking should be a field of that as well possibly.

No it uses get_cost_global_costmap service from costmap_2d_ros.cpp

[SteveMacenski]

Ah, then GetCosts instead: https://github.com/ros-navigation/navigation2/blob/main/nav2_msgs/srv/GetCosts.srv#L3. Though, that's very similar to IsPathValid so probably both I suppose if use footprint is set

navigation2/nav2_planner/src/planner_server.cpp

Line 697 in 7603938

cost = static_cast<unsigned int>(collision_checker_->footprintCostAtPose(