Commit 56bc09b5 authored by Florent Lamiraux's avatar Florent Lamiraux
Browse files

[Roadmap] Add method to merge two roadmaps.

parent 8a3f3ac8
Pipeline #16121 passed with stage
in 8 minutes and 59 seconds
......@@ -177,6 +177,9 @@ namespace hpp {
void addEdges (const NodePtr_t from, const NodePtr_t& to,
const PathPtr_t& path);
/// Add the nodes and edges of a roadmap into this one.
void merge(const RoadmapPtr_t& other);
/// Add a goal configuration
/// \param config configuration
/// If configuration is already in the roadmap, tag corresponding node
......
......@@ -179,6 +179,22 @@ namespace hpp {
return nodeFrom;
}
void Roadmap::merge(const RoadmapPtr_t& other)
{
// Map nodes of other roadmap with nodes of this one
std::map<core::NodePtr_t, core::NodePtr_t> cNode;
for (const core::NodePtr_t& node: other->nodes()) {
cNode[node] = this->addNode(node->configuration());
}
for (const core::EdgePtr_t& edge: other->edges()) {
if (edge->path()->length() == 0)
assert (edge->from() == edge->to());
else
this->addEdges(cNode[edge->from()], cNode[edge->to()],
edge->path());
}
}
NodePtr_t
Roadmap::nearestNode (const Configuration_t& configuration,
value_type& minDistance, bool reverse)
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment