Fixes smac planner non-circular footprint search issue

[SteveMacenski]

Resolves #4808 and #4314

The optimization was added to allow non-circular robots to use their full footprint costs in the traversibility function and heuristic function formulations. However, in confined settings, it doesn't work very well and its better to collision check using the full footprint but continue to use the center point costs for the traversal function / heuristic function formulations.

Metrics outlined in #4808 (comment) show that this not only fixes the issue but also doesn't impact speeds in open areas - but faster in confined areas due to the ambiguation of inscribed costs.

TODO

• benchmark
• backport

[SteveMacenski]

OG:

----------  -----------------------  ----------------  -----------------  --------
Planner     Average path length (m)  Average Time (s)  Average cost       Max cost
GridBased   51.10449455111457        0.133985194292    7.351264392666914  100.928
GridBased2  51.506940648368534       0.099103423996    7.247839998853134  103.572
----------  -----------------------  ----------------  -----------------  --------

Updated:

----------  -----------------------  -------------------  ------------------  --------
Planner     Average path length (m)  Average Time (s)     Average cost        Max cost
GridBased   51.115783254051415       0.116549856532       3.0431751735718704  84.04
GridBased2  51.517709317230256       0.07625796589600001  2.531342577997387   87.088
----------  -----------------------  -------------------  ------------------  --------

We see no major difference in path lengths, but these updates result in lower total costs and slightly improved compute times. First is Hybrid, second is Lattice