From a97ff1bfb839bc1b320b0070d3e5510ab6fb8759 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Wed, 5 Nov 2014 15:01:22 +0100
Subject: [PATCH] WaypointEdge::node return the waypoint node if the second
part is in it.
---
include/hpp/manipulation/graph/edge.hh | 5 ++++-
src/graph/edge.cc | 6 ++++++
2 files changed, 10 insertions(+), 1 deletion(-)
diff --git a/include/hpp/manipulation/graph/edge.hh b/include/hpp/manipulation/graph/edge.hh
index c32940f..1e67e6e 100644
--- a/include/hpp/manipulation/graph/edge.hh
+++ b/include/hpp/manipulation/graph/edge.hh
@@ -75,7 +75,7 @@ namespace hpp {
NodePtr_t from () const;
/// Get the node in which path is.
- NodePtr_t node () const;
+ virtual NodePtr_t node () const;
void isInNodeFrom (bool iinf)
{
@@ -176,6 +176,9 @@ namespace hpp {
/// inner nodes are named bname + "_n" + pos
void createWaypoint (const unsigned int depth, const std::string& bname = "WaypointEdge");
+ /// Get the node in which path after the waypoint is.
+ NodePtr_t node () const;
+
protected:
/// Initialization of the object.
void init (const EdgeWkPtr_t& weak, const GraphWkPtr_t& graph, const NodeWkPtr_t& from,
diff --git a/src/graph/edge.cc b/src/graph/edge.cc
index 57892d2..02baecd 100644
--- a/src/graph/edge.cc
+++ b/src/graph/edge.cc
@@ -240,6 +240,12 @@ namespace hpp {
config_ = Configuration_t(graph_.lock ()->robot ()->configSize ());
}
+ NodePtr_t WaypointEdge::node () const
+ {
+ if (isInNodeFrom ()) return waypoint_.second;
+ else return to ();
+ }
+
EdgePtr_t WaypointEdge::waypoint () const
{
return waypoint_.first;
--
GitLab