Fix zig-zaggin in smac planner due to discrete angular bin quantizations

[SteveMacenski]

Per #3172, we sometimes see zig-zagging behavior due to the discrete increments of angular quanitizations not fully utilizing the requested angular resolution. This issue is written out in detail in this comment: #3172 (comment)

This solves that by creating additional motion primitives to fill in the angular space for each expansion to hit every angular bin possible. As a result, keeping angular bin densities of 64/72 as previously utilized will create many more new primitives for certain configurations than previously. It may be wise to reduce your resolution to not create additional primitives that your application does not require.

TODO

• performance testing across reeds-shepp, dubin, various turning rads (0.05, 0.1, 0.4, 0.5, 1.0, 2.0, 10.0), and non-circular robots for path quality and runtime.
• Unit testing of new methods like populateExpansionsLog
• Traversal cost for primitives of turns
• Dynamic parameter support
• Document change: reduce resolution (72 no longer needed), new param in guide + migration guide. Post on Discourse about this major change for folks to update their config files adding zig zag docs docs.nav2.org#459
• Find new best practice resolution to use (e.g. 16, 32, 64, 72, ...) with rationale. More bins = more prims; lower $R_t$ = more prims
• test on realistic maps, not just benchmarking scripts -- why don't I see more change between the paths with various turning radii? Shouldn't the curves be very different? Need to look into that manually, something doesn't seem quite right (as if the static members are being shared between them?) or does the cost-awareness generally just guide it into the center so they have similar influences? I need to plot some turns. Or has to do with the number of smaller primitives available?

[SteveMacenski]

Benchmarking:

-----------------  -----------------------  --------------------  ------------------  --------
Planner            Average path length (m)  Average Time (s)      Average cost        Max cost
NORMAL_DUBIN       49.67057981917864        0.056411659300000014  26.42893242594106   98.44
NORMAL_REEDSSHEPP  49.66429946425317        0.056731877410000006  26.44040309517375   98.47
ON_DUBIN           49.670592919255476       0.05622163887999999   26.428906231633896  98.44
ON_REEDSSHEPP      49.66603145368965        0.05642396648999999   26.437818514359627  98.47
-----------------  -----------------------  --------------------  ------------------  --------

For previous changes to current changes in both the dubins and reeds-shepp models on the random benchmark map kindly provided by @jwallace42. No substantive changes needing optimization wrt either model

Turning radius tests next to see how it scales with primitive numbers

[SteveMacenski]

------------  -----------------------  -------------------  ------------------  --------
Planner       Average path length (m)  Average Time (s)     Average cost        Max cost
DUBIN_005     48.0687865862458         0.34696313113000005  33.63145014373353   113.14
DUBIN_01      48.05450925271065        0.33147114150999996  33.680642759452276  113.14
DUBIN_04      48.052733500317366       0.33054843583        33.683575324139866  113.14
DUBIN_10      48.052764603818105       0.32961878808        33.65879248833779   113.03
DUBIN_20      48.15930088892668        0.33270151082999994  33.91807622385019   115.74
DUBIN_005_ON  48.05101334085401        0.33693136072999996  33.69104135579259   113.14
DUBIN_01_ON   48.05975069796279        0.3672095316         33.66681511255692   113.14
DUBIN_04_ON   48.052900739188914       0.33240715681000005  33.68058711888695   113.14
DUBIN_10_ON   48.052764603818105       0.33412878039        33.65879248833779   113.03
DUBIN_20_ON   48.15930088892668        0.33433825997        33.91807622385019   115.74
------------  -----------------------  -------------------  ------------------  --------

For different turning radius'. First set are normal, second set are with the addtl primitives set on. More or less all the same or not notably different worth additional optimization. Repeated for RS

------------  -----------------------  ------------------  ------------------  --------
Planner       Average path length (m)  Average Time (s)    Average cost        Max cost
RS_005     43.65324072789744        0.6436250618000001  32.31692836447744   107.76
RS_01      43.63994696418641        0.6286339329499999  32.366211023211044  107.76
RS_04      43.637574425585875       0.6234673631        32.36706592640516   107.81
RS_10      43.636851297064844       0.62251453475       32.35099343403885   107.72
RS_20      43.69145877943302        0.6204847193199999  32.34252102557314   108.28
RS_ON     43.63690600772342        0.6254052830599999  32.370333885560356  107.76
RS_ON      43.64639300765541        0.6358236409899999  32.34366374449997   107.76
RS_ON      43.63763222137749        0.6308510688199999  32.366870994631284  107.81
RS_ON      43.636851297064844       0.6383208146099999  32.35099343403885   107.72
RS_ON      43.69145877943302        0.6360818663700001  32.34252102557314   108.28
------------  -----------------------  ------------------  ------------------  --------

[stevedanomodolor]

There seems to be not much of a difference between the activation of the new parameter, I tried using 64, 72 and with the new parameter on, and the behavior continues.

[SteveMacenski]

Thanks for the quick analysis! I think its due to me not updating this function yet (https://github.com/ros-planning/navigation2/blob/main/nav2_smac_planner/src/node_hybrid.cpp#L303) that was brought up in the ticket. I think because its not 0,1,2, we're penalizing it the most and also giving it the reversing penalty. I think that's why you see it happening sometimes (e.g. the graph shows occasional uses of the other bins) but not as regularly as it should be. I hope, at least :-)

I'll work on that tomorrow, hopefully that will resolve the issue! I'll need to re-run the testing metrics above, but that's relatively straight forward. I appreciate your experimentation with the charts, it was illuminating!

[SteveMacenski]

Traversal cost function updates made, but it involved some surgical changes so I'm going to be extremely careful in testing to make sure this still works as expected since it involved modifications that could be subtly error prone. I looked carefully over them and added a couple more unit tests to sanity check the actual traversal cost function, but since there were updates to the projections computation and how the data is propagated during search, its worth careful consideration.

@stevedanomodolor please test again :-)

[SteveMacenski]

With updates:

-----------------  -----------------------  -------------------  ------------------  --------
Planner            Average path length (m)  Average Time (s)     Average cost        Max cost
NORMAL_DUBIN       48.62226744406985        0.08540430985999999  2.315272051418369   91.66
NORMAL_REEDSSHEPP  48.620874918802116       0.08607285177000001  2.327718953474646   91.66
ON_DUBIN           48.62135754274054        0.08432577783999999  2.3343595784746456  91.8
ON_REEDSSHEPP      48.619247989185006       0.08586728219999998  2.3044361077010693  91.78
-----------------  -----------------------  -------------------  ------------------  --------

I'm messing with costmap settings, so the actual values aren't really important. The comparison with the original method is the important part.

-------------  -----------------------  -------------------  ------------------  --------
Planner        Average path length (m)  Average Time (s)     Average cost        Max cost
NORMAL_DUBIN   48.46639022624647        0.38991617460999994  33.311832403525656  111.88
NORMAL_DUBIN2  48.46688734930349        0.37671986905        33.31204292528036   111.88
NORMAL_DUBIN3  48.5784431139699         0.37746076980999993  33.517786595032454  114.44
ON_DUBIN       48.46639022624647        0.37777031248999987  33.311832403525656  111.88
ON_DUBIN2      48.46010691573797        0.3620015351         33.32492302248469   111.88
ON_DUBIN3      48.57142159711799        0.36241974367000007  33.53521806264665   114.44
-------------  -----------------------  -------------------  ------------------  --------

(for 0.05, 0.4, 2.0 turning radii).

I am seeing this use alot more search iterations (2x+) for some reason that I'm not totally sure about (even when the setting is off, I believe?) I need to do some looking into this. I'm testing right now by recompiling the current base branch and re-running some of my experiments to sanity check myself. I might just be gaslighting myself. Edit - the "off" mode is the same as the base branch, good to know.

See #3172 (comment) and below for more performance discussion

[codecov]

Codecov Report

Patch coverage: 71.24% and project coverage change: +0.81% 🎉

Comparison is base (c1e7713) 89.57% compared to head (e0f4057) 90.39%.
Report is 8 commits behind head on main.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #3752      +/-   ##
==========================================
+ Coverage   89.57%   90.39%   +0.81%     
==========================================
  Files         411      411              
  Lines       17894    18003     +109     
==========================================
+ Hits        16029    16274     +245     
+ Misses       1865     1729     -136     

Files Changed	Coverage Δ
..._smac_planner/include/nav2_smac_planner/a_star.hpp	50.00% <ø> (ø)
...c_planner/include/nav2_smac_planner/node_basic.hpp	100.00% <ø> (ø)
nav2_smac_planner/src/node_lattice.cpp	89.38% <ø> (-2.05%)	⬇️
nav2_smac_planner/src/smac_planner_hybrid.cpp	83.18% <46.87%> (-3.17%)	⬇️
nav2_smac_planner/src/a_star.cpp	92.61% <53.84%> (-0.25%)	⬇️
nav2_smac_planner/src/node_hybrid.cpp	89.84% <79.80%> (-3.81%)	⬇️
..._planner/include/nav2_smac_planner/node_hybrid.hpp	93.33% <100.00%> (-6.67%)	⬇️
...2_smac_planner/include/nav2_smac_planner/types.hpp	100.00% <100.00%> (ø)
nav2_smac_planner/src/node_basic.cpp	100.00% <100.00%> (ø)

... and 19 files with indirect coverage changes

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

[stevedanomodolor]

I performed some more tests based on the new changes, the behavior does not appear and the planner is able to use more of the bin space, although one thing to watch out for is the increment of planning time which make sense because we have increased the number of primitives used during the planning. With respect to planning time, when using the default minimum turning radius, 72 seems to be the optimal number of quantization bins. The results of the graphs are for 64, 72, 124, and 256 while the table includes more tests in between.
It would be nice to try this on a different map to see how it behaves, if you have any more map that i could try this on too.

Comparision

Figures

64 bins no interpolation

64 bins interpolation

72 bins no interpolation

72 bins interpolation

124 bins no interpolation

124 bins interpolation

256 bins no interpolation

256 bins interpolation

[SteveMacenski]

Great thanks for the detail!

[SteveMacenski]

@stevedanomodolor see new changes adding the additional features which can be enabled using the parameters

      downsample_obstacle_heuristic: false
      use_quadratic_cost_penalty: true