Add a method to clear all the roadmaps.

include/hpp/core/planner.hh

@@ -186,6 +186,9 @@ namespace hpp {
 			       CkwsRoadmapBuilderShPtr roadmapBuilder,
 			       bool display=false);
 
+      /// Clear roadmaps of each problem
+      virtual void clearRoadmaps ();
+
       /// \brief Get roadmap builder of i-th problem.
       /// \param rank Rank of problem in Planner::problemVector_.
       /// \return shared pointer to roadmap builder.

include/hpp/core/problem.hh

@@ -132,9 +132,8 @@ namespace hpp {
       void roadmapBuilder ( CkwsRoadmapBuilderShPtr inroadmapBuilder );
       /// \brief Get roadmap building strategy.
       CkwsRoadmapBuilderShPtr roadmapBuilder() const;
-      /// \brief set roadmap.
-      /// \param inRoadmap shared pointer to a roadmap.
-      void roadmap ( CkwsRoadmapShPtr inRoadmap ) ;
+      /// Clear the roadmap.
+      void clearRoadmap ();
       /// \brief Get roadmap.
       /// \return shared pointer to the roadmap.
       CkwsRoadmapShPtr roadmap() const;
@@ -227,8 +226,6 @@ namespace hpp {
       CkwsConfigShPtr initConf_;
       /// \brief Shared pointer to goal configuration.
       std::vector <CkwsConfigShPtr> goalConfigurations_;
-      /// \brief Shared pointer to a roadmap associate to the robot
-      CkwsRoadmapShPtr roadmap_;
       /// \brief Shared pointer to a roadmapBuilder
       CkwsRoadmapBuilderShPtr roadmapBuilder_;
       /// \brief Shared pointer to a optimizer for the path

src/planner.cc

@@ -368,6 +368,14 @@ namespace hpp {
 
     // ======================================================================
 
+    void Planner::clearRoadmaps ()
+    {
+      for (unsigned int i=0; i < problemVector_.size (); ++i) {
+	problemVector_ [i].clearRoadmap ();
+      }
+    }
+    // ======================================================================
+
     ktStatus
     Planner::pathOptimizerIthProblem(unsigned int rank,
 				     CkwsPathPlannerShPtr pathOptimizer)

src/problem.cc

@@ -81,7 +81,6 @@ namespace hpp {
       robot_(inProblem.robot_),
       initConf_(inProblem.initConf_),
       goalConfigurations_(inProblem.goalConfigurations_),
-      roadmap_(inProblem.roadmap_),
       roadmapBuilder_(inProblem.roadmapBuilder_),
       pathOptimizer_(inProblem.pathOptimizer_),
       configExtractor_(inProblem.configExtractor_),
@@ -560,15 +559,19 @@ namespace hpp {
     CkwsRoadmapShPtr Problem::roadmap() const
     {
 
-      return roadmap_ ;
+      return roadmapBuilder_->roadmap ();
     }
 
     // ======================================================================
 
-    void Problem::roadmap ( CkwsRoadmapShPtr inRoadmap )
+    void Problem::clearRoadmap ()
     {
-
-      roadmap_ = inRoadmap ;
+      if (roadmapBuilder_) {
+	if (roadmapBuilder_->roadmap ()) {
+	  roadmapBuilder_->roadmap ()->clear ();
+	}
+	resetGoalConfig ();
+      }
     }
 
     // ======================================================================