I am trying to create a boost graph of robot joint configuration space. For that I am using custom VertexProperty.
struct VertexProperties {
std::vector<double> joint_angles;
VertexProperties() : joint_angles(3){}
};
I have saved the graph using write_graphML with below implementation
std::string BoostGraph::vertexJointAngles(Vertex& v){
std::ostringstream sstream;
std::vector<double> jointAngles(graph[v].joint_angles);
for (std::size_t i = 0; i < jointAngles.size(); i++){
sstream << jointAngles[i];
if(i != jointAngles.size()-1) sstream << ',';
}
return sstream.str();
}
```
void BoostGraph::printGraphML(std::ostream &out){
boost::function_property_map<std::function<std::string(Vertex)>, Vertex> jointAngles([this](Vertex v)
{
return vertexJointAngles(v);
});
});
boost::dynamic_properties dp;
dp.property("Joint_Angles", jointAngles);
dp.property("edge_weight", boost::get(&EdgeProperties::weight, graph));
boost::write_graphml(out, graph, dp);
}
Now I need to read the graph again in another BoostGraph class instance for query and A* search. How can I convert the joint angles saved as string in graphml to vertex property?
I tried the below implementation ( I am just starting in Boost) but its not working.
std::vector<double> BoostGraph::readJointAngles(std::string joint_angle_string){
std::stringstream ss;
ss << joint_angle_string;
std::vector<double> joint_angle;
double number;
while (ss >> number)
joint_angle.push_back(number);
return joint_angle;
}
void BoostGraph::readGraphGraphML(std::istream& file){
boost::dynamic_properties dp;
boost::function_property_map<std::function<std::vector<double>(std::string)>, Vertex> jointAngles([this](std::string v)
{
return readJointAngles(v);
});
dp.property("Joint_Angles", jointAngles );
dp.property("edge_weight", boost::get(&EdgeProperties::weight, graph));
boost::read_graphml(file, graph, dp);
}
Good starting point there. Nice job.
Therefore I'm going to jump immediately to the pain point:
Writable property maps have to return something that models lvalue semantics. Your readJointAngles assumes a "procedural" conversion task instead. That's not a property map.
My first thought is to stay close to the concept of the property map by combining the read and write into a proxy class. The proxy class can be VertexProperties itself, but as I can imagine VertexProperties having multiple members with similar requirements, let me do the "pure" thing and have it separate here:
struct JointAngleWrapper {
std::vector<double>& _ref;
friend std::ostream& operator<<(std::ostream& os, JointAngleWrapper wrapper) {
for (auto sep = ""; auto el : wrapper._ref)
os << std::exchange(sep, ",") << el;
return os;
}
friend std::istream& operator>>(std::istream& is, JointAngleWrapper wrapper) {
std::vector<double> tmp;
while (is >> tmp.emplace_back())
if (char comma{}; !(is >> comma) || comma != ',')
break;
wrapper._ref = std::move(tmp);
is.clear();
return is;
}
};
Now you can replace the function property-map:
auto jointAngles = boost::make_function_property_map<Vertex>(
[ja = get(&VertexProperties::joint_angles, graph)](Vertex v) {
return JointAngleWrapper{ja[v]};
});
dp.property("Joint_Angles", jointAngles);
And the output will still be identical. Actually, we have a more light-weight property map abstraction that fits the bill even more elegantly:
auto jointAngles = make_transform_value_property_map(
[](std::vector<double>& ja) { return JointAngleWrapper{ja}; },
get(&VertexProperties::joint_angles, graph));
However, function_property_map is still not an lvalue-property map. That's sad.
Turns out I forgot about this snag. I've written about this rather central issue with read_graphml (or with dynamic_properties, really) before:
The approach I took there was to create my own ReadWritePropertyMap. Which is sensible, given that it essentially merges into what we already have above (JointAngleWrapper).
template <typename Prop> struct JA {
Prop inner;
JA(Prop map) : inner(map) { }
// traits
using value_type = std::string;
using reference = std::string;
using key_type = typename boost::property_traits<Prop>::key_type;
using category = boost::read_write_property_map_tag;
friend std::string get(JA map, key_type const& key) {
std::ostringstream oss;
for (auto sep = ""; auto el : get(map.inner, key))
oss << std::exchange(sep, ",") << el;
return oss.str();
}
friend void put(JA map, key_type const& key, value_type const& value) {
auto& v = get(map.inner, key);
v.clear();
std::istringstream iss(value);
while (iss >> v.emplace_back())
if (char comma{}; !(iss >> comma) || comma != ',')
break;
}
};
Now you can use it with success:
struct BoostGraph {
Graph graph;
auto properties() {
boost::dynamic_properties dp;
dp.property("Joint_Angles", JA{get(&VertexProperties::joint_angles, graph)});
dp.property("edge_weight", get(&EdgeProperties::weight, graph));
return dp;
}
void printGraphML(std::ostream& out) {
write_graphml(out, graph, properties());
}
void readGraphGraphML(std::istream& file) {
auto dp = properties();
read_graphml(file, graph, dp);
}
};
Which successfully roundtrips a "random" graph:
#include <boost/graph/random.hpp>
#include <random>
int main() {
std::stringstream file; // fake file in memory
{
BoostGraph g;
std::mt19937 prng{42};
generate_random_graph(g.graph, 5, 2, prng);
for (auto v : boost::make_iterator_range(vertices(g.graph)))
for (auto& angle : g.graph[v].joint_angles)
angle = std::uniform_real_distribution<double>(-M_PI, +M_PI)(prng);
g.printGraphML(file);
}
{
BoostGraph roundtrip;
roundtrip.readGraphGraphML(file);
roundtrip.printGraphML(std::cout);
}
}
Prints
<?xml version="1.0" encoding="UTF-8"?>
<graphml xmlns="http://graphml.graphdrawing.org/xmlns" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:schemaLocation="http://graphml.graphdrawing.org/xmlns http://graphml.graphdrawing.org/xmlns/1.0/graphml.xsd">
<key id="key0" for="node" attr.name="Joint_Angles" attr.type="string" />
<key id="key1" for="edge" attr.name="edge_weight" attr.type="double" />
<graph id="G" edgedefault="directed" parse.nodeids="free" parse.edgeids="canonical" parse.order="nodesfirst">
<node id="n0">
<data key="key0">1.75735,0.608528,-0.340343</data>
</node>
<node id="n1">
<data key="key0">-2.51343,-0.256047,-1.04484</data>
</node>
<node id="n2">
<data key="key0">-2.24393,0.94806,-2.78715</data>
</node>
<node id="n3">
<data key="key0">1.39486,2.75551,-3.1367</data>
</node>
<node id="n4">
<data key="key0">3.09266,0.738158,0.701538</data>
</node>
<edge id="e0" source="n1" target="n3">
<data key="key1">0</data>
</edge>
<edge id="e1" source="n4" target="n0">
<data key="key1">0</data>
</edge>
</graph>
</graphml>
#include <boost/graph/graphml.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/property_map/transform_value_property_map.hpp>
struct VertexProperties {
std::vector<double> joint_angles{0, 0, 0};
};
struct EdgeProperties {
double weight;
};
using Graph = boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS,
VertexProperties, EdgeProperties>;
using Vertex = Graph::vertex_descriptor;
using Edge = Graph::edge_descriptor;
template <typename Prop> struct JA {
Prop inner;
JA(Prop map) : inner(map) { }
// traits
using value_type = std::string;
using reference = std::string;
using key_type = typename boost::property_traits<Prop>::key_type;
using category = boost::read_write_property_map_tag;
friend std::string get(JA map, key_type const& key) {
std::ostringstream oss;
for (auto sep = ""; auto el : get(map.inner, key))
oss << std::exchange(sep, ",") << el;
return oss.str();
}
friend void put(JA map, key_type const& key, value_type const& value) {
auto& v = get(map.inner, key);
v.clear();
std::istringstream iss(value);
while (iss >> v.emplace_back())
if (char comma{}; !(iss >> comma) || comma != ',')
break;
}
};
struct BoostGraph {
Graph graph;
auto properties() {
boost::dynamic_properties dp;
dp.property("Joint_Angles", JA{get(&VertexProperties::joint_angles, graph)});
dp.property("edge_weight", get(&EdgeProperties::weight, graph));
return dp;
}
void printGraphML(std::ostream& out) {
write_graphml(out, graph, properties());
}
void readGraphGraphML(std::istream& file) {
auto dp = properties();
read_graphml(file, graph, dp);
}
};
#include <boost/graph/random.hpp>
#include <random>
int main() {
std::stringstream file; // fake file in memory
{
BoostGraph g;
std::mt19937 prng{42};
generate_random_graph(g.graph, 5, 2, prng);
for (auto v : boost::make_iterator_range(vertices(g.graph)))
for (auto& angle : g.graph[v].joint_angles)
angle = std::uniform_real_distribution<double>(-M_PI, +M_PI)(prng);
g.printGraphML(file);
}
{
BoostGraph roundtrip;
roundtrip.readGraphGraphML(file);
roundtrip.printGraphML(std::cout);
}
}