From cd722dc2e68b1d2045a89ea0915b7e5e3a7b4569 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Fri, 29 Aug 2014 14:02:51 +0200
Subject: [PATCH] Roadmap do not duplicate NodeHistogram
---
include/hpp/manipulation/roadmap.hh | 3 ++-
src/roadmap.cc | 15 ++++++++++++---
2 files changed, 14 insertions(+), 4 deletions(-)
diff --git a/include/hpp/manipulation/roadmap.hh b/include/hpp/manipulation/roadmap.hh
index 9a54daf..929830d 100644
--- a/include/hpp/manipulation/roadmap.hh
+++ b/include/hpp/manipulation/roadmap.hh
@@ -55,9 +55,10 @@ namespace hpp {
Roadmap (const core::DistancePtr_t& distance, const core::DevicePtr_t& robot);
private:
+ typedef std::list < graph::HistogramPtr_t > Histograms;
/// Keep track of the leaf that are explored.
/// There should be one histogram per foliation.
- std::vector < graph::HistogramPtr_t > histograms_;
+ Histograms histograms_;
};
} // namespace manipulation
} // namespace hpp
diff --git a/src/roadmap.cc b/src/roadmap.cc
index 10e478e..d7b78f3 100644
--- a/src/roadmap.cc
+++ b/src/roadmap.cc
@@ -14,6 +14,8 @@
// received a copy of the GNU Lesser General Public License along with
// hpp-manipulation. If not, see <http://www.gnu.org/licenses/>.
+#include <hpp/util/pointer.hh>
+
#include "hpp/manipulation/roadmap.hh"
namespace hpp {
@@ -29,8 +31,8 @@ namespace hpp {
void Roadmap::clear ()
{
Parent::clear ();
- std::vector < graph::HistogramPtr_t > newHistograms;
- std::vector < graph::HistogramPtr_t >::iterator it;
+ Histograms newHistograms;
+ Histograms::iterator it;
for (it = histograms_.begin(); it != histograms_.end(); it++) {
newHistograms.push_back ((*it)->clone ());
}
@@ -45,7 +47,7 @@ namespace hpp {
void Roadmap::statInsert (ConfigurationIn_t config)
{
- std::vector < graph::HistogramPtr_t >::iterator it;
+ Histograms::iterator it;
for (it = histograms_.begin(); it != histograms_.end(); it++) {
(*it)->add (config);
}
@@ -58,6 +60,13 @@ namespace hpp {
void Roadmap::constraintGraph (const graph::GraphPtr_t& graph)
{
+ Histograms::iterator it = histograms_.begin();
+ for (; it != histograms_.end();) {
+ if (HPP_DYNAMIC_PTR_CAST (graph::NodeHistogram, *it))
+ it = histograms_.erase (it);
+ else
+ it++;
+ }
histograms_.push_back (graph::HistogramPtr_t (new graph::NodeHistogram (graph)));
}
} // namespace manipulation
--
GitLab