From 457d857a25c115774537c3be360753d80472b826 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Tue, 16 Feb 2016 20:49:41 +0100
Subject: [PATCH] Nodes in NodeSelector can be ordered via weights
---
.../hpp/manipulation/graph/node-selector.hh | 12 +++---
src/graph/guided-node-selector.cc | 8 ++--
src/graph/node-selector.cc | 39 ++++++++++++++-----
3 files changed, 40 insertions(+), 19 deletions(-)
diff --git a/include/hpp/manipulation/graph/node-selector.hh b/include/hpp/manipulation/graph/node-selector.hh
index 300ef22e..e62c0f9b 100644
--- a/include/hpp/manipulation/graph/node-selector.hh
+++ b/include/hpp/manipulation/graph/node-selector.hh
@@ -34,7 +34,8 @@ namespace hpp {
static NodeSelectorPtr_t create(const std::string& name);
/// Create an empty node
- NodePtr_t createNode (const std::string& name, bool waypoint = false);
+ NodePtr_t createNode (const std::string& name, bool waypoint = false,
+ const int w = 0);
/// Returns the state of a configuration.
NodePtr_t getNode(ConfigurationIn_t config) const;
@@ -43,10 +44,7 @@ namespace hpp {
NodePtr_t getNode(RoadmapNodePtr_t node) const;
/// Returns a list of all the nodes
- const Nodes_t& getNodes () const
- {
- return orderedStates_;
- }
+ Nodes_t getNodes () const;
/// Select randomly an outgoing edge of the given node.
virtual EdgePtr_t chooseEdge(RoadmapNodePtr_t from) const;
@@ -81,7 +79,9 @@ namespace hpp {
virtual std::ostream& print (std::ostream& os) const;
/// List of the states of one end-effector, ordered by priority.
- Nodes_t orderedStates_;
+ typedef std::pair <int, NodePtr_t> WeighedNode_t;
+ typedef std::list <WeighedNode_t> WeighedNodes_t;
+ WeighedNodes_t orderedStates_;
Nodes_t waypoints_;
private:
diff --git a/src/graph/guided-node-selector.cc b/src/graph/guided-node-selector.cc
index 9bf354dc..a44640c3 100644
--- a/src/graph/guided-node-selector.cc
+++ b/src/graph/guided-node-selector.cc
@@ -130,9 +130,9 @@ namespace hpp {
std::ostream& GuidedNodeSelector::dotPrint (std::ostream& os, dot::DrawingAttributes) const
{
- for (Nodes_t::const_iterator it = orderedStates_.begin();
+ for (WeighedNodes_t::const_iterator it = orderedStates_.begin();
orderedStates_.end() != it; ++it)
- (*it)->dotPrint (os);
+ it->second->dotPrint (os);
return os;
}
@@ -140,9 +140,9 @@ namespace hpp {
{
os << "|-- ";
GraphComponent::print (os) << std::endl;
- for (Nodes_t::const_iterator it = orderedStates_.begin();
+ for (WeighedNodes_t::const_iterator it = orderedStates_.begin();
orderedStates_.end() != it; ++it)
- os << *(*it);
+ os << it->first << " " << *it->second;
return os;
}
} // namespace graph
diff --git a/src/graph/node-selector.cc b/src/graph/node-selector.cc
index 49a7787f..e5f77ca9 100644
--- a/src/graph/node-selector.cc
+++ b/src/graph/node-selector.cc
@@ -40,23 +40,44 @@ namespace hpp {
}
NodePtr_t NodeSelector::createNode (const std::string& name,
- bool waypoint)
+ bool waypoint, const int w)
{
NodePtr_t newNode = Node::create (name);
newNode->nodeSelector(wkPtr_);
newNode->parentGraph(graph_);
newNode->isWaypoint (waypoint);
if (waypoint) waypoints_.push_back(newNode);
- else orderedStates_.push_back(newNode);
+ else {
+ bool found = false;
+ for (WeighedNodes_t::iterator it = orderedStates_.begin();
+ it != orderedStates_.end (); ++it) {
+ if (it->first < w) {
+ orderedStates_.insert (it, WeighedNode_t(w,newNode));
+ found = true;
+ break;
+ }
+ }
+ if (!found)
+ orderedStates_.push_back (WeighedNode_t(w,newNode));
+ }
return newNode;
}
+ Nodes_t NodeSelector::getNodes () const
+ {
+ Nodes_t ret;
+ for (WeighedNodes_t::const_iterator it = orderedStates_.begin();
+ it != orderedStates_.end (); ++it)
+ ret.push_back (it->second);
+ return ret;
+ }
+
NodePtr_t NodeSelector::getNode(ConfigurationIn_t config) const
{
- for (Nodes_t::const_iterator it = orderedStates_.begin();
+ for (WeighedNodes_t::const_iterator it = orderedStates_.begin();
orderedStates_.end() != it; ++it) {
- if ((*it)->contains(config))
- return *it;
+ if (it->second->contains(config))
+ return it->second;
}
std::stringstream oss;
oss << "A configuration has no node:" << model::displayConfig (config);
@@ -95,9 +116,9 @@ namespace hpp {
std::ostream& NodeSelector::dotPrint (std::ostream& os, dot::DrawingAttributes) const
{
- for (Nodes_t::const_iterator it = orderedStates_.begin();
+ for (WeighedNodes_t::const_iterator it = orderedStates_.begin();
orderedStates_.end() != it; ++it)
- (*it)->dotPrint (os);
+ it->second->dotPrint (os);
return os;
}
@@ -105,9 +126,9 @@ namespace hpp {
{
os << "|-- ";
GraphComponent::print (os) << std::endl;
- for (Nodes_t::const_iterator it = orderedStates_.begin();
+ for (WeighedNodes_t::const_iterator it = orderedStates_.begin();
orderedStates_.end() != it; ++it)
- os << *(*it);
+ os << it->first << " " << *it->second;
return os;
}
} // namespace graph
--
GitLab