From b18d88d32ca6919f7aef74e346088ec0cfaf94b0 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Sat, 20 Aug 2016 17:44:56 +0200
Subject: [PATCH] Roadmap insert new nodes in the histograms of the Graph
* When a histogram is inserted, add all the current nodes to this
* histogram.
---
src/graph/graph.cc | 4 ++++
src/roadmap.cc | 30 +++++++++++++++++++-----------
2 files changed, 23 insertions(+), 11 deletions(-)
diff --git a/src/graph/graph.cc b/src/graph/graph.cc
index fc8385ec..7b73773b 100644
--- a/src/graph/graph.cc
+++ b/src/graph/graph.cc
@@ -22,6 +22,7 @@
#include "hpp/manipulation/graph/node-selector.hh"
#include "hpp/manipulation/graph/node.hh"
#include "hpp/manipulation/graph/edge.hh"
+#include "hpp/manipulation/graph/statistics.hh"
namespace hpp {
namespace manipulation {
@@ -40,6 +41,9 @@ namespace hpp {
GraphComponent::init (weak);
robot_ = robot;
wkPtr_ = weak;
+ insertHistogram(graph::HistogramPtr_t (
+ new graph::NodeHistogram (wkPtr_.lock()))
+ );
}
NodeSelectorPtr_t Graph::createNodeSelector (const std::string& name)
diff --git a/src/roadmap.cc b/src/roadmap.cc
index 2e5bc197..48b08ea5 100644
--- a/src/roadmap.cc
+++ b/src/roadmap.cc
@@ -22,9 +22,10 @@
#include <hpp/core/distance.hh>
#include <hpp/manipulation/roadmap.hh>
-#include <hpp/manipulation/graph/node.hh>
#include <hpp/manipulation/roadmap-node.hh>
#include <hpp/manipulation/symbolic-component.hh>
+#include <hpp/manipulation/graph/node.hh>
+#include <hpp/manipulation/graph/statistics.hh>
namespace hpp {
namespace manipulation {
@@ -42,9 +43,13 @@ namespace hpp {
void Roadmap::clear ()
{
Parent::clear ();
- for (Histograms_t::iterator it = histograms_.begin();
- it != histograms_.end(); ++it) {
+ Histograms_t::const_iterator it;
+ for (it = histograms_.begin(); it != histograms_.end(); ++it)
(*it)->clear ();
+ if (graph_) {
+ const Histograms_t& hs = graph_->histograms();
+ for (it = hs.begin(); it != hs.end(); ++it)
+ (*it)->clear ();
}
}
@@ -62,24 +67,27 @@ namespace hpp {
Histograms_t::const_iterator it;
for (it = histograms_.begin(); it != histograms_.end(); ++it)
(*it)->add (n);
+ if (graph_) {
+ const Histograms_t& hs = graph_->histograms();
+ for (it = hs.begin(); it != hs.end(); ++it)
+ (*it)->add (n);
+ }
}
void Roadmap::insertHistogram (const graph::HistogramPtr_t hist)
{
histograms_.push_back (hist);
+ core::Nodes_t::const_iterator _node;
+ for (_node = nodes().begin(); _node != nodes().end(); ++_node)
+ hist->add (static_cast <const RoadmapNodePtr_t>(*_node));
}
void Roadmap::constraintGraph (const graph::GraphPtr_t& graph)
{
graph_ = graph;
- Histograms::iterator it = histograms_.begin();
- for (; it != histograms_.end();) {
- if (HPP_DYNAMIC_PTR_CAST (graph::NodeHistogram, *it))
- it = histograms_.erase (it);
- else
- ++it;
- }
- insertHistogram (graph::HistogramPtr_t (new graph::NodeHistogram (graph)));
+ // FIXME Add the current nodes() to the graph->histograms()
+ // The main issue is that new histograms may be added to
+ // graph->histograms() and this class will not know it.
}
RoadmapNodePtr_t Roadmap::nearestNode (const ConfigurationPtr_t& configuration,
--
GitLab