Waypoint nodes are created through NodeSelector class

include/hpp/manipulation/graph/node-selector.hh

@@ -34,7 +34,7 @@ namespace hpp {
           static NodeSelectorPtr_t create(const std::string& name);
 
           /// Create an empty node
-          NodePtr_t createNode (const std::string& name);
+          NodePtr_t createNode (const std::string& name, bool waypoint = false);
 
           /// Returns the state of a configuration.
           NodePtr_t getNode(ConfigurationIn_t config) const;
@@ -82,6 +82,7 @@ namespace hpp {
 
           /// List of the states of one end-effector, ordered by priority.
           Nodes_t orderedStates_;
+          Nodes_t waypoints_;
 
         private:
           /// Weak pointer to itself.

include/hpp/manipulation/graph/node.hh

@@ -69,6 +69,16 @@ namespace hpp {
           /// the constraints. Instead, use the class NodeSelector.
           virtual bool contains (ConfigurationIn_t config) const;
 
+          inline bool isWaypoint () const
+          {
+            return isWaypoint_;
+          }
+
+          inline void isWaypoint (bool isWaypoint)
+          {
+            isWaypoint_ = isWaypoint;
+          }
+
           /// Get the parent NodeSelector.
           NodeSelectorWkPtr_t nodeSelector () const
           {
@@ -160,6 +170,8 @@ namespace hpp {
 
           /// Weak pointer to itself.
           NodeWkPtr_t wkPtr_;
+
+          bool isWaypoint_;
       }; // class Node
 
       /// \}

src/graph/node-selector.cc

@@ -39,12 +39,15 @@ namespace hpp {
         wkPtr_ = weak;
       }
 
-      NodePtr_t NodeSelector::createNode (const std::string& name)
+      NodePtr_t NodeSelector::createNode (const std::string& name,
+          bool waypoint)
       {
         NodePtr_t newNode = Node::create (name);
         newNode->nodeSelector(wkPtr_);
         newNode->parentGraph(graph_);
-        orderedStates_.push_back(newNode);
+        newNode->isWaypoint (waypoint);
+        if (waypoint) waypoints_.push_back(newNode);
+        else orderedStates_.push_back(newNode);
         return newNode;
       }
 

src/graph/node.cc

@@ -27,7 +27,8 @@ namespace hpp {
   namespace manipulation {
     namespace graph {
       Node::Node (const std::string& name) :
-	GraphComponent (name), configConstraints_ (new Constraint_t())
+	GraphComponent (name), configConstraints_ (new Constraint_t()),
+        isWaypoint_ (false)
       {}
 
       Node::~Node ()