Visitor Templates for BGL Solver (#80)

* Created header for event visitors

* Added event visitor as template argument to solver base classes

* Revised implementations of Dijkstra search to use event visitor template parameter

* Revised explicit template instantiations of BGL solver classes

* Updated unit test solver factory to work with new BGL event visitor template parameter

* Updated unit test

* Updated benchmarks

* Removed unnecessary FloatType template parameter from visitors

* Implemented macro to simplify explicit template instantiation of BGL solvers

* Clang formatting

* Added check of target vertex cost to Dijkstra search method

descartes_light/solvers/include/descartes_light/solvers/bgl/bgl_dijkstra_solver.h

@@ -2,75 +2,41 @@
 #define DESCARTES_LIGHT_SOLVERS_BGL_BGL_DIJKSTRA_SOLVER_H
 
 #include <descartes_light/solvers/bgl/bgl_solver.h>
+#include <descartes_light/solvers/bgl/event_visitors.h>
 
 namespace descartes_light
 {
 /**
  * @brief BGL solver implementation that constructs vertices and edges in the build function and uses Dijkstra's
  * algorithm with a default visitor to search the graph
  */
-template <typename FloatType>
-class BGLDijkstraSVSESolver : public BGLSolverBaseSVSE<FloatType>
+template <typename FloatType, typename Visitors>
+class BGLDijkstraSVSESolver : public BGLSolverBaseSVSE<FloatType, Visitors>
 {
 public:
-  using BGLSolverBaseSVSE<FloatType>::BGLSolverBaseSVSE;
+  using BGLSolverBaseSVSE<FloatType, Visitors>::BGLSolverBaseSVSE;
 
   SearchResult<FloatType> search() override;
 };
 
-using BGLDijkstraSVSESolverF = BGLDijkstraSVSESolver<float>;
-using BGLDijkstraSVSESolverD = BGLDijkstraSVSESolver<double>;
-
-/**
- * @brief BGL solver implementation that constructs vertices and edges in the build function and uses Dijkstra's
- * algorithm with a visitor that terminates the search once a vertex in the last rung of the graph is encountered rather
- * than allowing it to continue until the distance to all nodes in the graph has been calculated
- */
-template <typename FloatType>
-class BGLEfficientDijkstraSVSESolver : public BGLSolverBaseSVSE<FloatType>
-{
-public:
-  using BGLSolverBaseSVSE<FloatType>::BGLSolverBaseSVSE;
-
-  SearchResult<FloatType> search() override;
-};
-
-using BGLEfficientDijkstraSVSESolverF = BGLEfficientDijkstraSVSESolver<float>;
-using BGLEfficientDijkstraSVSESolverD = BGLEfficientDijkstraSVSESolver<double>;
+using BGLDijkstraSVSESolverF = BGLDijkstraSVSESolver<float, early_terminator<boost::on_examine_vertex>>;
+using BGLDijkstraSVSESolverD = BGLDijkstraSVSESolver<double, early_terminator<boost::on_examine_vertex>>;
 
 /**
  * @brief BGL solver implementation that constructs vertices build function and uses Dijkstra's
  * algorithm with an edge-adding visitor to search the graph
  */
-template <typename FloatType>
-class BGLDijkstraSVDESolver : public BGLSolverBaseSVDE<FloatType>
-{
-public:
-  using BGLSolverBaseSVDE<FloatType>::BGLSolverBaseSVDE;
-
-  SearchResult<FloatType> search() override;
-};
-
-using BGLDijkstraSVDESolverF = BGLDijkstraSVDESolver<float>;
-using BGLDijkstraSVDESolverD = BGLDijkstraSVDESolver<double>;
-
-/**
- * @brief BGL solver implementation that constructs vertices in the build function and uses Dijkstra's
- * algorithm with a visitor that adds edges and terminates the search once a vertex in the last rung of
- * the graph is encountered rather than allowing it to continue until the distance to all nodes in the
- * graph has been calculated
- */
-template <typename FloatType>
-class BGLEfficientDijkstraSVDESolver : public BGLSolverBaseSVDE<FloatType>
+template <typename FloatType, typename Visitors>
+class BGLDijkstraSVDESolver : public BGLSolverBaseSVDE<FloatType, Visitors>
 {
 public:
-  using BGLSolverBaseSVDE<FloatType>::BGLSolverBaseSVDE;
+  using BGLSolverBaseSVDE<FloatType, Visitors>::BGLSolverBaseSVDE;
 
   SearchResult<FloatType> search() override;
 };
 
-using BGLEfficientDijkstraSVDESolverF = BGLEfficientDijkstraSVDESolver<float>;
-using BGLEfficientDijkstraSVDESolverD = BGLEfficientDijkstraSVDESolver<double>;
+using BGLDijkstraSVDESolverF = BGLDijkstraSVDESolver<float, early_terminator<boost::on_examine_vertex>>;
+using BGLDijkstraSVDESolverD = BGLDijkstraSVDESolver<double, early_terminator<boost::on_examine_vertex>>;
 
 }  // namespace descartes_light
 

descartes_light/solvers/include/descartes_light/solvers/bgl/bgl_solver.h

@@ -31,11 +31,11 @@ namespace descartes_light
 /**
  * @brief Partial implementation for solvers leveraging the Boost Graph Library
  */
-template <typename FloatType>
+template <typename FloatType, typename Visitors>
 class BGLSolverBase : public Solver<FloatType>
 {
 public:
-  BGLSolverBase(unsigned num_threads = std::thread::hardware_concurrency());
+  BGLSolverBase(Visitors event_visitors, unsigned num_threads = std::thread::hardware_concurrency());
 
   inline const BGLGraph<FloatType>& getGraph() const { return graph_; }
 
@@ -56,6 +56,9 @@ class BGLSolverBase : public Solver<FloatType>
    */
   std::vector<typename State<FloatType>::ConstPtr> toStates(const std::vector<VertexDesc<FloatType>>& path) const;
 
+  /** @brief Event visitors for custom behavior in the search */
+  Visitors event_visitors_;
+  /** @brief Number of threads for parallel processing */
   unsigned num_threads_;
   /** @brief Graph representation of the planning problem */
   BGLGraph<FloatType> graph_;
@@ -73,11 +76,11 @@ class BGLSolverBase : public Solver<FloatType>
  * @details Constructs only vertices in the build function (i.e. statically) with the assumption that edges will be
  * added during the search (i.e. dynamically)
  */
-template <typename FloatType>
-class BGLSolverBaseSVDE : public BGLSolverBase<FloatType>
+template <typename FloatType, typename Visitors>
+class BGLSolverBaseSVDE : public BGLSolverBase<FloatType, Visitors>
 {
 public:
-  using BGLSolverBase<FloatType>::BGLSolverBase;
+  using BGLSolverBase<FloatType, Visitors>::BGLSolverBase;
 
   BuildStatus buildImpl(const std::vector<typename WaypointSampler<FloatType>::ConstPtr>& trajectory,
                         const std::vector<typename EdgeEvaluator<FloatType>::ConstPtr>& edge_eval,
@@ -91,11 +94,11 @@ class BGLSolverBaseSVDE : public BGLSolverBase<FloatType>
  * @brief BGL solver Static Vertex Static Edge (SVSE) partial implementation
  * @details Constructs both vertices and edges in the build function (i.e. statically)
  */
-template <typename FloatType>
-class BGLSolverBaseSVSE : public BGLSolverBaseSVDE<FloatType>
+template <typename FloatType, typename Visitors>
+class BGLSolverBaseSVSE : public BGLSolverBaseSVDE<FloatType, Visitors>
 {
 public:
-  using BGLSolverBaseSVDE<FloatType>::BGLSolverBaseSVDE;
+  using BGLSolverBaseSVDE<FloatType, Visitors>::BGLSolverBaseSVDE;
 
   BuildStatus buildImpl(const std::vector<typename WaypointSampler<FloatType>::ConstPtr>& trajectory,
                         const std::vector<typename EdgeEvaluator<FloatType>::ConstPtr>& edge_eval,

descartes_light/solvers/include/descartes_light/solvers/bgl/event_visitors.h

@@ -0,0 +1,65 @@
+#ifndef DESCARTES_LIGHT_SOLVERS_BGL_EVENT_VISITORS
+#define DESCARTES_LIGHT_SOLVERS_BGL_EVENT_VISITORS
+
+#include <descartes_light/solvers/bgl/boost_graph_types.h>
+
+#include <descartes_light/descartes_macros.h>
+DESCARTES_IGNORE_WARNINGS_PUSH
+#include <boost/graph/visitors.hpp>
+DESCARTES_IGNORE_WARNINGS_POP
+
+namespace descartes_light
+{
+/**
+ * @brief Event visitor that terminates the search when a vertex in the last rung of the graph is examined
+ * @details Throws the vertex descriptor that is the termination of the path once a vertex in the last rung of
+ * the graph is operated on
+ */
+template <typename EventType>
+struct early_terminator : public boost::base_visitor<early_terminator<EventType>>
+{
+  /** @brief Event filter typedef defining the events for which this visitor can be used */
+  typedef EventType event_filter;
+
+  early_terminator(long last_rung_idx);
+
+  template <typename FloatType>
+  void operator()(VertexDesc<FloatType> u, const BGLGraph<FloatType>& g);
+
+  const long last_rung_idx_;
+};
+
+/**
+ * @brief Event visitor that adds all edges to each vertex dynamically as the vertex is discovered during the graph
+ * search
+ */
+template <typename FloatType, typename EventType>
+struct add_all_edges_dynamically : public boost::base_visitor<add_all_edges_dynamically<FloatType, EventType>>
+{
+  /** @brief Event filter typedef defining the events for which this visitor can be used */
+  typedef EventType event_filter;
+
+  add_all_edges_dynamically(std::vector<typename EdgeEvaluator<FloatType>::ConstPtr> edge_eval,
+                            std::vector<std::vector<VertexDesc<FloatType>>> ladder_rungs);
+
+  void operator()(VertexDesc<FloatType> u, const BGLGraph<FloatType>& g);
+
+  const std::vector<typename EdgeEvaluator<FloatType>::ConstPtr> eval_;
+  const std::vector<std::vector<VertexDesc<FloatType>>> ladder_rungs_;
+};
+
+/**
+ * @brief Event visitor for updating vertex cost
+ */
+struct cost_recorder : public boost::base_visitor<cost_recorder>
+{
+  /** @brief Event filter typedef defining the events for which this visitor can be used */
+  typedef boost::on_tree_edge event_filter;
+
+  template <typename FloatType>
+  void operator()(EdgeDesc<FloatType> e, const BGLGraph<FloatType>& g);
+};
+
+}  // namespace descartes_light
+
+#endif  // DESCARTES_LIGHT_SOLVERS_BGL_EVENT_VISITORS_H