SMAC swept area collision checking

[tonynajjar]

Following up on #4271, it was discovered that in certain conditions, SMAC generates infeasible paths aka paths that cause the footprint to overlap with lethal cells.

The SMAC collision checker only checks for collision across the vertices of the footprint and not the interior.
This has the problem that if the lethal space is small enough e.g. one cell, it can fall inside the planned footprints and would would consider the corresponding node to be valid. Here is an example

This can be solved with checking the cells inside the footprint as well. Here is a draft of that https://github.com/ros-navigation/navigation2/pull/5250/files

However, checking all the cells inside the footprint might not be enough to solve all cases.

Consider this other scenario:

Although the first and the second planned footprints in themselves are feasible, the most direct way to get from first to second (pure rotation, or almost) would be in collision. I think some controllers are smart enough to come up with local plan that will not collide but I believe SMAC's plan should be feasible for a "dumber" controller that would just take the fastest way.

The way to solve this is described in this comment by @SteveMacenski. It involves interpolating with a higher resolution between two footprints if they are close enough to an obstacle.

[stevedanomodolor]

Will work on this issue.

[tonynajjar]

I'm also passively working on this internally since a couple of days but I'm not sure yet if the solution will be cleanly-architectured enough to be contributed back. I'll know more next week and can maybe share what I have as a base