Use footprint cost in CostCritic for scoring when available

[tonynajjar]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#5668
Primary OS tested on	Ubuntu
Robotic platform tested on	Custom simulation
Does this PR contain AI generated software?	(No; Yes and it is marked inline in the code)
Was this PR description generated by AI software?	Out of respect for maintainers, AI for human-to-human communications are banned

---

Description of contribution in a few bullet points

Status Quo: in the CostCritic, the footprint cost is only used to for collision checking. For scoring trajectories we use the point cost only.
I'm not sure why this decision was made historically but I see no drawbacks of using the footprint cost instead when it's available. I'm saying "when it's available" because for optimization reasons, the footprint cost is not only calculated - it's only calculated when the point cost is >= possible_collision_cost. When the footprint cost is not available, this indirectly means that we are not close to an obstacle, the point cost will be used which is not as accurate but that's not so important given no collision-risk (and it's the current behavior anyway)

In my test, I found that as expected, using the footprint cost (when available) for scoring trajectories gives a more consistent distance to obstacles, since it's orientation-agnostic

Additionally, I added && !near_goal to if (cost_for_scoring >= near_collision_cost_ && !near_goal) to be able to reach goals when their footprint cost is >= near_collision_cost_

P.S: I removed the inCollision just for code cleanliness, otherwise we would need to pass out of it whether we are using the point cost or the footprint cost. I'm open to refactoring propositions

Description of documentation updates required from your changes

Description of how this change was tested

---

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

[codecov]

Codecov Report

❌ Patch coverage is 72.72727% with 3 lines in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
nav2_mppi_controller/src/critics/cost_critic.cpp	72.72%	3 Missing ⚠️

Files with missing lines	Coverage Δ
...clude/nav2_mppi_controller/critics/cost_critic.hpp	100.00% <ø> (+18.75%)	⬆️
nav2_mppi_controller/src/critics/cost_critic.cpp	82.14% <72.72%> (-2.07%)	⬇️

... and 15 files with indirect coverage changes

🚀 New features to boost your workflow:

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