this is the continuation of another question I asked regarding boost graphs. (GraphML in Boost). After successfully reading the graph, I am trying to apply boost A* on some random start and goal nodes but its throwing segmentation fault.
Following are the details of my graph.
using Graph = boost::adjacency_list<boost::setS, boost::vecS, boost::undirectedS, VertexProperties, EdgeProperties>;
struct VertexProperties {
std::vector<double> joint_angles;
VertexProperties() : joint_angles(3){}
};
struct EdgeProperties {
double weight;
};
I used A* cities file from Boost as reference (A* Cities) to code my distance heuristic and astar_goal_visitor.
struct found_goal {}; // exception for termination
// visitor that terminates when we find the goal
template <typename Vertex>
class astar_goal_visitor : public boost::default_astar_visitor
{
public:
astar_goal_visitor(Vertex goal) : m_goal(goal) {}
template <class Graph>
void examine_vertex(Vertex u, Graph& g) {
if(u == m_goal)
throw found_goal();
}
private:
Vertex m_goal;
};
// euclidean distance heuristic
template <class Graph>
class distance_heuristic : public boost::astar_heuristic<typename Graph::Graph, double>
{
public:
typedef typename boost::graph_traits<typename Graph::Graph>::vertex_descriptor Vertex;
distance_heuristic(Vertex goal, Graph &graph)
: m_goal(goal), m_graph(graph) {}
double operator()(Vertex u)
{
double dx = m_graph.getGraph()[m_goal].joint_angles[0] - m_graph.getGraph()[u].joint_angles[0];
double dy = m_graph.getGraph()[m_goal].joint_angles[1] - m_graph.getGraph()[u].joint_angles[1];
double dz = m_graph.getGraph()[m_goal].joint_angles[2] - m_graph.getGraph()[u].joint_angles[2];
return ::sqrt(dx * dx + dy * dy + dz * dz);
}
private:
Graph m_graph;
Vertex m_goal;
};
As for astar_search parameters, the predecessor map is defined as below.
typedef boost::property_map < Graph, boost::vertex_index_t >::type IndexMap;
typedef boost::iterator_property_map < Vertex*, IndexMap, Vertex, Vertex& > PredecessorMap;
BoostGraph::PredecessorMap BoostGraph::getPredecessorMap(){
IndexMap indexMap = boost::get(boost::vertex_index, graph);
std::vector<Vertex> p(boost::num_vertices(graph));
PredecessorMap predecessorMap(&p[0], indexMap);
return predecessorMap;
}
The final code for search is
std::vector<double> d(boost::num_vertices(graph.getGraph()));
std::mt19937 gen(time(0));
BoostGraph::Vertex start = boost::random_vertex(graph.getGraph(), gen);
BoostGraph::Vertex goal = boost::random_vertex(graph.getGraph(), gen);
auto weightmap = boost::get(&EdgeProperties::weight, graph.getGraph());
try {
// call astar named parameter interface
boost::astar_search
(graph.getGraph(), start,
distance_heuristic<BoostGraph>
(goal, graph),
boost::predecessor_map(graph.getPredecessorMap()).distance_map(boost::make_iterator_property_map(d.begin(), boost::get(boost::vertex_index, graph.getGraph()))).
weight_map(weightmap).
visitor(astar_goal_visitor<BoostGraph::Vertex>(goal)));
} catch(found_goal fg) { // found a path to the goal
std::list<BoostGraph::Vertex> shortest_path;
for(BoostGraph::Vertex v = goal;; v = p[v]) {
shortest_path.push_front(v);
if(p[v] == v)
break;
}
}
The getGraph function of Class BoostGraph is defined below where graph is the protected member of class BoostGraph.
protected:
Graph graph;
const BoostGraph::Graph& BoostGraph::getGraph() const{
return graph;
}
The segmentation fault is coming in stl_tree.h and I have no idea what has gone wrong. Any help would be appreciated. Thanks
Your heuristic holds a copy of the graph. You're indexing using vertex descriptors belonging to different graphs.
Your predecessor map is a local variable (the vector), and the map is a dangling reference to it after getPredecessorMap returns. Just make the vector a member, and then getPredecessorMap can be eliminated, because it doesn't really add much.
Also, you're indexing into joint_angles without bounds checking. Prefer .at(n) over [n] if you want to be safer. In fact, consider using std::array<double, 3> instead of std::vector<double>.
All in all I get the impression that you've been trying to hide complexity in a class and member functions, however it leads to the code becoming fragmented and inviting lots of unnecessary lifetime issues.
There are also parts of the code you do not show. They likely hold more problems (e.g. getGraph() is crucial).
Here's my simplified take:
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/astar_search.hpp>
#include <boost/graph/random.hpp>
using JointAngles = std::vector<double>;
struct VertexProperties {
JointAngles joint_angles{0, 0, 0};
};
struct EdgeProperties {
double weight = 0;
};
using Graph = boost::adjacency_list<boost::setS, boost::vecS, boost::undirectedS,
VertexProperties, EdgeProperties>;
using Vertex = Graph::vertex_descriptor;
// visitor that terminates when we find the goal
struct goal_visitor : boost::default_astar_visitor {
struct found {}; // exception for termination
Vertex m_goal;
goal_visitor(Vertex g) : m_goal(g) {}
template <class Graph> void examine_vertex(Vertex u, Graph&) {
if (u == m_goal)
throw found{};
}
};
// euclidean distance heuristic
struct distance_heuristic : boost::astar_heuristic<Graph, double> {
distance_heuristic(Vertex goal, Graph& graph)
: m_graph(graph)
, m_goal(graph[goal].joint_angles) {}
double operator()(Vertex u) const {
auto& c = m_graph[u].joint_angles;
auto //
dx = m_goal.at(0) - c.at(0), //
dy = m_goal.at(1) - c.at(1), //
dz = m_goal.at(2) - c.at(2);
using std::sqrt; // allow ADL, good practice
return sqrt(dx * dx + dy * dy + dz * dz);
}
private:
Graph& m_graph; // reference!
JointAngles m_goal;
};
#include <random>
#include <fmt/ranges.h>
int main() {
Graph graph(4);
graph[0] = VertexProperties{{0, 0, 0}};
graph[1] = VertexProperties{{1, 1, 1}};
graph[2] = VertexProperties{{2, 2, 2}};
add_edge(0, 1, graph);
add_edge(1, 2, graph);
std::vector<Vertex> predecessors(num_vertices(graph));
std::vector<double> distances(num_vertices(graph));
auto index = get(boost::vertex_index, graph);
auto pmap = make_safe_iterator_property_map(predecessors.begin(), predecessors.size(), index);
auto dmap = make_safe_iterator_property_map(distances.begin(), distances.size(), index);
auto weightmap = get(&EdgeProperties::weight, graph);
std::mt19937 gen(std::random_device{}());
Vertex start = random_vertex(graph, gen);
Vertex goal = random_vertex(graph, gen);
try {
// call astar named parameter interface
astar_search( //
graph, start, distance_heuristic{goal, graph},
boost::predecessor_map(pmap) //
.distance_map(dmap)
.weight_map(weightmap)
.visitor(goal_visitor{goal}));
fmt::print("{} -> {}: No path\n", start, goal);
} catch (goal_visitor::found) {
std::list<Vertex> path;
for (auto cursor = goal;;) {
path.push_front(cursor);
auto previous = std::exchange(cursor, predecessors.at(cursor));
if (cursor == previous)
break;
}
fmt::print("{} -> {}: {}\n", start, goal, path);
}
}
Which prints e.g.
2 -> 1: [2, 1]
If you want to encapsulate, do it along the functional boundaries, instead of artificial boundaries that gave you the lifetime headaches you didn't need. If performance is no concern, you can reduce code with a facility like vector_property_map. For example:
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/astar_search.hpp>
#include <boost/graph/graph_utility.hpp>
#include <boost/graph/random.hpp>
#include <boost/property_map/transform_value_property_map.hpp>
#include <boost/property_map/vector_property_map.hpp>
#include <fmt/ranges.h>
#include <random>
class BoostGraph {
private:
using JointAngles = std::vector<double>;
struct VertexProperties {
JointAngles joint_angles{0, 0, 0};
};
struct EdgeProperties {
double weight = 0;
};
using Graph = boost::adjacency_list<boost::setS, boost::vecS, boost::undirectedS,
VertexProperties, EdgeProperties>;
using Vertex = Graph::vertex_descriptor;
public:
BoostGraph() : m_graph(4) {
// TODO read graph
m_graph[0] = VertexProperties{{0, 0, 0}};
m_graph[1] = VertexProperties{{1, 1, 1}};
m_graph[2] = VertexProperties{{2, 2, 2}};
add_edge(0, 1, m_graph);
add_edge(1, 2, m_graph);
}
friend std::ostream& operator<<(std::ostream& os, BoostGraph const& bg) {
auto name = boost::make_transform_value_property_map(
[ja = get(&VertexProperties::joint_angles, bg.m_graph)](Vertex v) {
return fmt::format("Vertex #{} ({})", v, ja[v]);
},
get(boost::vertex_index, bg.m_graph));
print_graph(bg.m_graph, name, os);
return os;
}
Vertex random_vertex() { return boost::random_vertex(m_graph, m_prng); }
std::list<Vertex> find_path(Vertex start, Vertex goal) const {
std::list<Vertex> path;
auto pmap = make_vector_property_map<Vertex>(get(boost::vertex_index, m_graph));
try {
astar_search( //
m_graph, start, distance_heuristic{goal, m_graph},
boost::predecessor_map(pmap) //
.weight_map(get(&EdgeProperties::weight, m_graph))
.visitor(finder{goal}));
} catch (finder::found) {
for (auto cursor = goal;;) {
path.push_front(cursor);
auto previous = std::exchange(cursor, pmap[cursor]);
if (cursor == previous)
break;
}
}
return path;
}
private:
// visitor that terminates when we find the goal
struct finder : boost::default_astar_visitor {
struct found {}; // exception for termination
Vertex m_goal;
finder(Vertex g) : m_goal(g) {}
void examine_vertex(Vertex u, Graph const&) const {
if (u == m_goal)
throw found{};
}
};
// euclidean distance heuristic
struct distance_heuristic : boost::astar_heuristic<Graph, double> {
distance_heuristic(Vertex goal, Graph const& graph)
: m_graph(graph)
, m_goal(graph[goal].joint_angles) {}
double operator()(Vertex u) const {
auto& c = m_graph[u].joint_angles;
auto //
dx = m_goal.at(0) - c.at(0), //
dy = m_goal.at(1) - c.at(1), //
dz = m_goal.at(2) - c.at(2);
using std::sqrt; // allow ADL, good practice
return sqrt(dx * dx + dy * dy + dz * dz);
}
private:
Graph const& m_graph; // reference!
JointAngles m_goal;
};
Graph m_graph;
std::mt19937 m_prng{std::random_device{}()};
};
int main() {
BoostGraph bg;
std::cout << "Graph: " << bg << "\n";
for (auto i = 0; i < 10; ++i) {
auto start = bg.random_vertex(), goal = bg.random_vertex();
auto path = bg.find_path(start, goal);
fmt::print("{} -> {}: {}\n", start, goal, path);
}
}
Printing e.g.
Graph: Vertex #0 ([0, 0, 0]) <--> Vertex #1 ([1, 1, 1])
Vertex #1 ([1, 1, 1]) <--> Vertex #0 ([0, 0, 0]) Vertex #2 ([2, 2, 2])
Vertex #2 ([2, 2, 2]) <--> Vertex #1 ([1, 1, 1])
Vertex #3 ([0, 0, 0]) <-->
2 -> 2: [2]
1 -> 0: [1, 0]
0 -> 1: [0, 1]
2 -> 0: [2, 1, 0]
3 -> 0: []
0 -> 3: []
0 -> 3: []
1 -> 0: [1, 0]
1 -> 0: [1, 0]
3 -> 1: []