From 74d91cdadd41a3f3b55344d3d9b5b4febc9131f4 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Wed, 27 Jan 2016 14:11:45 +0100
Subject: [PATCH] Waypoint nodes are created through NodeSelector class
---
include/hpp/manipulation/graph/node-selector.hh | 3 ++-
include/hpp/manipulation/graph/node.hh | 12 ++++++++++++
src/graph/node-selector.cc | 7 +++++--
src/graph/node.cc | 3 ++-
4 files changed, 21 insertions(+), 4 deletions(-)
diff --git a/include/hpp/manipulation/graph/node-selector.hh b/include/hpp/manipulation/graph/node-selector.hh
index ae7a5f1..300ef22 100644
--- a/include/hpp/manipulation/graph/node-selector.hh
+++ b/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.
diff --git a/include/hpp/manipulation/graph/node.hh b/include/hpp/manipulation/graph/node.hh
index fb78a4c..e47b924 100644
--- a/include/hpp/manipulation/graph/node.hh
+++ b/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
/// \}
diff --git a/src/graph/node-selector.cc b/src/graph/node-selector.cc
index ce29ad0..49a7787 100644
--- a/src/graph/node-selector.cc
+++ b/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;
}
diff --git a/src/graph/node.cc b/src/graph/node.cc
index 598a43a..c6e2429 100644
--- a/src/graph/node.cc
+++ b/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 ()
--
GitLab