From d56038bedba31d03a11dcf8f1e59bc0f927267c9 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Tue, 7 Sep 2021 13:55:47 -0500 Subject: [PATCH] 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 --- .../solvers/bgl/bgl_dijkstra_solver.h | 56 +---- .../descartes_light/solvers/bgl/bgl_solver.h | 19 +- .../solvers/bgl/event_visitor.h | 4 - .../solvers/bgl/event_visitors.h | 65 +++++ .../solvers/bgl/impl/bgl_dijkstra_solver.hpp | 233 +++++------------- .../solvers/bgl/impl/bgl_solver.hpp | 46 ++-- .../solvers/bgl/impl/event_visitors.hpp | 120 ++++----- .../solvers/src/bgl/bgl_solver.cpp | 58 +++-- .../test/benchmarks/benchmarks.cpp | 18 +- .../test/impl/solver_factory.hpp | 32 ++- .../descartes_light/test/solver_factory.h | 2 +- descartes_light/test/src/solver_factory.cpp | 13 +- descartes_light/test/utest.cpp | 12 +- 13 files changed, 304 insertions(+), 374 deletions(-) delete mode 100644 descartes_light/solvers/include/descartes_light/solvers/bgl/event_visitor.h create mode 100644 descartes_light/solvers/include/descartes_light/solvers/bgl/event_visitors.h diff --git a/descartes_light/solvers/include/descartes_light/solvers/bgl/bgl_dijkstra_solver.h b/descartes_light/solvers/include/descartes_light/solvers/bgl/bgl_dijkstra_solver.h index b4f8d15a..fdaef57a 100644 --- a/descartes_light/solvers/include/descartes_light/solvers/bgl/bgl_dijkstra_solver.h +++ b/descartes_light/solvers/include/descartes_light/solvers/bgl/bgl_dijkstra_solver.h @@ -2,6 +2,7 @@ #define DESCARTES_LIGHT_SOLVERS_BGL_BGL_DIJKSTRA_SOLVER_H #include +#include namespace descartes_light { @@ -9,68 +10,33 @@ 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 -class BGLDijkstraSVSESolver : public BGLSolverBaseSVSE +template +class BGLDijkstraSVSESolver : public BGLSolverBaseSVSE { public: - using BGLSolverBaseSVSE::BGLSolverBaseSVSE; + using BGLSolverBaseSVSE::BGLSolverBaseSVSE; SearchResult search() override; }; -using BGLDijkstraSVSESolverF = BGLDijkstraSVSESolver; -using BGLDijkstraSVSESolverD = BGLDijkstraSVSESolver; - -/** - * @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 -class BGLEfficientDijkstraSVSESolver : public BGLSolverBaseSVSE -{ -public: - using BGLSolverBaseSVSE::BGLSolverBaseSVSE; - - SearchResult search() override; -}; - -using BGLEfficientDijkstraSVSESolverF = BGLEfficientDijkstraSVSESolver; -using BGLEfficientDijkstraSVSESolverD = BGLEfficientDijkstraSVSESolver; +using BGLDijkstraSVSESolverF = BGLDijkstraSVSESolver>; +using BGLDijkstraSVSESolverD = BGLDijkstraSVSESolver>; /** * @brief BGL solver implementation that constructs vertices build function and uses Dijkstra's * algorithm with an edge-adding visitor to search the graph */ -template -class BGLDijkstraSVDESolver : public BGLSolverBaseSVDE -{ -public: - using BGLSolverBaseSVDE::BGLSolverBaseSVDE; - - SearchResult search() override; -}; - -using BGLDijkstraSVDESolverF = BGLDijkstraSVDESolver; -using BGLDijkstraSVDESolverD = BGLDijkstraSVDESolver; - -/** - * @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 -class BGLEfficientDijkstraSVDESolver : public BGLSolverBaseSVDE +template +class BGLDijkstraSVDESolver : public BGLSolverBaseSVDE { public: - using BGLSolverBaseSVDE::BGLSolverBaseSVDE; + using BGLSolverBaseSVDE::BGLSolverBaseSVDE; SearchResult search() override; }; -using BGLEfficientDijkstraSVDESolverF = BGLEfficientDijkstraSVDESolver; -using BGLEfficientDijkstraSVDESolverD = BGLEfficientDijkstraSVDESolver; +using BGLDijkstraSVDESolverF = BGLDijkstraSVDESolver>; +using BGLDijkstraSVDESolverD = BGLDijkstraSVDESolver>; } // namespace descartes_light diff --git a/descartes_light/solvers/include/descartes_light/solvers/bgl/bgl_solver.h b/descartes_light/solvers/include/descartes_light/solvers/bgl/bgl_solver.h index ae2501ad..fda0d9a3 100644 --- a/descartes_light/solvers/include/descartes_light/solvers/bgl/bgl_solver.h +++ b/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 +template class BGLSolverBase : public Solver { public: - BGLSolverBase(unsigned num_threads = std::thread::hardware_concurrency()); + BGLSolverBase(Visitors event_visitors, unsigned num_threads = std::thread::hardware_concurrency()); inline const BGLGraph& getGraph() const { return graph_; } @@ -56,6 +56,9 @@ class BGLSolverBase : public Solver */ std::vector::ConstPtr> toStates(const std::vector>& 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 graph_; @@ -73,11 +76,11 @@ class BGLSolverBase : public Solver * @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 -class BGLSolverBaseSVDE : public BGLSolverBase +template +class BGLSolverBaseSVDE : public BGLSolverBase { public: - using BGLSolverBase::BGLSolverBase; + using BGLSolverBase::BGLSolverBase; BuildStatus buildImpl(const std::vector::ConstPtr>& trajectory, const std::vector::ConstPtr>& edge_eval, @@ -91,11 +94,11 @@ class BGLSolverBaseSVDE : public BGLSolverBase * @brief BGL solver Static Vertex Static Edge (SVSE) partial implementation * @details Constructs both vertices and edges in the build function (i.e. statically) */ -template -class BGLSolverBaseSVSE : public BGLSolverBaseSVDE +template +class BGLSolverBaseSVSE : public BGLSolverBaseSVDE { public: - using BGLSolverBaseSVDE::BGLSolverBaseSVDE; + using BGLSolverBaseSVDE::BGLSolverBaseSVDE; BuildStatus buildImpl(const std::vector::ConstPtr>& trajectory, const std::vector::ConstPtr>& edge_eval, diff --git a/descartes_light/solvers/include/descartes_light/solvers/bgl/event_visitor.h b/descartes_light/solvers/include/descartes_light/solvers/bgl/event_visitor.h deleted file mode 100644 index 428cb4ca..00000000 --- a/descartes_light/solvers/include/descartes_light/solvers/bgl/event_visitor.h +++ /dev/null @@ -1,4 +0,0 @@ -#ifndef EVENT_VISITOR_H -#define EVENT_VISITOR_H - -#endif // EVENT_VISITOR_H diff --git a/descartes_light/solvers/include/descartes_light/solvers/bgl/event_visitors.h b/descartes_light/solvers/include/descartes_light/solvers/bgl/event_visitors.h new file mode 100644 index 00000000..a6376546 --- /dev/null +++ b/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 + +#include +DESCARTES_IGNORE_WARNINGS_PUSH +#include +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 +struct early_terminator : public boost::base_visitor> +{ + /** @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 + void operator()(VertexDesc u, const BGLGraph& 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 +struct add_all_edges_dynamically : public boost::base_visitor> +{ + /** @brief Event filter typedef defining the events for which this visitor can be used */ + typedef EventType event_filter; + + add_all_edges_dynamically(std::vector::ConstPtr> edge_eval, + std::vector>> ladder_rungs); + + void operator()(VertexDesc u, const BGLGraph& g); + + const std::vector::ConstPtr> eval_; + const std::vector>> ladder_rungs_; +}; + +/** + * @brief Event visitor for updating vertex cost + */ +struct cost_recorder : public boost::base_visitor +{ + /** @brief Event filter typedef defining the events for which this visitor can be used */ + typedef boost::on_tree_edge event_filter; + + template + void operator()(EdgeDesc e, const BGLGraph& g); +}; + +} // namespace descartes_light + +#endif // DESCARTES_LIGHT_SOLVERS_BGL_EVENT_VISITORS_H diff --git a/descartes_light/solvers/include/descartes_light/solvers/bgl/impl/bgl_dijkstra_solver.hpp b/descartes_light/solvers/include/descartes_light/solvers/bgl/impl/bgl_dijkstra_solver.hpp index 5ba16145..a655e006 100644 --- a/descartes_light/solvers/include/descartes_light/solvers/bgl/impl/bgl_dijkstra_solver.hpp +++ b/descartes_light/solvers/include/descartes_light/solvers/bgl/impl/bgl_dijkstra_solver.hpp @@ -11,87 +11,32 @@ DESCARTES_IGNORE_WARNINGS_POP namespace descartes_light { -template -SearchResult BGLDijkstraSVSESolver::search() +template +static VertexDesc solveDijkstra(BGLGraph& graph, + std::vector>& predecessors, + const VertexDesc& source, + const Visitors& event_visitors, + const std::vector>>& ladder_rungs) { - // Convenience aliases - auto& graph_ = BGLSolverBase::graph_; - const auto& source_ = BGLSolverBase::source_; - auto& predecessors_ = BGLSolverBase::predecessors_; - const auto& ladder_rungs_ = BGLSolverBase::ladder_rungs_; - // Internal properties - auto index_prop_map = boost::get(boost::vertex_index, graph_); - auto weight_prop_map = boost::get(boost::edge_weight, graph_); - auto color_prop_map = boost::get(&Vertex::color, graph_); - auto distance_prop_map = boost::get(&Vertex::distance, graph_); - - typedef typename boost::property_map, boost::vertex_index_t>::type IndexMap; - typedef boost::iterator_property_map>::iterator, IndexMap> PredecessorMap; - predecessors_.resize(boost::num_vertices(graph_), std::numeric_limits::max()); - PredecessorMap predecessor_it_map = boost::make_iterator_property_map(predecessors_.begin(), index_prop_map); - - // Perform the search - boost::dijkstra_shortest_paths(graph_, - source_, - predecessor_it_map, - distance_prop_map, - weight_prop_map, - index_prop_map, - std::less<>(), - std::plus<>(), - std::numeric_limits::max(), - static_cast(0.0), - boost::default_dijkstra_visitor(), - color_prop_map); - - // Find lowest cost node in last rung - auto target = std::min_element(ladder_rungs_.back().begin(), - ladder_rungs_.back().end(), - [&](const VertexDesc& a, const VertexDesc& b) { - return graph_[a].distance < graph_[b].distance; - }); - - SearchResult result; - - // Reconstruct the path from the predecesor map; remove the artificial start state - const auto vd_path = BGLSolverBase::reconstructPath(source_, *target); - result.trajectory = BGLSolverBase::toStates(vd_path); - result.trajectory.erase(result.trajectory.begin()); - - result.cost = graph_[*target].distance; - - return result; -} - -template -SearchResult BGLEfficientDijkstraSVSESolver::search() -{ - // Convenience aliases - auto& graph_ = BGLSolverBase::graph_; - const auto& source_ = BGLSolverBase::source_; - auto& predecessors_ = BGLSolverBase::predecessors_; - const auto& ladder_rungs_ = BGLSolverBase::ladder_rungs_; + auto index_prop_map = boost::get(boost::vertex_index, graph); + auto weight_prop_map = boost::get(boost::edge_weight, graph); + auto color_prop_map = boost::get(&Vertex::color, graph); + auto distance_prop_map = boost::get(&Vertex::distance, graph); - // Internal properties - auto index_prop_map = boost::get(boost::vertex_index, graph_); - auto weight_prop_map = boost::get(boost::edge_weight, graph_); - auto color_prop_map = boost::get(&Vertex::color, graph_); - auto distance_prop_map = boost::get(&Vertex::distance, graph_); + predecessors.resize(boost::num_vertices(graph), std::numeric_limits::max()); typedef typename boost::property_map, boost::vertex_index_t>::type IndexMap; typedef boost::iterator_property_map>::iterator, IndexMap> PredecessorMap; - predecessors_.resize(boost::num_vertices(graph_), std::numeric_limits::max()); - PredecessorMap predecessor_it_map = boost::make_iterator_property_map(predecessors_.begin(), index_prop_map); + PredecessorMap predecessor_it_map = boost::make_iterator_property_map(predecessors.begin(), index_prop_map); - const long last_rung_idx = static_cast(ladder_rungs_.size() - 1); - auto visitor = boost::make_dijkstra_visitor(early_terminator(last_rung_idx)); + auto visitor = boost::make_dijkstra_visitor(event_visitors); // Perform the search try { - boost::dijkstra_shortest_paths(graph_, - source_, + boost::dijkstra_shortest_paths(graph, + source, predecessor_it_map, distance_prop_map, weight_prop_map, @@ -102,140 +47,80 @@ SearchResult BGLEfficientDijkstraSVSESolver::search() static_cast(0.0), visitor, color_prop_map); + + // 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& a, const VertexDesc& 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::max()) + throw *target; } catch (const VertexDesc& target) { - SearchResult result; - - // Reconstruct the path from the predecesor map; remove the artificial start state - const auto vd_path = BGLSolverBase::reconstructPath(source_, target); - result.trajectory = BGLSolverBase::toStates(vd_path); - result.trajectory.erase(result.trajectory.begin()); - - result.cost = graph_[target].distance; - - return result; + return target; } // If the visitor never threw the vertex descriptor, there was an issue with the search throw std::runtime_error("Search failed to encounter vertex associated with the last waypoint in the trajectory"); } -template -SearchResult BGLDijkstraSVDESolver::search() +template +SearchResult BGLDijkstraSVSESolver::search() { // Convenience aliases - auto& graph_ = BGLSolverBase::graph_; - const auto& source_ = BGLSolverBase::source_; - auto& predecessors_ = BGLSolverBase::predecessors_; - auto& ladder_rungs_ = BGLSolverBase::ladder_rungs_; - auto& edge_eval_ = BGLSolverBaseSVDE::edge_eval_; + auto& graph_ = BGLSolverBase::graph_; + const auto& source_ = BGLSolverBase::source_; + auto& predecessors_ = BGLSolverBase::predecessors_; + const auto& ladder_rungs_ = BGLSolverBase::ladder_rungs_; + const auto& event_visitors_ = BGLSolverBase::event_visitors_; - // Internal properties - auto index_prop_map = boost::get(boost::vertex_index, graph_); - auto weight_prop_map = boost::get(boost::edge_weight, graph_); - auto color_prop_map = boost::get(&Vertex::color, graph_); - auto distance_prop_map = boost::get(&Vertex::distance, graph_); - - typedef typename boost::property_map, boost::vertex_index_t>::type IndexMap; - typedef boost::iterator_property_map>::iterator, IndexMap> PredecessorMap; - predecessors_.resize(boost::num_vertices(graph_), std::numeric_limits::max()); - PredecessorMap predecessor_it_map = boost::make_iterator_property_map(predecessors_.begin(), index_prop_map); - - auto visitor = boost::make_dijkstra_visitor( - add_all_edges_dynamically(edge_eval_, ladder_rungs_)); - - // Perform the search - boost::dijkstra_shortest_paths(graph_, - source_, - predecessor_it_map, - distance_prop_map, - weight_prop_map, - index_prop_map, - std::less<>(), - std::plus<>(), - std::numeric_limits::max(), - static_cast(0.0), - visitor, - color_prop_map); - - // Find lowest cost node in last rung - auto target = std::min_element(ladder_rungs_.back().begin(), - ladder_rungs_.back().end(), - [&](const VertexDesc& a, const VertexDesc& b) { - return graph_[a].distance < graph_[b].distance; - }); + VertexDesc target = + solveDijkstra(graph_, predecessors_, source_, event_visitors_, ladder_rungs_); SearchResult result; // Reconstruct the path from the predecesor map; remove the artificial start state - const auto vd_path = BGLSolverBase::reconstructPath(source_, *target); - result.trajectory = BGLSolverBase::toStates(vd_path); + const auto vd_path = BGLSolverBase::reconstructPath(source_, target); + result.trajectory = BGLSolverBase::toStates(vd_path); result.trajectory.erase(result.trajectory.begin()); - result.cost = graph_[*target].distance; + result.cost = graph_[target].distance; return result; } -template -SearchResult BGLEfficientDijkstraSVDESolver::search() +template +SearchResult BGLDijkstraSVDESolver::search() { // Convenience aliases - auto& graph_ = BGLSolverBase::graph_; - const auto& source_ = BGLSolverBase::source_; - auto& predecessors_ = BGLSolverBase::predecessors_; - auto& ladder_rungs_ = BGLSolverBase::ladder_rungs_; - auto& edge_eval_ = BGLSolverBaseSVDE::edge_eval_; - - // Internal properties - auto index_prop_map = boost::get(boost::vertex_index, graph_); - auto weight_prop_map = boost::get(boost::edge_weight, graph_); - auto color_prop_map = boost::get(&Vertex::color, graph_); - auto distance_prop_map = boost::get(&Vertex::distance, graph_); + auto& graph_ = BGLSolverBase::graph_; + const auto& source_ = BGLSolverBase::source_; + auto& predecessors_ = BGLSolverBase::predecessors_; + const auto& ladder_rungs_ = BGLSolverBase::ladder_rungs_; + const auto& event_visitors_ = BGLSolverBase::event_visitors_; - typedef typename boost::property_map, boost::vertex_index_t>::type IndexMap; - typedef boost::iterator_property_map>::iterator, IndexMap> PredecessorMap; - predecessors_.resize(boost::num_vertices(graph_), std::numeric_limits::max()); - PredecessorMap predecessor_it_map = boost::make_iterator_property_map(predecessors_.begin(), index_prop_map); + // Create the dynamic edge adding event visitor + const auto& edge_eval_ = BGLSolverBaseSVDE::edge_eval_; + auto vis = std::make_pair(add_all_edges_dynamically(edge_eval_, ladder_rungs_), + event_visitors_); - const long last_rung_idx = static_cast(ladder_rungs_.size() - 1); - auto visitor = boost::make_dijkstra_visitor( - std::make_pair(early_terminator(last_rung_idx), - add_all_edges_dynamically(edge_eval_, ladder_rungs_))); + VertexDesc target = solveDijkstra(graph_, predecessors_, source_, vis, ladder_rungs_); - // Perform the search - try - { - boost::dijkstra_shortest_paths(graph_, - source_, - predecessor_it_map, - distance_prop_map, - weight_prop_map, - index_prop_map, - std::less<>(), - std::plus<>(), - std::numeric_limits::max(), - static_cast(0.0), - visitor, - color_prop_map); - } - catch (const VertexDesc& target) - { - SearchResult result; - - // Reconstruct the path from the predecesor map; remove the artificial start state - const auto vd_path = BGLSolverBase::reconstructPath(source_, target); - result.trajectory = BGLSolverBase::toStates(vd_path); - result.trajectory.erase(result.trajectory.begin()); + SearchResult result; - result.cost = graph_[target].distance; + // Reconstruct the path from the predecesor map; remove the artificial start state + const auto vd_path = BGLSolverBase::reconstructPath(source_, target); + result.trajectory = BGLSolverBase::toStates(vd_path); + result.trajectory.erase(result.trajectory.begin()); - return result; - } + result.cost = graph_[target].distance; - // If the visitor never threw the vertex descriptor, there was an issue with the search - throw std::runtime_error("Search failed to encounter vertex associated with the last waypoint in the trajectory"); + return result; } } // namespace descartes_light diff --git a/descartes_light/solvers/include/descartes_light/solvers/bgl/impl/bgl_solver.hpp b/descartes_light/solvers/include/descartes_light/solvers/bgl/impl/bgl_solver.hpp index 7834ef23..0081ca82 100644 --- a/descartes_light/solvers/include/descartes_light/solvers/bgl/impl/bgl_solver.hpp +++ b/descartes_light/solvers/include/descartes_light/solvers/bgl/impl/bgl_solver.hpp @@ -64,12 +64,16 @@ static void reportFailedVertices(const std::vector& indices) namespace descartes_light { -template -BGLSolverBase::BGLSolverBase(unsigned num_threads) : num_threads_{ num_threads } {}; +template +BGLSolverBase::BGLSolverBase(Visitors event_visitors, unsigned num_threads) + : event_visitors_(std::move(event_visitors)), num_threads_{ num_threads } +{ +} -template -std::vector> BGLSolverBase::reconstructPath(const VertexDesc& source, - const VertexDesc& target) const +template +std::vector> +BGLSolverBase::reconstructPath(const VertexDesc& source, + const VertexDesc& target) const { // Reconstruct the path from predecessors std::vector> path; @@ -90,9 +94,9 @@ std::vector> BGLSolverBase::reconstructPath(con return path; } -template +template std::vector::ConstPtr> -BGLSolverBase::toStates(const std::vector>& path) const +BGLSolverBase::toStates(const std::vector>& path) const { // Get the state information from the graph using the vertex descriptors std::vector::ConstPtr> out; @@ -104,8 +108,8 @@ BGLSolverBase::toStates(const std::vector>& pat return out; } -template -void BGLSolverBase::writeGraphWithPath(const std::string& filename) const +template +void BGLSolverBase::writeGraphWithPath(const std::string& filename) const { std::ofstream file(filename); if (!file.good()) @@ -130,16 +134,16 @@ void BGLSolverBase::writeGraphWithPath(const std::string& filename) c boost::write_graphviz(file, sg); } -template -BuildStatus BGLSolverBaseSVDE::buildImpl( +template +BuildStatus BGLSolverBaseSVDE::buildImpl( const std::vector::ConstPtr>& trajectory, const std::vector::ConstPtr>& edge_evaluators, const std::vector::ConstPtr>& state_evaluators) { // Convenience aliases - auto& ladder_rungs_ = BGLSolverBase::ladder_rungs_; - auto& graph_ = BGLSolverBase::graph_; - auto& source_ = BGLSolverBase::source_; + auto& ladder_rungs_ = BGLSolverBase::ladder_rungs_; + auto& graph_ = BGLSolverBase::graph_; + auto& source_ = BGLSolverBase::source_; BuildStatus status; edge_eval_ = std::move(edge_evaluators); @@ -149,7 +153,7 @@ BuildStatus BGLSolverBaseSVDE::buildImpl( long cnt = 0; std::chrono::time_point start_time = Clock::now(); -#pragma omp parallel for num_threads(BGLSolverBase ::num_threads_) +#pragma omp parallel for num_threads(BGLSolverBase ::num_threads_) for (long i = 0; i < static_cast(trajectory.size()); ++i) { std::vector> samples = trajectory[static_cast(i)]->sample(); @@ -219,23 +223,23 @@ BuildStatus BGLSolverBaseSVDE::buildImpl( return status; } -template -BuildStatus BGLSolverBaseSVSE::buildImpl( +template +BuildStatus BGLSolverBaseSVSE::buildImpl( const std::vector::ConstPtr>& trajectory, const std::vector::ConstPtr>& edge_evaluators, const std::vector::ConstPtr>& state_evaluators) { // Convenience aliases - auto& ladder_rungs_ = BGLSolverBase::ladder_rungs_; - auto& graph_ = BGLSolverBase::graph_; + auto& ladder_rungs_ = BGLSolverBase::ladder_rungs_; + auto& graph_ = BGLSolverBase::graph_; // Use the base class to build the vertices - BuildStatus status = BGLSolverBaseSVDE::buildImpl(trajectory, edge_evaluators, state_evaluators); + BuildStatus status = BGLSolverBaseSVDE::buildImpl(trajectory, edge_evaluators, state_evaluators); // Build Edges long cnt = 0; auto start_time = Clock::now(); -#pragma omp parallel for num_threads(BGLSolverBase ::num_threads_) +#pragma omp parallel for num_threads(BGLSolverBase ::num_threads_) for (long i = 1; i < static_cast(trajectory.size()); ++i) { auto& from = ladder_rungs_[static_cast(i) - 1]; diff --git a/descartes_light/solvers/include/descartes_light/solvers/bgl/impl/event_visitors.hpp b/descartes_light/solvers/include/descartes_light/solvers/bgl/impl/event_visitors.hpp index 3a72da87..6c6fa69b 100644 --- a/descartes_light/solvers/include/descartes_light/solvers/bgl/impl/event_visitors.hpp +++ b/descartes_light/solvers/include/descartes_light/solvers/bgl/impl/event_visitors.hpp @@ -1,105 +1,71 @@ #ifndef DESCARTES_LIGHT_SOLVERS_BGL_IMPL_EVENT_VISITORS_HPP #define DESCARTES_LIGHT_SOLVERS_BGL_IMPL_EVENT_VISITORS_HPP -#include - -#include -DESCARTES_IGNORE_WARNINGS_PUSH -#include -DESCARTES_IGNORE_WARNINGS_POP +#include 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 -struct early_terminator : public boost::base_visitor> +template +early_terminator::early_terminator(long last_rung_idx) : last_rung_idx_(last_rung_idx) { - /** @brief Event filter typedef defining the events for which this visitor can be used */ - typedef EventType event_filter; - - early_terminator(long last_rung_idx) : last_rung_idx_(last_rung_idx) {} +} - void operator()(VertexDesc u, const BGLGraph& g) - { - if (g[u].rung_idx == last_rung_idx_) - throw u; - } - - const long last_rung_idx_; -}; +template +template +void early_terminator::operator()(VertexDesc u, const BGLGraph& g) +{ + if (g[u].rung_idx == last_rung_idx_) + throw u; +} -/** - * @brief Event visitor that adds all edges to each vertex dynamically as the vertex is discovered during the graph - * search - */ template -struct add_all_edges_dynamically : public boost::base_visitor> +add_all_edges_dynamically::add_all_edges_dynamically( + std::vector::ConstPtr> edge_eval, + std::vector>> ladder_rungs) + : eval_(std::move(edge_eval)), ladder_rungs_(std::move(ladder_rungs)) { - /** @brief Event filter typedef defining the events for which this visitor can be used */ - typedef EventType event_filter; +} - add_all_edges_dynamically(std::vector::ConstPtr> edge_eval, - std::vector>> ladder_rungs) - : eval_(std::move(edge_eval)), ladder_rungs_(std::move(ladder_rungs)) - { - } - - void operator()(VertexDesc u, const BGLGraph& g) +template +void add_all_edges_dynamically::operator()(VertexDesc u, const BGLGraph& g) +{ + auto out_deg = boost::out_degree(u, g); + // return if the vertex has any out edges + if (out_deg == 0) { - auto out_deg = boost::out_degree(u, g); - // return if the vertex has any out edges - if (out_deg == 0) + std::size_t current_rung = static_cast(g[u].rung_idx); + std::size_t next_rung = static_cast(current_rung + 1); + if (next_rung < ladder_rungs_.size()) { - std::size_t current_rung = static_cast(g[u].rung_idx); - std::size_t next_rung = static_cast(current_rung + 1); - if (next_rung < ladder_rungs_.size()) + for (std::size_t s = 0; s < ladder_rungs_[next_rung].size(); ++s) { - for (std::size_t s = 0; s < ladder_rungs_[next_rung].size(); ++s) + std::pair results = + eval_[current_rung]->evaluate(*g[u].sample.state, *g[ladder_rungs_[next_rung][s]].sample.state); + if (results.first) { - std::pair results = eval_[static_cast(current_rung)]->evaluate( - *g[u].sample.state, *g[ladder_rungs_[next_rung][s]].sample.state); - if (results.first) - { - FloatType cost = results.second + g[ladder_rungs_[next_rung][s]].sample.cost; - if (current_rung == 0) - cost += g[u].sample.cost; - VertexDesc target_vert = ladder_rungs_[next_rung][s]; - BGLGraph* mutable_graph_ = const_cast*>(&g); - boost::add_edge(u, target_vert, cost, *mutable_graph_); - } + FloatType cost = results.second + g[ladder_rungs_[next_rung][s]].sample.cost; + if (current_rung == 0) + cost += g[u].sample.cost; + VertexDesc target_vert = ladder_rungs_[next_rung][s]; + BGLGraph* mutable_graph_ = const_cast*>(&g); + boost::add_edge(u, target_vert, cost, *mutable_graph_); } } } } +} - const std::vector::ConstPtr> eval_; - const std::vector>> ladder_rungs_; -}; - -/** - * @brief Event visitor for updating vertex cost - */ template -struct cost_recorder : public boost::base_visitor> +void cost_recorder::operator()(EdgeDesc e, const BGLGraph& g) { - /** @brief Event filter typedef defining the events for which this visitor can be used */ - typedef boost::on_tree_edge event_filter; - - void operator()(EdgeDesc e, const BGLGraph& g) - { - VertexDesc target = boost::target(e, g); - VertexDesc source = boost::source(e, g); - auto edge_weight_map = boost::get(boost::edge_weight, g); + VertexDesc target = boost::target(e, g); + VertexDesc source = boost::source(e, g); + auto edge_weight_map = boost::get(boost::edge_weight, g); - BGLGraph* mutable_graph_ = const_cast*>(&g); + BGLGraph* mutable_graph_ = const_cast*>(&g); - mutable_graph_->operator[](target).distance = g[source].distance + g[target].sample.cost + edge_weight_map[e]; - } -}; + mutable_graph_->operator[](target).distance = g[source].distance + g[target].sample.cost + edge_weight_map[e]; +} } // namespace descartes_light diff --git a/descartes_light/solvers/src/bgl/bgl_solver.cpp b/descartes_light/solvers/src/bgl/bgl_solver.cpp index e04e513a..0c3a4963 100644 --- a/descartes_light/solvers/src/bgl/bgl_solver.cpp +++ b/descartes_light/solvers/src/bgl/bgl_solver.cpp @@ -19,31 +19,53 @@ #include #include +#include +DESCARTES_IGNORE_WARNINGS_PUSH +#include +#include +DESCARTES_IGNORE_WARNINGS_POP + +// Macro for explicitly instantiating a class with a variable number of template arguments +#define INSTANTIATE(T, ...) template class T<__VA_ARGS__>; + +// Implementation macro for explicitly instantiating a class for a specific element of the Cartesian product +// (class_name, (template params)) +#define INSTANTIATE_PRODUCT_IMPL(r, product) \ + INSTANTIATE(BOOST_PP_SEQ_HEAD(product), BOOST_PP_SEQ_ENUM(BOOST_PP_SEQ_TAIL(product))) + +/** + * Macro for instantiating a template class for a Cartesian product of float types and event visitor types + * For example, instantiation of class Foo with types ((float)(double)) and visitors ((A)(B)) would result in 4 + * instantiations: + * - Foo + * - Foo + * - Foo + * - Foo + */ +#define INSTANTIATE_PRODUCT(TEMPLATE, FLOAT_TYPES, EVENT_VISITORS) \ + BOOST_PP_SEQ_FOR_EACH_PRODUCT(INSTANTIATE_PRODUCT_IMPL, ((TEMPLATE))(FLOAT_TYPES)(EVENT_VISITORS)) + namespace descartes_light { -// Explicit template instantiation -// Partial implementations -template class BGLSolverBase; -template class BGLSolverBase; - -template class BGLSolverBaseSVDE; -template class BGLSolverBaseSVDE; +// Event visitors +template struct early_terminator; -template class BGLSolverBaseSVSE; -template class BGLSolverBaseSVSE; +template struct add_all_edges_dynamically; +template struct add_all_edges_dynamically; -// Full Implementations -template class BGLDijkstraSVSESolver; -template class BGLDijkstraSVSESolver; +// Explicit template instantiation +#define FLOAT_TYPES (double)(float) +#define DIJKSTRA_EVENT_VISITORS (boost::null_visitor)(early_terminator) -template class BGLEfficientDijkstraSVSESolver; -template class BGLEfficientDijkstraSVSESolver; +// Partial implementations +INSTANTIATE_PRODUCT(BGLSolverBase, FLOAT_TYPES, DIJKSTRA_EVENT_VISITORS) +INSTANTIATE_PRODUCT(BGLSolverBaseSVSE, FLOAT_TYPES, DIJKSTRA_EVENT_VISITORS) +INSTANTIATE_PRODUCT(BGLSolverBaseSVDE, FLOAT_TYPES, DIJKSTRA_EVENT_VISITORS) -template class BGLDijkstraSVDESolver; -template class BGLDijkstraSVDESolver; +// BGL Dijkstra search +INSTANTIATE_PRODUCT(BGLDijkstraSVSESolver, FLOAT_TYPES, DIJKSTRA_EVENT_VISITORS) +INSTANTIATE_PRODUCT(BGLDijkstraSVDESolver, FLOAT_TYPES, DIJKSTRA_EVENT_VISITORS) -template class BGLEfficientDijkstraSVDESolver; -template class BGLEfficientDijkstraSVDESolver; // Free functions template SubGraph createDecoratedSubGraph(const BGLGraph& g); template SubGraph createDecoratedSubGraph(const BGLGraph& g); diff --git a/descartes_light/test/benchmarks/benchmarks.cpp b/descartes_light/test/benchmarks/benchmarks.cpp index ae7241a0..78477f8b 100644 --- a/descartes_light/test/benchmarks/benchmarks.cpp +++ b/descartes_light/test/benchmarks/benchmarks.cpp @@ -51,7 +51,7 @@ void benchmark(const SolverFactory& factory) createSamplers(dof, n_waypoints, samples_per_wp, state_cost); // Create the solver - typename descartes_light::Solver::Ptr solver = factory.create(); + typename descartes_light::Solver::Ptr solver = factory.create(static_cast(n_waypoints)); // Build the graph auto start_time = Clock::now(); @@ -80,13 +80,21 @@ int main(int, char**) benchmark(SolverFactory()); benchmark(SolverFactory()); - // BGL full Dijkstra solver + // BGL Dijkstra full search, static vertex static edge + benchmark(SolverFactory>()); + benchmark(SolverFactory>()); + + // BGL Dijkstra early termination, static vertex static edge benchmark(SolverFactory()); benchmark(SolverFactory()); - // BGL efficient Dijkstra solver - benchmark(SolverFactory()); - benchmark(SolverFactory()); + // BGL Dijkstra full search, static vertex dynamic edge + benchmark(SolverFactory>()); + benchmark(SolverFactory>()); + + // BGL Dijkstra early termination, static vertex, dynamic edge + benchmark(SolverFactory()); + benchmark(SolverFactory()); return 0; } diff --git a/descartes_light/test/include/descartes_light/test/impl/solver_factory.hpp b/descartes_light/test/include/descartes_light/test/impl/solver_factory.hpp index b51c87df..0e6dd93f 100644 --- a/descartes_light/test/include/descartes_light/test/impl/solver_factory.hpp +++ b/descartes_light/test/include/descartes_light/test/impl/solver_factory.hpp @@ -10,40 +10,50 @@ namespace descartes_light template struct SolverFactory> { - typename Solver::Ptr create() const { return std::make_shared>(1); } + typename Solver::Ptr create(long) const { return std::make_shared>(1); } }; // Boost full Dijkstra graph solver factory template -struct SolverFactory> +struct SolverFactory> { - typename Solver::Ptr create() const { return std::make_shared>(1); } + using Visitors = boost::null_visitor; + typename Solver::Ptr create(long) const + { + return std::make_shared>(Visitors(), 1); + } }; // Boost efficient Dijkstra graph solver factory template -struct SolverFactory> +struct SolverFactory>> { - typename Solver::Ptr create() const + using Visitors = early_terminator; + typename Solver::Ptr create(long n_waypoints) const { - return std::make_shared>(1); + return std::make_shared>(Visitors(n_waypoints - 1), 1); } }; // Dynamic Boost full Dijkstra graph solver factory template -struct SolverFactory> +struct SolverFactory> { - typename Solver::Ptr create() const { return std::make_shared>(1); } + using Visitors = boost::null_visitor; + typename Solver::Ptr create(long) const + { + return std::make_shared>(Visitors(), 1); + } }; // Dynamic Boost efficient Dijkstra graph solver factory template -struct SolverFactory> +struct SolverFactory>> { - typename Solver::Ptr create() const + using Visitors = early_terminator; + typename Solver::Ptr create(long n_waypoints) const { - return std::make_shared>(1); + return std::make_shared>(Visitors(n_waypoints - 1), 1); } }; diff --git a/descartes_light/test/include/descartes_light/test/solver_factory.h b/descartes_light/test/include/descartes_light/test/solver_factory.h index 7f1949ec..77f1dec2 100644 --- a/descartes_light/test/include/descartes_light/test/solver_factory.h +++ b/descartes_light/test/include/descartes_light/test/solver_factory.h @@ -11,7 +11,7 @@ template struct SolverFactory { using FloatType = typename SolverT::FloatT; - typename Solver::Ptr create() const; + typename Solver::Ptr create(long n_waypoints) const; }; } // namespace descartes_light diff --git a/descartes_light/test/src/solver_factory.cpp b/descartes_light/test/src/solver_factory.cpp index 2df01857..81beb012 100644 --- a/descartes_light/test/src/solver_factory.cpp +++ b/descartes_light/test/src/solver_factory.cpp @@ -4,12 +4,17 @@ namespace descartes_light { template struct SolverFactory; template struct SolverFactory; +// Naive BGL Dijkstra search, static vertex static edge +template struct SolverFactory>; +template struct SolverFactory>; +// BGL Dijkstra search with early termination, static vertex static edge template struct SolverFactory; template struct SolverFactory; -template struct SolverFactory; -template struct SolverFactory; +// Naive BGL Dijkstra search, static vertex dynamic edge +template struct SolverFactory>; +template struct SolverFactory>; +// BGL Dijkstra searhc with early termination, static vertex dynamic edge template struct SolverFactory; template struct SolverFactory; -template struct SolverFactory; -template struct SolverFactory; + } // namespace descartes_light diff --git a/descartes_light/test/utest.cpp b/descartes_light/test/utest.cpp index b3a07221..27c15436 100644 --- a/descartes_light/test/utest.cpp +++ b/descartes_light/test/utest.cpp @@ -41,21 +41,21 @@ class SolverFixture : public ::testing::Test using Implementations = ::testing::Types, SolverFactory, + SolverFactory>, + SolverFactory>, SolverFactory, SolverFactory, - SolverFactory, - SolverFactory, + SolverFactory>, + SolverFactory>, SolverFactory, - SolverFactory, - SolverFactory, - SolverFactory>; + SolverFactory>; TYPED_TEST_CASE(SolverFixture, Implementations); TYPED_TEST(SolverFixture, KnownPathTest) { using FloatType = typename TypeParam::FloatType; - typename Solver::Ptr solver = this->Factory.create(); + typename Solver::Ptr solver = this->Factory.create(static_cast(this->n_waypoints)); // Build a graph where one sample for each waypoint is an all zero state; evaluate edges using the Euclidean distance // metric Since each waypoint has an all-zero state, the shortest path should be through these samples