BGL "efficient" Dijkstra Solver

[marip8]

This PR creates an alternative BGL solver using the Dijkstra search algorithm that uses a custom visitor to terminate the search once the first node in the last "rung" of the graph is encountered. The BGL Dijkstra search with a default visitor terminates once the distance from the start vertex to every vertex in the graph has been calculated, which performs much more work than necessary for most Descartes use-cases

Results

The colors in the diagrams below correspond to the status of the vertices in the search:

• White = unvisited
• Gray = identified as targets in the search frontier but not yet visited
• Black = visited
• Green = visited and selected as part of lowest cost path

BGLDijkstraSolverVE

BGLEfficientDijkstraSolverVE

The new Dijkstra solver implementation visits fewer vertices and thus requires less time to perform the search. The benchmarks will updated in a future PR to quantify how many fewer vertices are visited for planning problems with a variety of sizes.

The next step in the development would be to create a Dijkstra visitor with this same behavior that adds edges dynamically during search rather than upfront during build. This should reduce both the time and memory foot print required by the solver

[marip8]

@colin-lewis-19 @Levi-Armstrong here are some specific reasons for the design choices in #71 that became evident as a part of this PR:

• Why can't a visitor base class be passed into the BGL solver class constructor?
  • A boost visitor is actually a type-eraser, and the requirements for visitors of different search algorithms (Dijkstra, BFS, DFS, A*) are different. A common visitor would have to implement all methods for all types of visitors, most of which would be no-op. This is not impossible, but relatively hacky
  • The boost visitor type erasers are also templates based on the graph and visitor type, so we would have to add an additional template argument to our solver classes to support this. This isn't hard but it starts to make the code unreadable
  • Even if we were to store a visitor type eraser inside the class, there are some issues:
    • Each visitor can affect the search behavior differently. Specifically in this PR, the visitor throws the final vertex descriptor to interrupt the search and uses that descriptor to generate the path, which is very different behavior than the existing Dijkstra solver implementation. We won't be able to anticipate all variations in behavior of the visitors; therefore it doesn't do much good to maintain a visitor type eraser in the solver class
    • Visitors may need to be constructed or configured with information that is only available at search time. Specifically in this PR, the visitor needs to know the index of the waypoint in the trajectory. We could attempt to add a method for generically configuring a visitor (via factory or giving access to the graph, etc.), but that seems overly complicated
• Why can't we use a function pointer (or something equivalent) to specify the boost search algorithm?
  • Each search algorithm function takes different parameters, so it is not easy or straightforward to represent those functions generically

[Levi-Armstrong]

One comment but LGTM.