[Smac State Lattice] Support Omni Lateral Motions

[SteveMacenski]

#2582 Brings the state lattice planner in to Nav2 main for advanced beta testing (e.g. should entirely work, but no ability to generate new control sets yet and doesn't support rotations in place)

• Support, or test if natively work, rotations in place - only travel costs adding support for in-place rotations in state lattice #2747
• Support lateral motions in travel costs penalty (penalty for changing too)
• Update lookup table heuristic to be based on the actual control set in use, not on dubin / reeds-shepp models
• Consider analytic expansion changes for omni robots that have the ability to move laterally and/or enable approximate paths with tolerances / max iterations on approach. Find lateral & only use shortest with set motion model (RS/D). (ref from Josh: For analytic expansion I found that running linear interpolation between the cur node pose and the goal pose is more robust for omni systems. Analytic expansions are more successful as expected and the oscillation of the path near the goal is less pronounced)

[SteveMacenski]

Notes for Omni support;

• If potentially just use 4 directions F/R/L/R prim headings, not just straight lateral? The N branch factor doesn't seem to matter 2xing for reverse very much. That would just 4x? Also wouldn't require sp parameter, just runtime setting. Would that make the heuristics more doable with just dubin/reedshepp w/o HLUT?
• Analytic in 4 dir too? (Or really just 2 if reeds sheep is on, since reverse in one is forward in another)— maybe thats the solution for analytic in omni: reeds for both Forward/Back directions, and on lateral (Left/Right) direction. Lookup table would just have to compare with lateral motion too and take the least number.
• Just rotate in place questions for T (just weight?) no need for H changes, those are baked in for dist and obst heuristics already

[SteveMacenski]

See #5231