Fix SMAC smoother footprint check

[MobiusRoamer]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#5330
Primary OS tested on	Ubuntu24.04
Robotic platform tested on	Unit test
Does this PR contain AI generated software?	Only for code to reproduce the regression test, also for debugging missing #include
Was this PR description generated by AI software?	No

---

Description of contribution in a few bullet points

• Fixes SMAC smoother can generate infeasible paths when using non-circular robots in very confined spaces #5330 : SMAC smoothing needs to account for non circular footprints
  (Note on scope proposed in the original issue: this pr does not try to redesign the smoother to retain "true" orientation information or enforce min-turning-radius feasibility during smoothing. The goal here is fixing the immediate false-negative collision case for non-circular footprints, preserving existing smoother structure.)
• Adds regression test: given safe footprint, SMAC Smoothing will not cause false collision and report unsafe

Code change:

• Extended (in nav2_smac_planner/include/nav2_smac_planner/smoother.hpp) Smoother::smooth() and Smoother::smoothImpl() to optionally accept footprint and use_radius (whether bot is radius based or not)
• Extended logic (in nav2_smac_planner/src/smoother.cpp): kept original centre-cell cost check for circular footprint; added a non circular footprint validation path if costmap is a valid pointer && !use_radius

Description of documentation updates required from your changes

No updates required. This is only bug fix.

Description of how this change was tested

The test isolates the smoother's own collision-validation behavior:

• Start with a footprint-valid path validated by footprint_checker.footprintCostAtPose() to be safe
• Checks whether smoothing itself can introduce a footprint collision and whether that update is rejected
• Scenario: a rectangular non-circular footprint, an obstacle placed so that the original path is valid (see Test Details at the end). After smoothing, the corner is pulled inward, the bot centre remains in a free cell, but the rectangular footprint overlaps the obstacle
• Expected: SMAC smoother detects the footprint collision, roll back to the previous valid path, returns false.
• Previously: smoother only checked the centre cell cost, so it returned success and accepted the colliding smoothed path

---

Future work that may be required in bullet points

no know follow up work

Test Details & Results

To visualize the footprint geometry a bit, r denotes the robot centre, `

1.2m * 0.4m
        [===========]
        [     r     ]
        [===========]
              ^
            center
X is our obstacle in the following landscape

y
^
| 12m    r----r----r----r----r----r
|        |
|        |     X  obstacle around (3.1, 11.7), not to scale
|        |
| 10m    r
|        |
|  8m    r
|        |
|  6m    r
|        |
|  4m    r
|        |
|  2m    r
|
+--------+----+----+----+----+----+----> x
        2m   4m   6m   8m  10m  12m  14m

To reproduce the regression test:

cd <nav2_ws>

git -C src/navigation2 fetch https://github.com/MobiusRoamer/navigation2.git fix/smac-smoother-footprint-5330
git -C src/navigation2 switch -C test-smac-smoother-footprint FETCH_HEAD

then (replace --packages-select with --packages-up-to nav2_smac_planner if dependencies not installed yet)

colcon build \
  --symlink-install \
  --packages-select nav2_smac_planner \
  --cmake-args -DBUILD_TESTING=ON

source install/setup.bash

./build/nav2_smac_planner/test/test_smoother \
  --gtest_filter='SmootherTest.rejects_non_circular_footprint_collision'

Does this fit with the original scope? Let me know if more changes are needed or if you would like to see additional testing / screenshot from the local run

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-*.

[codecov]

Codecov Report

❌ Patch coverage is 80.00000% with 5 lines in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
...de/nav2_smac_planner/smac_planner_lattice_impl.hpp	0.00%	3 Missing ⚠️
...ude/nav2_smac_planner/smac_planner_hybrid_impl.hpp	0.00%	2 Missing ⚠️

Files with missing lines	Coverage Δ
...include/nav2_smac_planner/smac_planner_2d_impl.hpp	91.78% <100.00%> (-1.34%)	⬇️
...mac_planner/include/nav2_smac_planner/smoother.hpp	100.00% <ø> (ø)
nav2_smac_planner/src/smoother.cpp	97.02% <100.00%> (+0.14%)	⬆️
...ude/nav2_smac_planner/smac_planner_hybrid_impl.hpp	89.54% <0.00%> (-0.21%)	⬇️
...de/nav2_smac_planner/smac_planner_lattice_impl.hpp	89.58% <0.00%> (-0.47%)	⬇️

... and 33 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]

But good first go at it!

[SteveMacenski]

There's no caller of this passing in the footprint using this new API.

ALso if footprint == std::vector<geometry_msgs::msg::Point>() then you know you're using radius, so the second argument is redundant. Just pass in footprint to smooth() when this API is used only when needed

[SteveMacenski]

Here and below: I think we both can agree that you may be able to do better to integrate this more cleanly.

If under if (costmap) you can then check if footprint == vector<Point>() to know whether to use single point or footprint cost checker more concisely. You can also store the FootprintCollisionChecker as a class member so you don't reconstruct I think.

You can also reorder calling updateApproximatePathOrientations before we try to collision check in general so its always just done for us instead of waiting til the end.

I think there's a few things you can do to make this higher quality and less redundant