Remove redundant collision checks in the Smac Planners to optimize performance

[SteveMacenski]

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

[SteveMacenski]

Metrics - using the Nav2 Configuration Guide params and square footprint. Only changes are to enable reeds-shepp and reversing lattice models and primitive interpoliation for worse case, best quality paths.

      footprint: "[[-0.12,-0.12],[-0.12,0.12],[0.12,0.12],[0.12,-0.12]]"
      footprint_padding: 0.01

OG - Circular Footprint

-------  -----------------------  ----------------  ------------------  --------
Planner  Average path length (m)  Average Time (s)  Average cost        Max cost
Smac2D   47.639370331429          0.086773990585    5.061860290119722   73.355
SmacH    47.38869794837135        0.0795818771      2.5960123944431515  76.635
SmacL    47.60278983355273        0.103121812275    2.5983444893147305  78.79
-------  -----------------------  ----------------  ------------------  --------

OG - Square Footprint

-------  -----------------------  -------------------  ------------  --------
Planner  Average path length (m)  Average Time (s)     Average cost  Max cost
Smac2D   48.9120505587239         0.08313646537499998  0.0           0.0
SmacH    48.623306527281294       0.07705760402        0.0           0.0
SmacL    48.89427005891646        0.04611722896        0.0           0.0
-------  -----------------------  -------------------  ------------  --------

-------  -----------------------  -------------------  ------------------  --------
Planner  Average path length (m)  Average Time (s)     Average cost        Max cost
Smac2D   48.245652716683665       0.092420269175       3.531131647234082   56.2
SmacH    47.975177713854535       0.08151648674499999  2.00535412228579    60.66
SmacL    48.20998196686074        0.04928258929        1.8597471918630963  63.955
-------  -----------------------  -------------------  ------------------  --------

New - Circular footprint

-------  -----------------------  -------------------  ------------------  --------
Planner  Average path length (m)  Average Time (s)     Average cost        Max cost
Smac2D   47.639370331429          0.07289994052000001  5.061860290119722   73.355
SmacH    47.395172690250966       0.06941825326500001  2.5343591746756853  77.16
SmacL    47.59425993423111        0.09092634793        2.6181442160250423  78.72
-------  -----------------------  -------------------  ------------------  --------

New - Square Footprint

-------  -----------------------  -------------------  ------------  --------
Planner  Average path length (m)  Average Time (s)     Average cost  Max cost
Smac2D   49.13365922967904        0.08007527833000001  0.0           0.0
SmacH    48.848065460826184       0.09706023092999999  0.0           0.0
SmacL    49.11530369655175        0.09356848702500001  0.0           0.0
-------  -----------------------  -------------------  ------------  --------

-------  -----------------------  -------------------  ------------------  --------
Planner  Average path length (m)  Average Time (s)     Average cost        Max cost
Smac2D   48.245652716683665       0.09150618284500002  3.531131647234082   56.2
SmacH    47.982923425733794       0.07982247542500001  1.8487661335358085  59.71
SmacL    48.210520632553695       0.08297703394500001  1.8759522437913996  64.115
-------  -----------------------  -------------------  ------------------  --------

Appears to be faster for Circular cases (likely due to stopping re-checks), but slower for non-circular footprints - which was the intent of this work. Further investigation is required, there may be a bug or unexpected performance change. Perhaps worth trying to see if the (a) coarse-to-fine is the problem or the (b) caching of collision states for returning -- and if one or the other is sensible to merge while I work on the other. I'm thinking there's a bug somewhere that fine checks are happening more than they should, its related to cache misses for bringing the costmap back into the equation, static/inlining of functions.

[SteveMacenski]

Updated new - Circular:

-------  -----------------------  ----------------  ------------------  --------
Planner  Average path length (m)  Average Time (s)  Average cost        Max cost
Smac2D   47.639370331429          0.073402781955    5.061860290119722   73.355
SmacH    47.395172690250966       0.06915741386     2.5343591746756853  77.16
SmacL    47.59425993423111        0.09037644597     2.6181442160250423  78.72
-------  -----------------------  ----------------  ------------------  --------

Updated new - Square:

-------  -----------------------  --------------------  ------------------  --------
Planner  Average path length (m)  Average Time (s)      Average cost        Max cost
Smac2D   48.245652716683665       0.077554006435        3.531131647234082   56.2
SmacH    47.983503888167306       0.06487024561499999   1.821972585823637   59.39
SmacL    48.208850302906924       0.040086090940000006  1.8889808436100952  64.345
-------  -----------------------  --------------------  ------------------  --------

Reduced to only remove redundant checking without coarse-to-fine update.

Improvement: 2D - 7.5-15%. Hybrid: 13-15%. Lattice: 12-13%.

That's a pretty nice imrpovement!

[codecov]

Codecov Report

Attention: Patch coverage is 96.55172% with 1 line in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
nav2_smac_planner/src/a_star.cpp	66.66%	1 Missing ⚠️

Files with missing lines	Coverage Δ
..._smac_planner/include/nav2_smac_planner/a_star.hpp	50.00% <ø> (ø)
...smac_planner/include/nav2_smac_planner/node_2d.hpp	100.00% <ø> (ø)
..._planner/include/nav2_smac_planner/node_hybrid.hpp	95.65% <ø> (ø)
...planner/include/nav2_smac_planner/node_lattice.hpp	100.00% <ø> (ø)
nav2_smac_planner/src/node_2d.cpp	98.18% <100.00%> (+0.14%)	⬆️
nav2_smac_planner/src/node_hybrid.cpp	90.27% <100.00%> (+0.09%)	⬆️
nav2_smac_planner/src/node_lattice.cpp	89.23% <100.00%> (+0.03%)	⬆️
nav2_smac_planner/src/a_star.cpp	91.75% <66.66%> (-0.44%)	⬇️

... and 6 files with indirect coverage changes

[BCKSELFDRIVEWORLD]

Hello @SteveMacenski , we obtained a similar result during the tests conducted on Saha Robotic's Speedy service robots in the field.

OG

Planner          Average path length (m)  Average Time (s)    Average cost        Max cost
GridBasedStatic  51.55709706238913        0.9006800704600001  10.496236574113523  41.6

New

---------------  -----------------------  ------------------  ------------------  --------
Planner          Average path length (m)  Average Time (s)    Average cost        Max cost
GridBasedStatic  51.55709706238913        0.7960560523400001  10.496236574113523  41.6
---------------  -----------------------  ------------------  ------------------  --------

[SteveMacenski]

I have got to love that determinism 😄 That's about 11.5%!