From caecc03116426bba9a7864206d70c2d928613bf5 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Tue, 22 Jul 2014 10:23:22 +0200
Subject: [PATCH] Add convinient methods to retrieve informations in the
components
---
include/hpp/manipulation/graph/graph.hh | 5 +++++
.../hpp/manipulation/graph/node-selector.hh | 5 ++++-
include/hpp/manipulation/graph/node.hh | 11 ++++++++++-
src/graph/graph.cc | 10 ++++++++++
src/graph/node-selector.cc | 9 +++++++++
src/graph/node.cc | 19 ++++++++++++++++---
6 files changed, 54 insertions(+), 5 deletions(-)
diff --git a/include/hpp/manipulation/graph/graph.hh b/include/hpp/manipulation/graph/graph.hh
index 34ea8a8c..047d281d 100644
--- a/include/hpp/manipulation/graph/graph.hh
+++ b/include/hpp/manipulation/graph/graph.hh
@@ -18,6 +18,7 @@
# define HPP_MANIPULATION_GRAPH_GRAPH_HH
# include <string>
+# include <hpp/util/assertion.hh>
# include "hpp/manipulation/robot.hh"
@@ -74,6 +75,10 @@ namespace hpp {
/// Select randomly outgoing edges of the given nodes.
virtual Edges_t chooseEdge(const Nodes_t& node);
+ /// Return the NodeSelector with the given name if any,
+ /// NULL pointer if not found.
+ NodeSelectorPtr_t getNodeSelectorByName (const std::string& name);
+
protected:
/// Initialization of the object.
void init (const GraphWkPtr_t& weak, RobotPtr_t robot);
diff --git a/include/hpp/manipulation/graph/node-selector.hh b/include/hpp/manipulation/graph/node-selector.hh
index 01c2b57e..dc6834a9 100644
--- a/include/hpp/manipulation/graph/node-selector.hh
+++ b/include/hpp/manipulation/graph/node-selector.hh
@@ -33,7 +33,10 @@ namespace hpp {
/// Create a new NodeSelector.
static NodeSelectorPtr_t create();
- /// Create a nodes with the constraints
+ /// Create an empty node
+ NodePtr_t createNode ();
+
+ /// Create a node with the constraints
NodePtr_t createNode (const ConstraintPtr_t& constraints);
/// Returns the state of a configuration.
diff --git a/include/hpp/manipulation/graph/node.hh b/include/hpp/manipulation/graph/node.hh
index 4055e8de..9766a3c3 100644
--- a/include/hpp/manipulation/graph/node.hh
+++ b/include/hpp/manipulation/graph/node.hh
@@ -34,6 +34,9 @@ namespace hpp {
{
public:
/// Create a new node.
+ static NodePtr_t create ();
+
+ /// Create a new node with the specified constraints
static NodePtr_t create (const ConstraintPtr_t& constraints);
/// Create a link from this node to the given node.
@@ -72,9 +75,15 @@ namespace hpp {
};
/// Get the neighbors
- const Edges_t& neighbors() const;
+ const Edges_t& neighbors() const
+ {
+ return neighbors_;
+ }
protected:
+ /// Initialize the object.
+ void init (const NodeWkPtr_t& self);
+
/// Initialize the object.
void init (const NodeWkPtr_t& self, const ConstraintPtr_t& constraints);
diff --git a/src/graph/graph.cc b/src/graph/graph.cc
index 3bcfdd20..1ac73fc2 100644
--- a/src/graph/graph.cc
+++ b/src/graph/graph.cc
@@ -60,6 +60,16 @@ namespace hpp {
edges.push_back( (*it)->nodeSelector().lock()->chooseEdge(*it) );
return edges;
}
+
+ NodeSelectorPtr_t Graph::getNodeSelectorByName (const std::string& name)
+ {
+ for (NodeSelectors_t::iterator it = nodeSelectors_.begin();
+ it == nodeSelectors_.end(); it++) {
+ if (name == (*it)->name())
+ return *it;
+ }
+ return NodeSelectorPtr_t();
+ }
} // namespace graph
} // namespace manipulation
} // namespace hpp
diff --git a/src/graph/node-selector.cc b/src/graph/node-selector.cc
index 4e666436..17cff49a 100644
--- a/src/graph/node-selector.cc
+++ b/src/graph/node-selector.cc
@@ -34,6 +34,14 @@ namespace hpp {
wkPtr_ = weak;
}
+ NodePtr_t NodeSelector::createNode ()
+ {
+ NodePtr_t newNode = Node::create();
+ newNode->nodeSelector(wkPtr_);
+ orderedStates_.push_back(newNode);
+ return newNode;
+ }
+
NodePtr_t NodeSelector::createNode (const ConstraintPtr_t& constraints)
{
NodePtr_t newNode = Node::create(constraints);
@@ -48,6 +56,7 @@ namespace hpp {
orderedStates_.end() == it; it++)
if ((*it)->contains(config))
return *it;
+ return NodePtr_t ();
}
EdgePtr_t NodeSelector::chooseEdge(const NodePtr_t& node)
diff --git a/src/graph/node.cc b/src/graph/node.cc
index ec2e33fb..ad0ebd53 100644
--- a/src/graph/node.cc
+++ b/src/graph/node.cc
@@ -19,6 +19,14 @@
namespace hpp {
namespace manipulation {
namespace graph {
+ NodePtr_t Node::create ()
+ {
+ Node* node = new Node;
+ NodePtr_t shPtr(node);
+ shPtr->init(shPtr);
+ return shPtr;
+ }
+
NodePtr_t Node::create (const ConstraintPtr_t& constraints)
{
Node* node = new Node;
@@ -27,10 +35,15 @@ namespace hpp {
return shPtr;
}
- void Node::init (const NodeWkPtr_t& self, const ConstraintPtr_t& constraints)
+ void Node::init (const NodeWkPtr_t& weak)
+ {
+ wkPtr_ = weak;
+ }
+
+ void Node::init (const NodeWkPtr_t& self, const ConstraintPtr_t& constraint)
{
- wkPtr_ = self;
- constraints_ = constraints;
+ init(self);
+ constraints(constraint);
}
EdgePtr_t Node::linkTo(const NodePtr_t& to, const ConstraintPtr_t& constraints)
--
GitLab