Commit 8989d2d7 authored by Florent Lamiraux's avatar Florent Lamiraux
Browse files

[Roadmap] Add method to insert a PathVector in a roadmap.

parent ab36b3b0
Pipeline #16160 passed with stage
in 8 minutes and 46 seconds
......@@ -180,6 +180,12 @@ namespace hpp {
/// Add the nodes and edges of a roadmap into this one.
void merge(const RoadmapPtr_t& other);
/// Add a PathVector instance in the roadmap
/// Waypoints are inserted as nodes,
/// each elementary path is inserted as an edge
/// \param backAndForth whether to insert the reverse path as well.
void insertPathVector(const PathVectorPtr_t& path, bool backAndForth);
/// Add a goal configuration
/// \param config configuration
/// If configuration is already in the roadmap, tag corresponding node
......
......@@ -27,6 +27,7 @@
#include <hpp/core/edge.hh>
#include <hpp/core/node.hh>
#include <hpp/core/path.hh>
#include <hpp/core/path-vector.hh>
#include <hpp/core/roadmap.hh>
#include <../src/nearest-neighbor/basic.hh>
......@@ -195,6 +196,27 @@ namespace hpp {
}
}
void Roadmap::insertPathVector(const PathVectorPtr_t& path,
bool backAndForth)
{
if (path->constraints()) {
throw std::logic_error("Cannot insert a path vector with constraints"
" in a roadmap.");
}
Configuration_t q_init(path->initial());
NodePtr_t n(addNode(q_init));
for (std::size_t i=0; i<path->numberPaths(); ++i) {
PathPtr_t p(path->pathAtRank(i));
if (backAndForth){
n = addNodeAndEdges(n, ConfigurationPtr_t(new Configuration_t
(p->end())), p);
} else {
n = addNodeAndEdge(n, ConfigurationPtr_t(new Configuration_t
(p->end())), p);
}
}
}
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