State Lattice Planner addition

[SteveMacenski]

State Lattice

• Leverages dynamic programming and distance heuristics from A* for increased speed
• Leverages Hybrid-A* collision checking and smoothing capabilities as well
• New control set computation methods (TODO for branch)

TODOs:

• Merge in Matt's work on control set generation with a handful of useful default files (ackermann, diff drive, omni of a few basic resolution or turning radius options)
• Merge in Josh's traversal cost functions
• Merge in Josh's primitives file parsing code
• Test integration of these components for speed and path quality
• Collision check between end points for state validity of the primitive expansion + include points in final path
• Non-equal bin size integration for state lattice
• Reconsider default algorithms with this in its final form
• Add State Lattice test coverage and add Hybrid-A* / State Lattice into system tests

Updates from meeting:

• Make ackermann + diff (add L/R turns with turn in place cost) + omni (?)
• param to allow for reverse direction expansions as well
• Omni: if goal is in cove can only be achieved going sideways, even if we added a sideways prim, the heuristics on dubin will be terrible because it assumes only forward motion. Further, analytic expansion wouldn't help get to goal in that situation so it would take much longer to search just the right branch. Maybe need to have a disable analytic expansion option and/or a param for using just L2 distances for omni? Other option would be to force users to have a HLUT precomputed instead on BFS. Or have heuristic for side ways motion primitives be L2 instead? Have analytic expansion also work on 90/270 deg for lateral? maybe compute motion heuristic at each forward, left, and right side of the robot and expand in the direction of lowest distance to goal?

[SteveMacenski]

Closing -- new WIP branch to open shortly