From a6d20370451138c9dea35d3e0bb7722aa6e1b14f Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Tue, 27 Mar 2018 18:16:48 +0200
Subject: [PATCH] ProblemSolver has a map of graph::GraphPtr_t
---
include/hpp/manipulation/graph/graph.hh | 5 ++++-
include/hpp/manipulation/problem-solver.hh | 4 +++-
src/graph/graph.cc | 8 ++++++++
src/graph/helper.cc | 4 ++--
src/problem-solver.cc | 10 +++++++---
src/problem.cc | 1 +
6 files changed, 25 insertions(+), 7 deletions(-)
diff --git a/include/hpp/manipulation/graph/graph.hh b/include/hpp/manipulation/graph/graph.hh
index 9d1b67c..c432ccf 100644
--- a/include/hpp/manipulation/graph/graph.hh
+++ b/include/hpp/manipulation/graph/graph.hh
@@ -235,9 +235,12 @@ namespace hpp {
/// Get the robot.
const DevicePtr_t& robot () const;
- /// Get the steering Method
+ /// Get the problem
const ProblemPtr_t& problem () const;
+ /// Set the problem
+ void problem (const ProblemPtr_t& problem);
+
/// Register an histogram representing a foliation
void insertHistogram (const graph::HistogramPtr_t& hist)
{
diff --git a/include/hpp/manipulation/problem-solver.hh b/include/hpp/manipulation/problem-solver.hh
index 3dddc4e..cd63fe1 100644
--- a/include/hpp/manipulation/problem-solver.hh
+++ b/include/hpp/manipulation/problem-solver.hh
@@ -63,7 +63,7 @@ namespace hpp {
/// \{
/// Set the constraint graph
- void constraintGraph (const graph::GraphPtr_t& graph);
+ void constraintGraph (const std::string& graph);
/// Get the constraint graph
graph::GraphPtr_t constraintGraph () const;
@@ -134,6 +134,8 @@ namespace hpp {
void setTargetState (const graph::StatePtr_t state);
+ core::Container <graph::GraphPtr_t> graphs;
+
protected:
virtual void initializeProblem (ProblemPtr_t problem);
diff --git a/src/graph/graph.cc b/src/graph/graph.cc
index f50d5af..97c8458 100644
--- a/src/graph/graph.cc
+++ b/src/graph/graph.cc
@@ -108,6 +108,14 @@ namespace hpp {
return robot_;
}
+ void Graph::problem (const ProblemPtr_t& problem)
+ {
+ if (problem_ != problem) {
+ problem_ = problem;
+ setDirty();
+ }
+ }
+
const ProblemPtr_t& Graph::problem () const
{
return problem_;
diff --git a/src/graph/helper.cc b/src/graph/helper.cc
index 224c71d..1dd9637 100644
--- a/src/graph/helper.cc
+++ b/src/graph/helper.cc
@@ -1113,7 +1113,8 @@ namespace hpp {
}
GraphPtr_t graph = Graph::create (graphName,
ps->robot(), ps->problem());
- ps->constraintGraph (graph);
+ ps->graphs.add (graphName, graph);
+ ps->constraintGraph (graphName);
graph->stateSelector (
GuidedStateSelector::create ("stateSelector",
ps->roadmap ()));
@@ -1121,7 +1122,6 @@ namespace hpp {
graph->errorThreshold (ps->errorThreshold ());
graphBuilder (ps, objects, grippers, graph, rules);
- ps->constraintGraph (graph);
return graph;
}
} // namespace helper
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index 6016b9f..f70e317 100644
--- a/src/problem-solver.cc
+++ b/src/problem-solver.cc
@@ -197,15 +197,19 @@ namespace hpp {
{
problem_ = problem;
core::ProblemSolver::initializeProblem (problem_);
+ problem_->constraintGraph (constraintGraph_);
if (problem_->pathValidation ())
problem_->pathValidation ()->constraintGraph (constraintGraph_);
}
- void ProblemSolver::constraintGraph (const graph::GraphPtr_t& graph)
+ void ProblemSolver::constraintGraph (const std::string& graphName)
{
- constraintGraph_ = graph;
+ if (!graphs.has (graphName))
+ throw std::invalid_argument ("ProblemSolver has no graph named " + graphName);
+ constraintGraph_ = graphs.get(graphName);
RoadmapPtr_t r = HPP_DYNAMIC_PTR_CAST (Roadmap, roadmap());
- if (r) r->constraintGraph (graph);
+ if (r) r->constraintGraph (constraintGraph_);
+ if (problem_) problem_->constraintGraph (constraintGraph_);
}
graph::GraphPtr_t ProblemSolver::constraintGraph () const
diff --git a/src/problem.cc b/src/problem.cc
index 883f4a9..ca00798 100644
--- a/src/problem.cc
+++ b/src/problem.cc
@@ -38,6 +38,7 @@ namespace hpp {
void Problem::constraintGraph (const graph::GraphPtr_t& graph)
{
graph_ = graph;
+ graph_->problem (this);
if (pathValidation ())
pathValidation ()->constraintGraph (graph);
WeighedDistancePtr_t d = HPP_DYNAMIC_PTR_CAST (WeighedDistance,
--
GitLab