Fix for robot footprint collision in obstacles critic

[LeifHookedWireless]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	No known tickets
Primary OS tested on	Ubuntu
Robotic platform tested on	Peanut's in-house robot (sim)

---

Description of contribution in a few bullet points

• Inscribed/Circumscribed costs must be updated to take into account the current shape of the robot.
• ObstaclesCritic::findCircumscribedCost() was only being called once, in initialize()
• in initialize(), findCircumscribedCost() uses placeholder inner/outer radius values which come from robot_radius.
• findCircumscribedCost() was never called again, and the circumscribed cost was never updated based on the footprint.
• depending on the value of robot_radius, this could cause too much or too little footprint checking in costAtPose()

Description of documentation updates required from your changes

• No documentation changes.

---

Future work that may be required in bullet points

• The SMAC planner may have a similar problem. I will investigate when I have time.

For Maintainers:

• Check that any new parameters added are updated in navigation.ros.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

[LeifHookedWireless]

This was the main reason our robot was driving through walls. robot_radius was smaller than the actual footprint, so the footprint was effectively ignored in the controller, even though consider_footprint_ was true.

[SteveMacenski]

I approve, I’m waiting until I’m in front of a computer again this evening to test the runtime considerations.

[codecov]

Codecov Report

All modified and coverable lines are covered by tests ✅

Comparison is base (0756ee1) 90.36% compared to head (fa09c78) 90.28%.
Report is 2 commits behind head on main.

❗ Current head fa09c78 differs from pull request most recent head c201121. Consider uploading reports for the commit c201121 to get more accurate results

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #3878      +/-   ##
==========================================
- Coverage   90.36%   90.28%   -0.09%     
==========================================
  Files         415      413       -2     
  Lines       18446    18311     -135     
==========================================
- Hits        16669    16532     -137     
- Misses       1777     1779       +2     

Files	Coverage Δ
...2_mppi_controller/src/critics/obstacles_critic.cpp	81.33% <100.00%> (+0.25%)	⬆️

... and 31 files with indirect coverage changes

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

[LeifHookedWireless]

I think my description is slightly wrong: it will almost always cause less footprint checking than should be.
The placeholder inner/outer radii are based on a poly of robot_radius. The inscribed and circumscribed radii of a regular polygon are always going to be pretty close to each other, compared to a non-round robot.

[SteveMacenski]

Actually- can you test if this issue is resolved if we change these to references? Get once, have forever. The function that obtains the values may need to be updated to return references not copies. But that would completely mitigate additional runtime costs to get the most current state

[BriceRenaudeau]

I had similar issues with the obstacles_critic. and that's why we developed a similar critic.

The radii are not updated correctly as @LeifHookedWireless mentioned. This modification will help with dynamic footprint.

But there is still several issues with the obstacles_critic:

• The obstacles_critic is using the cost from inflate layer to compute distances by using the invert function of the cost. It makes inflation_layer cost_scaling_factor tunning useless, its good because it reduces tunning and it may be dangerous as it has a quantization effect on the distance).
• The score is computed differently when the robot is far from obstacles and when it is close.
  https://github.com/ros-planning/navigation2/blob/7009ffba5f85c50ac97fd0057924b0f1447c5e85/nav2_mppi_controller/src/critics/obstacles_critic.cpp#L147-L151
  Why is the inflation radius used in the obstacle score? There is an X20 scale difference as collision_margin_distance_ ~ 0.1 and inflation_radius_ ~ 2.
• If the footprint has some points in the inscribed area, the score is the same regardless of the distance to the obstacle. Because footprintCostAtPose returns the highest cost which will be "INSCRIBED" whatever the distance to the obstacle, distanceToObstacle(cost) will return the same distance so the score will be the same in a situation where it is important to evade obstacles.

[SteveMacenski]

I won't argue that this is perfect, but I'll say that I tested alot of formulations that didn't work nearly as well as this one. I'm not stuck to it what so ever if we have some alternatives that would be better / faster / stronger.

that's why we developed a similar critic

Plz share what you have that works better?

The obstacles_critic is using the cost from inflate layer to compute distances by using the invert function of the cost. It makes inflation_layer cost_scaling_factor tunning useless, its good because it reduces tunning and it may be dangerous as it has a quantization effect on the distance).

That's intentional. Decoupling the potential field so that the critic can decide what to do using the same units (meters) as the other critics to keep things proportionate so that the tuning weights are the only difference is a nice to have thing. Mostly though, I did this because when trying to use raw costs and normalizing them, I could never really get good performance and made the tuning way-way-way-way too sensitive to minuscule changes in the cost scaling factor and inflation radius. It was absolutely insane the degree that which you had to tune to get even remotely reasonable results. It was untenable in the formulations I tried.

The score is computed differently when the robot is far from obstacles and when it is close.

Of course, I don't see why you mention this is a 'bad' thing. We need to have very high cost for things that basically in collision, but you don't want to necessarily have high cost when you're further away from things. Decoupling "nice to have" behavior from "critical" behavior allows you to tune more finely. If we could find a mathematical model that gave us both of those at the same time, I'd be more than happy to test / use it. I think this is a really nice behavior to be able to tune how we act in "normal" situations separately from how we penalize near-obstacle behavior.

There is an X20 scale difference

Yes, but they're all in meters, like all other critics using SI units to keep the scales consistent between critics. The critical weight should just be 20x larger. That seems completely normal and reasonable. We weight things more highly when we care about them or they're smaller. See the defaults, its set as higher, just as other critic are set to higher when their SI unit formation is proportionately lower to others.

If the footprint has some points in the inscribed area, the score is the same regardless of the distance to the obstacle. Because footprintCostAtPose returns the highest cost which will be "INSCRIBED" whatever the distance to the obstacle, distanceToObstacle(cost) will return the same distance so the score will be the same in a situation where it is important to evade obstacles.

This is a fair point. There's a reason why TSDF is on my potential improvements list

---

I'm very open to suggestions. This is, as I mentioned in my ROSCon talk, one of the bigger quirks of MPPI and I would really like to improve this critic in pretty much every regard and very open to your thoughts on what is better to replace it with. Please share more about your implementation if it works better 😄

[BriceRenaudeau]

Thans for you detailed reply.
I think it is beyond to this issue now so I will keep it short and I will be happy to discuss in another thread.

The critic we are using is a simplified version of the obstacles_critcs. It uses inflation costs directly as the exponential creates the behavior we want (close = very detailed high scores and far = low scores). There is no distance computation (m). It works well enough and allow custom inflations to be taken into acount.

[SteveMacenski]

@BriceRenaudeau can you share a branch or open a ticket to chat?

@LeifHookedWireless did you try the references item so we don't recompute every iteration? I know you were at ROSCon like I, so I don't expect yet, just jogging our collective memories

[LeifHookedWireless]

@LeifHookedWireless did you try the references item so we don't recompute every iteration? I know you were at ROSCon like I, so I don't expect yet, just jogging our collective memories

@SteveMacenski Sorry, been busy. I'll look into it soon.

[LeifHookedWireless]

I'm not seeing any way to use references here.
The CircumscribedCost needs to be recalculated whenever the costmap or footprint changes, which will happen at least once between ObstaclesCritic::initialize() and ObsctacleCritics::score().
It's not stored anywhere.

It's not trivial to keep track of when those two have changed.
Since it is a fairly lightweight calculation, I suggest just computing it before score() every time.

[SteveMacenski]

Can you show that its a lightweight operation with some timers? How long is this taking?

[LeifHookedWireless]

If we assume that the costmap resolution and inflation layer parameters don't change during navigation, then we only have to recalculate the cost if getCircumscribedRadius() changes, so that could be an optimization.

[SteveMacenski]

Seems reasonable, can you just put a timer around that function and benchmark it to know that this isn't going to be some big performance hit?

[LeifHookedWireless]

I added some timers and ran some tests in my simulated hallway including passing through a narrow doorway.
With consider_footprint: false, I saw ObstaclesCritc:score() take about 4ms on average (7ms max) in the hallway and doorway.
With consider_footprint: true I was ObstaclesCritc:score() take about 4ms on average (8ms max) in the hallway, and over 100ms passing through the doorway.
findCircumscribedCost is less than 1% of the cost in any case.

[LeifHookedWireless]

I have a suggestion to address the high cost of collision detection when using the footprint.
Since the footprint code is only active near obstacles, a lot of the checks are very close to each other and I suspect that many are effectively identical to each other.
Caching results in the FootprintCollisionChecker may result in a dramatic speed-up.
I'm interested in trying this, though it may be a week or two away depending on how busy I am.

[SteveMacenski]

Sorry, maybe I should have been more explicit. Put a timer only around that findCircumscribedCost in score and let me know specifically how long that takes alone.

Caching results in the FootprintCollisionChecker may result in a dramatic speed-up.

Perhaps, but how do you plan to cache anything if we're working in continuous coordinates? There will be almost guaranteed to be no 2 identical poses in any of the trajectories being scored. They might be similar, but not the same. You'd have to bin both the orientation and the cells to approximately collision check, so you'd have to decide how it is you'd want to do that binning.

We do something related in the Smac Planner, where its version of the collision checker precomputes the footprints at the binned orientations so that we're not owning sin/cos operations in the checks and are simply translating the footprints to the current position to check at the right angle for each bin. You could do something like that for approximate checks so that you have a cache of x,y in costmap cells and yaw of precomputed angular quantizations. I don't do a cache in the Smac Planner just due to the framework its not possible to overlap in the same way it is here.

I think one could get a similar result by having the option to skip every-other trajectory point for collision checking -- which is also on my mind as a potential option to speed things up (but not ideal, obviously). That seems like the 80/20 solution but caching would be more a complete solution -- but the angular bins would need to be relatively small to make sure the approximate check isn't "substantially" worse / inaccurate. The overlap between samples would really only exist for the first ~10-30% of the trajectories. After that, the divergence due to the noise distributions would make overlaps increasingly rare, especially if you have small quantizations to make sure the accuracy remains high.

Overall, its something that I've thought about / thinking about, but there are some real open questions about its efficacy. It very well would improve performance, but I don't think its a silver bullet. I suspect for the full collision checking improvements, its going to have to be a few things like this, where caching plays a role alongside others.

Something of that matter is also on my mind if one had a GPU: where it might make sense to literally brute force the entire costmap at every cell / angle its collision state using the GPU and doing lookups here from that computed cache.

[LeifHookedWireless]

Sorry, maybe I should have been more explicit. Put a timer only around that findCircumscribedCost in score and let me know specifically how long that takes alone.

I did, but the numbers were so small I didn't think they were worth mentioning.
I measured ~6 microseconds worst case and ~1 microsecond in the typical case.
Compared to several milliseconds for score().

Caching results in the FootprintCollisionChecker may result in a dramatic speed-up.

Perhaps, but how do you plan to cache anything if we're working in continuous coordinates?

I wasn't planning on working in continuous coordinates. The footprint costmap collision uses discrete coordinates. e.g. FootprintCollisionChecker::lineCost(int x0, int x1, int y0, int y1)
You can hash those 4 ints as a key and use it to cache the results.
Even thought the (x,y,yaw) all vary slightly, I'm seeing a large number of repeated edges. Seems like we should be able to take advantage of that.
I'll let you know if I come up with anything.

[SteveMacenski]

You can hash those 4 ints as a key and use it to cache the results.

I think that's unlikely to result in major improvements, but I could be wrong! I doubt there are that many redundant checks for the exact same line segment. But hey, I'm happy to be surprised. Just remember that hashtable lookups are O(1), but that doesn't mean they're fast. I've found from experience with the Smac Planner and others in Nav2 that creating a storage semantic in std::vector is far, far faster for lookup-heavy operations.

[LeifHookedWireless]

I think that's unlikely to result in major improvements, but I could be wrong! I doubt there are that many redundant checks for the exact same line segment. But hey, I'm happy to be surprised. Just remember that hashtable lookups are O(1), but that doesn't mean they're fast. I've found from experience with the Smac Planner and others in Nav2 that creating a storage semantic in std::vector is far, far faster for lookup-heavy operations.

I implemented a cache with a std:map and I'm seeing a large number of cache hits for segments and even entire footprints. (And this is with a small 2.5cm grid size)
However the performance is not great. The costmap checks are pretty fast and, as you say, the lookups are pretty slow. So I'm getting something that's roughly a wash, performance wise.
I still think there might be something here though. I will try your std::vector suggestion next.