Nodes in NodeSelector can be ordered via weights

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:

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

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