BGL Depth First Search (#77)

* Make default initial distance of vertex equal to float max

* Implement BGL depth first search

* Updated DFS solver to use Visitors template

* Updated test library to create solver factories for the various BGL DFS solvers

* Updated unit test with DFS solvers

* Updated benchmarks with DFS solvers

* Updated Dijkstra solver documentation

* Revised explicit template instantiation of BGL solvers

* Removed visitors template from BGL solver partial implementations

* Added macro for explicitly instantiating a template class for a sequence of types; revised explicit template instantiation of BGL solver partial implementations

* Updated unit test template parameter to be the solver itself instead of the factory

Co-authored-by: Michael Ripperger <michael.ripperger@swri.org>
Co-authored-by: ctlewis <colin.lewis@swri.org>

descartes_light/solvers/include/descartes_light/solvers/bgl/bgl_dfs_solver.h

@@ -0,0 +1,71 @@
+/*
+ * Software License Agreement (Apache License)
+ *
+ * Copyright (c) 2021, Southwest Research Institute
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef DESCARTES_LIGHT_SOLVERS_BGL_BGL_DFS_SOLVER_H
+#define DESCARTES_LIGHT_SOLVERS_BGL_BGL_DFS_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 a depth first
+ * search with a specifiable visitor to search the graph
+ * @details Note: the solver internally creates event visitors for recording predecessors and vertex distances.
+ * Therefore, these event visitors do not need to be specified as template parameters
+ */
+template <typename FloatType, typename Visitors>
+class BGLDepthFirstSVSESolver : public BGLSolverBaseSVSE<FloatType>
+{
+public:
+ BGLDepthFirstSVSESolver(Visitors event_visitors, unsigned num_threads = std::thread::hardware_concurrency());
+ SearchResult<FloatType> search() override;
+
+protected:
+ /** @brief Event visitors for custom behavior in the search */
+ Visitors event_visitors_;
+};
+
+using BGLDepthFirstSVSESolverF = BGLDepthFirstSVSESolver<float, early_terminator<boost::on_discover_vertex>>;
+using BGLDepthFirstSVSESolverD = BGLDepthFirstSVSESolver<double, early_terminator<boost::on_discover_vertex>>;
+
+/**
+ * @brief BGL solver implementation that constructs vertices build function and uses a depth first search
+ * with an edge-adding visitor to search the graph
+ * @details Note: the solver internally creates event visitors for recording predecessors and vertex distances and for
+ * adding edges dynamically to the graph. Therefore, these event visitors do not need to be specified as template
+ * parameters
+ */
+template <typename FloatType, typename Visitors>
+class BGLDepthFirstSVDESolver : public BGLSolverBaseSVDE<FloatType>
+{
+public:
+ BGLDepthFirstSVDESolver(Visitors event_visitors, unsigned num_threads = std::thread::hardware_concurrency());
+ SearchResult<FloatType> search() override;
+
+protected:
+ /** @brief Event visitors for custom behavior in the search */
+ Visitors event_visitors_;
+};
+
+using BGLDepthFirstSVDESolverF = BGLDepthFirstSVDESolver<float, early_terminator<boost::on_discover_vertex>>;
+using BGLDepthFirstSVDESolverD = BGLDepthFirstSVDESolver<double, early_terminator<boost::on_discover_vertex>>;
+
+} // namespace descartes_light
+
+#endif // DESCARTES_LIGHT_SOLVERS_BGL_BGL_DFS_SOLVER_H

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

@@ -8,15 +8,18 @@ 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
+ * algorithm with a specifiable visitor to search the graph
  */
 template <typename FloatType, typename Visitors>
-class BGLDijkstraSVSESolver : public BGLSolverBaseSVSE<FloatType, Visitors>
+class BGLDijkstraSVSESolver : public BGLSolverBaseSVSE<FloatType>
 {
 public:
- using BGLSolverBaseSVSE<FloatType, Visitors>::BGLSolverBaseSVSE;
-
+ BGLDijkstraSVSESolver(Visitors event_visitors, unsigned num_threads = std::thread::hardware_concurrency());
  SearchResult<FloatType> search() override;
+
+protected:
+ /** @brief Event visitors for custom behavior in the search */
+ Visitors event_visitors_;
 };
 
 using BGLDijkstraSVSESolverF = BGLDijkstraSVSESolver<float, early_terminator<boost::on_examine_vertex>>;
@@ -25,14 +28,19 @@ using BGLDijkstraSVSESolverD = BGLDijkstraSVSESolver<double, early_terminator<bo
 /**
  * @brief BGL solver implementation that constructs vertices build function and uses Dijkstra's
  * algorithm with an edge-adding visitor to search the graph
+ * @details Note: the solver internally creates an event visitor for adding edges dynamically to the graph. Therefore,
+ * this event visitor does not need to be specified as a template parameter
  */
 template <typename FloatType, typename Visitors>
-class BGLDijkstraSVDESolver : public BGLSolverBaseSVDE<FloatType, Visitors>
+class BGLDijkstraSVDESolver : public BGLSolverBaseSVDE<FloatType>
 {
 public:
- using BGLSolverBaseSVDE<FloatType, Visitors>::BGLSolverBaseSVDE;
-
+ BGLDijkstraSVDESolver(Visitors event_visitors, unsigned num_threads = std::thread::hardware_concurrency());
  SearchResult<FloatType> search() override;
+
+protected:
+ /** @brief Event visitors for custom behavior in the search */
+ Visitors event_visitors_;
 };
 
 using BGLDijkstraSVDESolverF = BGLDijkstraSVDESolver<float, early_terminator<boost::on_examine_vertex>>;

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, typename Visitors>
+template <typename FloatType>
 class BGLSolverBase : public Solver<FloatType>
 {
 public:
- BGLSolverBase(Visitors event_visitors, unsigned num_threads = std::thread::hardware_concurrency());
+ BGLSolverBase(unsigned num_threads = std::thread::hardware_concurrency());
 
  inline const BGLGraph<FloatType>& getGraph() const { return graph_; }
 
@@ -56,8 +56,6 @@ 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 */
@@ -76,11 +74,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, typename Visitors>
-class BGLSolverBaseSVDE : public BGLSolverBase<FloatType, Visitors>
+template <typename FloatType>
+class BGLSolverBaseSVDE : public BGLSolverBase<FloatType>
 {
 public:
- using BGLSolverBase<FloatType, Visitors>::BGLSolverBase;
+ using BGLSolverBase<FloatType>::BGLSolverBase;
 
  BuildStatus buildImpl(const std::vector<typename WaypointSampler<FloatType>::ConstPtr>& trajectory,
  const std::vector<typename EdgeEvaluator<FloatType>::ConstPtr>& edge_eval,
@@ -94,11 +92,11 @@ class BGLSolverBaseSVDE : public BGLSolverBase<FloatType, Visitors>
  * @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, typename Visitors>
-class BGLSolverBaseSVSE : public BGLSolverBaseSVDE<FloatType, Visitors>
+template <typename FloatType>
+class BGLSolverBaseSVSE : public BGLSolverBaseSVDE<FloatType>
 {
 public:
- using BGLSolverBaseSVDE<FloatType, Visitors>::BGLSolverBaseSVDE;
+ using BGLSolverBaseSVDE<FloatType>::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/boost_graph_types.h

@@ -42,7 +42,7 @@ struct Vertex
  long rung_idx{ -1 };
 
  /** @brief Distance from the start node, assigned graph algorithm at search-time */
- FloatType distance{ static_cast<FloatType>(0.0) };
+ FloatType distance{ std::numeric_limits<FloatType>::max() };
  /** @brief Search status "color" of the vertex: visited, opened, unvisited. Assigned by the graph algorithm at
  * search-time */
  ColorT color{ 0 };

descartes_light/solvers/include/descartes_light/solvers/bgl/impl/bgl_dfs_solver.hpp

@@ -0,0 +1,156 @@
+/*
+ * Software License Agreement (Apache License)
+ *
+ * Copyright (c) 2021, Southwest Research Institute
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef DESCARTES_LIGHT_SOLVERS_BGL_IMPL_DFS_SOLVER_HPP
+#define DESCARTES_LIGHT_SOLVERS_BGL_IMPL_DFS_SOLVER_HPP
+
+#include <descartes_light/solvers/bgl/bgl_dfs_solver.h>
+#include <descartes_light/solvers/bgl/impl/event_visitors.hpp>
+
+#include <descartes_light/descartes_macros.h>
+DESCARTES_IGNORE_WARNINGS_PUSH
+#include <boost/graph/depth_first_search.hpp>
+DESCARTES_IGNORE_WARNINGS_POP
+
+namespace descartes_light
+{
+template <typename FloatType, typename Visitors>
+static VertexDesc<FloatType> solveDFS(BGLGraph<FloatType>& graph,
+ const VertexDesc<FloatType>& source,
+ const Visitors& event_visitors,
+ const std::vector<std::vector<VertexDesc<FloatType>>>& ladder_rungs)
+{
+ auto color_prop_map = boost::get(&Vertex<FloatType>::color, graph);
+ auto visitor = boost::make_dfs_visitor(event_visitors);
+
+ try
+ {
+ graph[source].distance = 0.0;
+ boost::depth_first_search(graph, visitor, color_prop_map, source);
+
+ // In the case that the visitor does not throw the target vertex descriptor, find the lowest cost vertex in last
+ // rung of the ladder graph
+ auto target = std::min_element(ladder_rungs.back().begin(),
+ ladder_rungs.back().end(),
+ [&](const VertexDesc<FloatType>& a, const VertexDesc<FloatType>& b) {
+ return graph[a].distance < graph[b].distance;
+ });
+
+ // Check that the identified lowest cost vertex is valid and has a cost less than inf
+ if (target != ladder_rungs.back().end() && graph[*target].distance < std::numeric_limits<FloatType>::max())
+ throw *target;
+ }
+ catch (const VertexDesc<FloatType>& target)
+ {
+ return target;
+ }
+
+ throw std::runtime_error("DFS search failed to encounter a vertex in the last rung of the plan graph");
+}
+
+template <typename FloatType, typename Visitors>
+BGLDepthFirstSVSESolver<FloatType, Visitors>::BGLDepthFirstSVSESolver(Visitors event_visitors, unsigned num_threads)
+ : BGLSolverBaseSVSE<FloatType>(num_threads), event_visitors_(std::move(event_visitors))
+{
+}
+
+template <typename FloatType, typename Visitors>
+SearchResult<FloatType> BGLDepthFirstSVSESolver<FloatType, Visitors>::search()
+{
+ // Internal properties
+ auto& graph_ = BGLSolverBase<FloatType>::graph_;
+ const auto& source_ = BGLSolverBase<FloatType>::source_;
+ auto& predecessors_ = BGLSolverBase<FloatType>::predecessors_;
+ const auto& ladder_rungs_ = BGLSolverBase<FloatType>::ladder_rungs_;
+
+ // Resize the container of predecessors to match the number of vertices
+ // Assign the initial values as a vertex descriptor that is definitely not in the graph to avoid mistakes in the
+ // future reconstructing the path
+ predecessors_.resize(boost::num_vertices(graph_), std::numeric_limits<VertexDesc<FloatType>>::max());
+
+ // Assign the predecessor of the source node to itself since the BGL DFS will not do it
+ predecessors_.at(source_) = source_;
+
+ // Create an event visitor that combines the internally specified visitors with ones that record predecessors and
+ // vertex costs (i.e. distances)
+ auto visitor = std::make_pair(boost::record_predecessors(predecessors_.data(), boost::on_tree_edge()),
+ std::make_pair(cost_recorder(), event_visitors_));
+
+ const VertexDesc<FloatType> target = solveDFS(graph_, source_, visitor, ladder_rungs_);
+
+ SearchResult<FloatType> result;
+
+ // Reconstruct the path from the predecesor map; remove the artificial start state
+ const auto vd_path = BGLSolverBase<FloatType>::reconstructPath(source_, target);
+ result.trajectory = BGLSolverBase<FloatType>::toStates(vd_path);
+ result.trajectory.erase(result.trajectory.begin());
+
+ result.cost = graph_[target].distance;
+
+ return result;
+}
+
+template <typename FloatType, typename Visitors>
+BGLDepthFirstSVDESolver<FloatType, Visitors>::BGLDepthFirstSVDESolver(Visitors event_visitors, unsigned num_threads)
+ : BGLSolverBaseSVDE<FloatType>(num_threads), event_visitors_(std::move(event_visitors))
+{
+}
+
+template <typename FloatType, typename Visitors>
+SearchResult<FloatType> BGLDepthFirstSVDESolver<FloatType, Visitors>::search()
+{
+ // Internal properties
+ auto& graph_ = BGLSolverBase<FloatType>::graph_;
+ const auto& source_ = BGLSolverBase<FloatType>::source_;
+ auto& predecessors_ = BGLSolverBase<FloatType>::predecessors_;
+ const auto& ladder_rungs_ = BGLSolverBase<FloatType>::ladder_rungs_;
+
+ // Resize the container of predecessors to match the number of vertices
+ // Assign the initial values as a vertex descriptor that is definitely not in the graph to avoid mistakes in the
+ // future reconstructing the path
+ predecessors_.resize(boost::num_vertices(graph_), std::numeric_limits<VertexDesc<FloatType>>::max());
+
+ // Assign the predecessor of the source node to itself since the BGL DFS will not do it
+ predecessors_.at(source_) = source_;
+
+ // Make a visitor that combines the internally specified event visitors with ones that add all edges dynamically and
+ // record predecessors and vertex costs (i.e. distances)
+ const auto& edge_eval_ = BGLSolverBaseSVDE<FloatType>::edge_eval_;
+ auto visitor = std::make_pair(
+ boost::record_predecessors(predecessors_.data(), boost::on_tree_edge()),
+ std::make_pair(
+ cost_recorder(),
+ std::make_pair(add_all_edges_dynamically<FloatType, boost::on_discover_vertex>(edge_eval_, ladder_rungs_),
+ event_visitors_)));
+
+ const VertexDesc<FloatType> target = solveDFS(graph_, source_, visitor, ladder_rungs_);
+
+ SearchResult<FloatType> result;
+
+ // Reconstruct the path from the predecesor map; remove the artificial start state
+ const auto vd_path = BGLSolverBase<FloatType>::reconstructPath(source_, target);
+ result.trajectory = BGLSolverBase<FloatType>::toStates(vd_path);
+ result.trajectory.erase(result.trajectory.begin());
+
+ result.cost = graph_[target].distance;
+
+ return result;
+}
+
+} // namespace descartes_light
+
+#endif // DESCARTES_LIGHT_SOLVERS_BGL_IMPL_DFS_SOLVER_HPP