Skip to content

Commit 228dfc0

Browse files
colin-lewis-19ctlewismarip8
authored
Dynamic Edge Adding Visitor (#76)
* Add event visitor which adds edges to all nodes in the subsequent rung from the current node. * Add static vertex dynamic edge (SVDE) implementations for Dijkstra & Efficient Dijkstra search * Removed NoEdge unit test for compatibility * restored edge evaluators to dynamic graph parent class * PR changes * Make event visitor members const Co-authored-by: ctlewis <colin.lewis@swri.org> Co-authored-by: Michael Ripperger <michael.ripperger@swri.org>
1 parent 44e0de3 commit 228dfc0

10 files changed

Lines changed: 266 additions & 28 deletions

File tree

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

Lines changed: 34 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -38,6 +38,40 @@ class BGLEfficientDijkstraSVSESolver : public BGLSolverBaseSVSE<FloatType>
3838
using BGLEfficientDijkstraSVSESolverF = BGLEfficientDijkstraSVSESolver<float>;
3939
using BGLEfficientDijkstraSVSESolverD = BGLEfficientDijkstraSVSESolver<double>;
4040

41+
/**
42+
* @brief BGL solver implementation that constructs vertices build function and uses Dijkstra's
43+
* algorithm with an edge-adding visitor to search the graph
44+
*/
45+
template <typename FloatType>
46+
class BGLDijkstraSVDESolver : public BGLSolverBaseSVDE<FloatType>
47+
{
48+
public:
49+
using BGLSolverBaseSVDE<FloatType>::BGLSolverBaseSVDE;
50+
51+
SearchResult<FloatType> search() override;
52+
};
53+
54+
using BGLDijkstraSVDESolverF = BGLDijkstraSVDESolver<float>;
55+
using BGLDijkstraSVDESolverD = BGLDijkstraSVDESolver<double>;
56+
57+
/**
58+
* @brief BGL solver implementation that constructs vertices in the build function and uses Dijkstra's
59+
* algorithm with a visitor that adds edges and terminates the search once a vertex in the last rung of
60+
* the graph is encountered rather than allowing it to continue until the distance to all nodes in the
61+
* graph has been calculated
62+
*/
63+
template <typename FloatType>
64+
class BGLEfficientDijkstraSVDESolver : public BGLSolverBaseSVDE<FloatType>
65+
{
66+
public:
67+
using BGLSolverBaseSVDE<FloatType>::BGLSolverBaseSVDE;
68+
69+
SearchResult<FloatType> search() override;
70+
};
71+
72+
using BGLEfficientDijkstraSVDESolverF = BGLEfficientDijkstraSVDESolver<float>;
73+
using BGLEfficientDijkstraSVDESolverD = BGLEfficientDijkstraSVDESolver<double>;
74+
4175
} // namespace descartes_light
4276

4377
#endif // DESCARTES_LIGHT_SOLVERS_BGL_BGL_DIJKSTRA_SOLVER_H

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

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -82,6 +82,9 @@ class BGLSolverBaseSVDE : public BGLSolverBase<FloatType>
8282
BuildStatus buildImpl(const std::vector<typename WaypointSampler<FloatType>::ConstPtr>& trajectory,
8383
const std::vector<typename EdgeEvaluator<FloatType>::ConstPtr>& edge_eval,
8484
const std::vector<typename StateEvaluator<FloatType>::ConstPtr>& state_eval) override;
85+
86+
protected:
87+
std::vector<typename EdgeEvaluator<FloatType>::ConstPtr> edge_eval_;
8588
};
8689

8790
/**

‎descartes_light/solvers/include/descartes_light/solvers/bgl/event_visitor.h‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,4 @@
1+
#ifndef EVENT_VISITOR_H
2+
#define EVENT_VISITOR_H
3+
4+
#endif // EVENT_VISITOR_H

‎descartes_light/solvers/include/descartes_light/solvers/bgl/impl/bgl_dijkstra_solver.hpp‎

Lines changed: 118 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -85,7 +85,124 @@ SearchResult<FloatType> BGLEfficientDijkstraSVSESolver<FloatType>::search()
8585
PredecessorMap predecessor_it_map = boost::make_iterator_property_map(predecessors_.begin(), index_prop_map);
8686

8787
const long last_rung_idx = static_cast<long>(ladder_rungs_.size() - 1);
88-
auto visitor = boost::make_dijkstra_visitor(early_terminator<FloatType>(last_rung_idx));
88+
auto visitor = boost::make_dijkstra_visitor(early_terminator<FloatType, boost::on_examine_vertex>(last_rung_idx));
89+
90+
// Perform the search
91+
try
92+
{
93+
boost::dijkstra_shortest_paths(graph_,
94+
source_,
95+
predecessor_it_map,
96+
distance_prop_map,
97+
weight_prop_map,
98+
index_prop_map,
99+
std::less<>(),
100+
std::plus<>(),
101+
std::numeric_limits<FloatType>::max(),
102+
static_cast<FloatType>(0.0),
103+
visitor,
104+
color_prop_map);
105+
}
106+
catch (const VertexDesc<FloatType>& target)
107+
{
108+
SearchResult<FloatType> result;
109+
110+
// Reconstruct the path from the predecesor map; remove the artificial start state
111+
const auto vd_path = BGLSolverBase<FloatType>::reconstructPath(source_, target);
112+
result.trajectory = BGLSolverBase<FloatType>::toStates(vd_path);
113+
result.trajectory.erase(result.trajectory.begin());
114+
115+
result.cost = graph_[target].distance;
116+
117+
return result;
118+
}
119+
120+
// If the visitor never threw the vertex descriptor, there was an issue with the search
121+
throw std::runtime_error("Search failed to encounter vertex associated with the last waypoint in the trajectory");
122+
}
123+
124+
template <typename FloatType>
125+
SearchResult<FloatType> BGLDijkstraSVDESolver<FloatType>::search()
126+
{
127+
// Convenience aliases
128+
auto& graph_ = BGLSolverBase<FloatType>::graph_;
129+
const auto& source_ = BGLSolverBase<FloatType>::source_;
130+
auto& predecessors_ = BGLSolverBase<FloatType>::predecessors_;
131+
auto& ladder_rungs_ = BGLSolverBase<FloatType>::ladder_rungs_;
132+
auto& edge_eval_ = BGLSolverBaseSVDE<FloatType>::edge_eval_;
133+
134+
// Internal properties
135+
auto index_prop_map = boost::get(boost::vertex_index, graph_);
136+
auto weight_prop_map = boost::get(boost::edge_weight, graph_);
137+
auto color_prop_map = boost::get(&Vertex<FloatType>::color, graph_);
138+
auto distance_prop_map = boost::get(&Vertex<FloatType>::distance, graph_);
139+
140+
typedef typename boost::property_map<BGLGraph<FloatType>, boost::vertex_index_t>::type IndexMap;
141+
typedef boost::iterator_property_map<typename std::vector<VertexDesc<FloatType>>::iterator, IndexMap> PredecessorMap;
142+
predecessors_.resize(boost::num_vertices(graph_), std::numeric_limits<std::size_t>::max());
143+
PredecessorMap predecessor_it_map = boost::make_iterator_property_map(predecessors_.begin(), index_prop_map);
144+
145+
auto visitor = boost::make_dijkstra_visitor(
146+
add_all_edges_dynamically<FloatType, boost::on_examine_vertex>(edge_eval_, ladder_rungs_));
147+
148+
// Perform the search
149+
boost::dijkstra_shortest_paths(graph_,
150+
source_,
151+
predecessor_it_map,
152+
distance_prop_map,
153+
weight_prop_map,
154+
index_prop_map,
155+
std::less<>(),
156+
std::plus<>(),
157+
std::numeric_limits<FloatType>::max(),
158+
static_cast<FloatType>(0.0),
159+
visitor,
160+
color_prop_map);
161+
162+
// Find lowest cost node in last rung
163+
auto target = std::min_element(ladder_rungs_.back().begin(),
164+
ladder_rungs_.back().end(),
165+
[&](const VertexDesc<FloatType>& a, const VertexDesc<FloatType>& b) {
166+
return graph_[a].distance < graph_[b].distance;
167+
});
168+
169+
SearchResult<FloatType> result;
170+
171+
// Reconstruct the path from the predecesor map; remove the artificial start state
172+
const auto vd_path = BGLSolverBase<FloatType>::reconstructPath(source_, *target);
173+
result.trajectory = BGLSolverBase<FloatType>::toStates(vd_path);
174+
result.trajectory.erase(result.trajectory.begin());
175+
176+
result.cost = graph_[*target].distance;
177+
178+
return result;
179+
}
180+
181+
template <typename FloatType>
182+
SearchResult<FloatType> BGLEfficientDijkstraSVDESolver<FloatType>::search()
183+
{
184+
// Convenience aliases
185+
auto& graph_ = BGLSolverBase<FloatType>::graph_;
186+
const auto& source_ = BGLSolverBase<FloatType>::source_;
187+
auto& predecessors_ = BGLSolverBase<FloatType>::predecessors_;
188+
auto& ladder_rungs_ = BGLSolverBase<FloatType>::ladder_rungs_;
189+
auto& edge_eval_ = BGLSolverBaseSVDE<FloatType>::edge_eval_;
190+
191+
// Internal properties
192+
auto index_prop_map = boost::get(boost::vertex_index, graph_);
193+
auto weight_prop_map = boost::get(boost::edge_weight, graph_);
194+
auto color_prop_map = boost::get(&Vertex<FloatType>::color, graph_);
195+
auto distance_prop_map = boost::get(&Vertex<FloatType>::distance, graph_);
196+
197+
typedef typename boost::property_map<BGLGraph<FloatType>, boost::vertex_index_t>::type IndexMap;
198+
typedef boost::iterator_property_map<typename std::vector<VertexDesc<FloatType>>::iterator, IndexMap> PredecessorMap;
199+
predecessors_.resize(boost::num_vertices(graph_), std::numeric_limits<std::size_t>::max());
200+
PredecessorMap predecessor_it_map = boost::make_iterator_property_map(predecessors_.begin(), index_prop_map);
201+
202+
const long last_rung_idx = static_cast<long>(ladder_rungs_.size() - 1);
203+
auto visitor = boost::make_dijkstra_visitor(
204+
std::make_pair(early_terminator<FloatType, boost::on_examine_vertex>(last_rung_idx),
205+
add_all_edges_dynamically<FloatType, boost::on_examine_vertex>(edge_eval_, ladder_rungs_)));
89206

90207
// Perform the search
91208
try

‎descartes_light/solvers/include/descartes_light/solvers/bgl/impl/bgl_solver.hpp‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -133,7 +133,7 @@ void BGLSolverBase<FloatType>::writeGraphWithPath(const std::string& filename) c
133133
template <typename FloatType>
134134
BuildStatus BGLSolverBaseSVDE<FloatType>::buildImpl(
135135
const std::vector<typename WaypointSampler<FloatType>::ConstPtr>& trajectory,
136-
const std::vector<typename EdgeEvaluator<FloatType>::ConstPtr>&,
136+
const std::vector<typename EdgeEvaluator<FloatType>::ConstPtr>& edge_evaluators,
137137
const std::vector<typename StateEvaluator<FloatType>::ConstPtr>& state_evaluators)
138138
{
139139
// Convenience aliases
@@ -142,6 +142,7 @@ BuildStatus BGLSolverBaseSVDE<FloatType>::buildImpl(
142142
auto& source_ = BGLSolverBase<FloatType>::source_;
143143

144144
BuildStatus status;
145+
edge_eval_ = std::move(edge_evaluators);
145146

146147
// Build Vertices
147148
ladder_rungs_.resize(trajectory.size());

‎descartes_light/solvers/include/descartes_light/solvers/bgl/impl/event_visitors.hpp‎

Lines changed: 74 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -11,15 +11,15 @@ DESCARTES_IGNORE_WARNINGS_POP
1111
namespace descartes_light
1212
{
1313
/**
14-
* @brief Event visitor that terminates the search when a vertex in the last rung of the graph is encountered
14+
* @brief Event visitor that terminates the search when a vertex in the last rung of the graph is examined
1515
* @details Throws the vertex descriptor that is the termination of the path once a vertex in the last rung of
16-
* the graph is encountered
16+
* the graph is operated on
1717
*/
18-
template <typename FloatType>
19-
struct early_terminator : public boost::base_visitor<early_terminator<FloatType>>
18+
template <typename FloatType, typename EventType>
19+
struct early_terminator : public boost::base_visitor<early_terminator<FloatType, EventType>>
2020
{
2121
/** @brief Event filter typedef defining the events for which this visitor can be used */
22-
typedef boost::on_examine_vertex event_filter;
22+
typedef EventType event_filter;
2323

2424
early_terminator(long last_rung_idx) : last_rung_idx_(last_rung_idx) {}
2525

@@ -32,6 +32,75 @@ struct early_terminator : public boost::base_visitor<early_terminator<FloatType>
3232
const long last_rung_idx_;
3333
};
3434

35+
/**
36+
* @brief Event visitor that adds all edges to each vertex dynamically as the vertex is discovered during the graph
37+
* search
38+
*/
39+
template <typename FloatType, typename EventType>
40+
struct add_all_edges_dynamically : public boost::base_visitor<add_all_edges_dynamically<FloatType, EventType>>
41+
{
42+
/** @brief Event filter typedef defining the events for which this visitor can be used */
43+
typedef EventType event_filter;
44+
45+
add_all_edges_dynamically(std::vector<typename EdgeEvaluator<FloatType>::ConstPtr> edge_eval,
46+
std::vector<std::vector<VertexDesc<FloatType>>> ladder_rungs)
47+
: eval_(std::move(edge_eval)), ladder_rungs_(std::move(ladder_rungs))
48+
{
49+
}
50+
51+
void operator()(VertexDesc<FloatType> u, const BGLGraph<FloatType>& g)
52+
{
53+
auto out_deg = boost::out_degree(u, g);
54+
// return if the vertex has any out edges
55+
if (out_deg == 0)
56+
{
57+
std::size_t current_rung = static_cast<std::size_t>(g[u].rung_idx);
58+
std::size_t next_rung = static_cast<std::size_t>(current_rung + 1);
59+
if (next_rung < ladder_rungs_.size())
60+
{
61+
for (std::size_t s = 0; s < ladder_rungs_[next_rung].size(); ++s)
62+
{
63+
std::pair<bool, FloatType> results = eval_[static_cast<size_t>(current_rung)]->evaluate(
64+
*g[u].sample.state, *g[ladder_rungs_[next_rung][s]].sample.state);
65+
if (results.first)
66+
{
67+
FloatType cost = results.second + g[ladder_rungs_[next_rung][s]].sample.cost;
68+
if (current_rung == 0)
69+
cost += g[u].sample.cost;
70+
VertexDesc<FloatType> target_vert = ladder_rungs_[next_rung][s];
71+
BGLGraph<FloatType>* mutable_graph_ = const_cast<BGLGraph<FloatType>*>(&g);
72+
boost::add_edge(u, target_vert, cost, *mutable_graph_);
73+
}
74+
}
75+
}
76+
}
77+
}
78+
79+
const std::vector<typename EdgeEvaluator<FloatType>::ConstPtr> eval_;
80+
const std::vector<std::vector<VertexDesc<FloatType>>> ladder_rungs_;
81+
};
82+
83+
/**
84+
* @brief Event visitor for updating vertex cost
85+
*/
86+
template <typename FloatType>
87+
struct cost_recorder : public boost::base_visitor<cost_recorder<FloatType>>
88+
{
89+
/** @brief Event filter typedef defining the events for which this visitor can be used */
90+
typedef boost::on_tree_edge event_filter;
91+
92+
void operator()(EdgeDesc<FloatType> e, const BGLGraph<FloatType>& g)
93+
{
94+
VertexDesc<FloatType> target = boost::target(e, g);
95+
VertexDesc<FloatType> source = boost::source(e, g);
96+
auto edge_weight_map = boost::get(boost::edge_weight, g);
97+
98+
BGLGraph<FloatType>* mutable_graph_ = const_cast<BGLGraph<FloatType>*>(&g);
99+
100+
mutable_graph_->operator[](target).distance = g[source].distance + g[target].sample.cost + edge_weight_map[e];
101+
}
102+
};
103+
35104
} // namespace descartes_light
36105

37106
#endif // DESCARTES_LIGHT_SOLVERS_BGL_IMPL_EVENT_VISITORS_HPP

‎descartes_light/solvers/src/bgl/bgl_solver.cpp‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -39,6 +39,11 @@ template class BGLDijkstraSVSESolver<float>;
3939
template class BGLEfficientDijkstraSVSESolver<double>;
4040
template class BGLEfficientDijkstraSVSESolver<float>;
4141

42+
template class BGLDijkstraSVDESolver<double>;
43+
template class BGLDijkstraSVDESolver<float>;
44+
45+
template class BGLEfficientDijkstraSVDESolver<double>;
46+
template class BGLEfficientDijkstraSVDESolver<float>;
4247
// Free functions
4348
template SubGraph<double> createDecoratedSubGraph(const BGLGraph<double>& g);
4449
template SubGraph<float> createDecoratedSubGraph(const BGLGraph<float>& g);

‎descartes_light/test/include/descartes_light/test/impl/solver_factory.hpp‎

Lines changed: 17 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -30,4 +30,21 @@ struct SolverFactory<BGLEfficientDijkstraSVSESolver<FloatType>>
3030
}
3131
};
3232

33+
// Dynamic Boost full Dijkstra graph solver factory
34+
template <typename FloatType>
35+
struct SolverFactory<BGLDijkstraSVDESolver<FloatType>>
36+
{
37+
typename Solver<FloatType>::Ptr create() const { return std::make_shared<BGLDijkstraSVDESolver<FloatType>>(1); }
38+
};
39+
40+
// Dynamic Boost efficient Dijkstra graph solver factory
41+
template <typename FloatType>
42+
struct SolverFactory<BGLEfficientDijkstraSVDESolver<FloatType>>
43+
{
44+
typename Solver<FloatType>::Ptr create() const
45+
{
46+
return std::make_shared<BGLEfficientDijkstraSVDESolver<FloatType>>(1);
47+
}
48+
};
49+
3350
} // namespace descartes_light

‎descartes_light/test/src/solver_factory.cpp‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -8,4 +8,8 @@ template struct SolverFactory<BGLDijkstraSVSESolverF>;
88
template struct SolverFactory<BGLDijkstraSVSESolverD>;
99
template struct SolverFactory<BGLEfficientDijkstraSVSESolverF>;
1010
template struct SolverFactory<BGLEfficientDijkstraSVSESolverD>;
11+
template struct SolverFactory<BGLDijkstraSVDESolverF>;
12+
template struct SolverFactory<BGLDijkstraSVDESolverD>;
13+
template struct SolverFactory<BGLEfficientDijkstraSVDESolverF>;
14+
template struct SolverFactory<BGLEfficientDijkstraSVDESolverD>;
1115
} // namespace descartes_light

‎descartes_light/test/utest.cpp‎

Lines changed: 5 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -44,30 +44,14 @@ using Implementations = ::testing::Types<SolverFactory<LadderGraphSolverF>,
4444
SolverFactory<BGLDijkstraSVSESolverF>,
4545
SolverFactory<BGLDijkstraSVSESolverD>,
4646
SolverFactory<BGLEfficientDijkstraSVSESolverF>,
47-
SolverFactory<BGLEfficientDijkstraSVSESolverD>>;
47+
SolverFactory<BGLEfficientDijkstraSVSESolverD>,
48+
SolverFactory<BGLDijkstraSVDESolverF>,
49+
SolverFactory<BGLDijkstraSVDESolverD>,
50+
SolverFactory<BGLEfficientDijkstraSVDESolverF>,
51+
SolverFactory<BGLEfficientDijkstraSVDESolverD>>;
4852

4953
TYPED_TEST_CASE(SolverFixture, Implementations);
5054

51-
TYPED_TEST(SolverFixture, NoEdges)
52-
{
53-
using FloatType = typename TypeParam::FloatType;
54-
typename Solver<FloatType>::Ptr solver = this->Factory.create();
55-
56-
auto edge_eval = std::make_shared<const NaiveEdgeEvaluator<FloatType>>(false);
57-
auto state_eval = std::make_shared<const NaiveStateEvaluator<FloatType>>(true, this->state_cost);
58-
59-
BuildStatus status = solver->build(this->samplers, { edge_eval }, { state_eval });
60-
ASSERT_FALSE(status);
61-
62-
std::vector<std::size_t> expected_failed_edges(this->n_waypoints - 1);
63-
std::iota(expected_failed_edges.begin(), expected_failed_edges.end(), 0);
64-
65-
ASSERT_TRUE(std::equal(status.failed_edges.begin(), status.failed_edges.end(), expected_failed_edges.begin()));
66-
ASSERT_EQ(status.failed_vertices.size(), 0);
67-
68-
ASSERT_THROW(solver->search(), std::runtime_error); // NOLINT
69-
}
70-
7155
TYPED_TEST(SolverFixture, KnownPathTest)
7256
{
7357
using FloatType = typename TypeParam::FloatType;

0 commit comments

Comments
 (0)