Remove redundant collision checks in the Smac Planners to optimize performance (backport #4857)

[mergify]

This PR introduces a coarse-to-fine collision checking method in Smac Planners.

It checks coarsely for node collision when expanded to know if the node is clearly in collision with obstacles by checking its center point for SE2 footprints only (same behavior for circular / radius defined robots). If we're valid, then we queue the node for expansion. Once expanded, then we check the node for collision finely by checking the full SE2 footprint.

This reduces substantial numbers of full SE2 footprint checks for non-circular robots while also leaving the behavior for circular robots the same.

Edit: removed. These did not speed up performance due to cache misses by moving costmap checks into the main loop in the middle of other operations rather than batch processing them.

Additionally, I store the collision checker's state so that additional queuing of nodes will prevent unnecessary re-checking of collision states. For fine checks, it shouldn't be redone since we only visit nodes once, but it can reduce point-checks in coarse collision checking in dense expansion trees.

TODO:

• Benchmark
• Test on maps
• Backport J

  ---

  This is an automatic backport of pull request Remove redundant collision checks in the Smac Planners to optimize performance #4857 done by Mergify.

[mergify]

@mergify[bot], all pull requests must be targeted towards the main development branch.
Once merged into main, it is possible to backport to @jazzy, but it must be in main
to have these changes reflected into new distributions.