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 always 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 24 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 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.

This allows for consistent behavior with circular and non-circular robots, or when changing whether to use the SE2 collision check or not doesn't require retuning of the weights.

Also because then some part of the trajectory are going to be scored based on the center point costs and others are going to be edge costs, which are not representing the same thing / consistent. That may result in issues in the optimization problem where trajectories that are near obstacles are being scored (primarily) based on edge costs, while trajectories further away will be scored (primarily) based on center point-costs. Those are two different cost spaces. I can ACK that this would probably lead to the robot keeping further from obstacles, but not just based on the costmap/weighting parameterizations, that introduces essentially a non-continuous piecemeal function as this critic's backbone rather than an exponential decay. That makes this much harder to tune and kind of unexpected behavior.

I think this also may cause some issues when trying to go through narrow spaces where all of the robot's footprint is going to be in 253 inflated space. Imagine an aisle just 30cm wider than a pretty wide robot. There'd be essentially no difference between staying in the middle of the aisle versus 30cm off to the side as much as possible that is non-collision. They'd all be scored as 253 uniformly rather than the center of the aisle having a preference with a cost valley. How should we address that?

With all this said, I don't think this is inherently a bad contribution for rejection. I'd want to hear your feedback to these concerns, but this also could be parameterized as an option to use. I'd suggest adding some warnings about the issues I bringup above when in use - especially if you can verify that you see these behaviors in action now that you're aware of them (and still want to use this formation).

---

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_

I don't agree with this, since it would bypass the critical collision checking near the goal. This is the only critic stopping the robot from running into things, so I think this is safety-critic that we consider costs. Else, we only penalize in collision situations rather than situations very close to collisions. The near_collision_cost parameter is there for a reason so that this can be tuned if you want to set this high or low.

You can get the same behavior simply by setting near_collision_cost to 254 - so that you only consider collisions rather than near-obstacles. Agreed? I think this part should be reverted.

[sebatztian]

@SteveMacenski This was also the core issue of the discussion in #5590

Would it not be an option to remove the cost plateau (or disable it via parameter) and compute the inscribed radius cost similar to how the circumscribed radius cost is already computed, and update it only when the footprint changes.

That way there is no inherent distance information loss close to the robot.

Possible downsides I see:

• the implementation change affects a lot of packages
• the range (0,253) covers the inscribed radius as well, leading to bigger discretization steps.

[SteveMacenski]

I'm trying to parse that suggestion. So you're suggesting removing the inflation layer and creating an eq. that doesn't have the inscribed radius 253 cost? That would be essentially a TSDF (if having linear distance-based information) which is something that's been on my mind for some time. I actually abandoned that because of the general dislike of the Obstacle Critic which scaled cost based on linear distance rather than exponential cost decay. People seem to really want exponential cost increases near obstacles.

We could though have an exponential decay function without the 253 inscribed plateau, sure. That could be something that could even be parameterized in the inflation layer about whether to include it or not (or a simple layer deriving from the Inflation Layer that does this). It would just require that the Cost Critic has this enabled on the inflation layer, which while we do require that they are tuned together to get the behavior you like, adds a bit more of a formal tie in that this must be done. But maybe that's OK.

We'd have to check everywhere that collision checking is done in the stack to make sure it can support that feature. Maybe it could be an easy fix so perhaps not too hard actually.

I think that's a legit option to consider for exploration. I don't see an immediate answer for why that would be bad other than a computational hit on the inflation layer computation, but even that is probably negligible.

@tonynajjar ?

[tonynajjar]

First of all, I actually never thought about the drawback of the 253 plateau of the inflation layer and how it causes distance-information loss. I hope I'm not missing something crucial given how old the plateau implementation is, but I tend to agree with the idea of removing that plateau to have no distance loss and instead use a comparison with the inscribed radius cost to determine when a point is in collision.

That said, I think we are not saying that this is a direct solution to the inconsistent distance between non-circular footprint and obstacle right? For that we would still need to use the footprint cost I believe

Also because then some part of the trajectory are going to be scored based on the center point costs and others are going to be edge costs, which are not representing the same thing / consistent.

You're actually theoretically right there. In my testing I didn't see any weird behavior but that's probably attenuated by the fact that I have a relatively small footprint. This would be not an issue if we used footprint costs everywhere no? I know that's computationally heavy but is there any other drawback I should know of?

I think this also may cause some issues when trying to go through narrow spaces where all of the robot's footprint is going to be in 253 inflated space.

Yes, this would be a drawback of using footprint costs but that's not inherently because of using footprint cost, that's because of the plateau at 253 which would be solved with @sebatztian's proposal

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_

I'll just delay that discussion to not overcrowd the other topics for now