Update Smac Planner to use previous planning experiences for future re-plannings

[SteveMacenski]

There have been discussions over time about how to handle replanning such that plans follow generally the same route to avoid oscillations or create more predictable paths (even if those paths are technically less optimal).

I've proposed in the past to (a) checking for path cost changes or (b) replan and check if the new plan is 'sufficiently better' to override the existing one. Both of these can be implemented at the behavior tree level which provides us some flexibility to define the behavior better.

Another option would be to use Experience Graphs within the planner such that future plans towards the same goal take into account previous iteration's search to steer it faster and generally in the same route as previously (@mikeferguson). It might also be worth seeing if we can "warm up" the graph search by pre-queuing the search with the previous path before opening up to general search so that we attempt to find solutions in the same general tree as the previous solution was found.

This ticket doesn't have a precise implementation or details, but opening up a discussion on enabling more reliable replanning and finding community interest to help implement or provide their thoughts / ideas about what would be best for their applications

[mikeferguson]

Here's a few notes I had sitting around when I started to look at adding E-graph support to the SMAC Lattice Planner (like two years ago):

• Main steps would be:
  • Add functions for saving/loading experiences (this should be done first to enable testing - this is the kind of feature that suits itself well to test driven development)
  • Update getNeighbors in the NodeT class to return the regular neighbors + any connections on the experience graph
  • Update getHeuristicCost in NodeT since edges on the experience graph would have lower cost (this is the tricky part)
• Indices are based on lattice coordinates - so that’s actually easy - no worry about shifting around and whatnot as was the case with the SBPL version of E-graphs
• “Approach” should really be called refinement (iterations)