fix bug on initialization of connection strategy + stub for roadmap e…

…xpansion

--HG--
rename : src/ompl/geometric/planners/prm/BasicPRM.h => src/ompl/geometric/planners/prm/PRM.h
rename : src/ompl/geometric/planners/prm/src/BasicPRM.cpp => src/ompl/geometric/planners/prm/src/PRM.cpp

py-bindings/headers_geometric.txt

@@ -1,7 +1,8 @@
 ../src/ompl/geometric/PathGeometric.h
 ../src/ompl/geometric/PathSimplifier.h
 ../src/ompl/geometric/SimpleSetup.h
-../src/ompl/geometric/planners/prm/BasicPRM.h
+../src/ompl/geometric/planners/prm/ConnectionStrategy.h
+../src/ompl/geometric/planners/prm/PRM.h
 ../src/ompl/geometric/planners/est/EST.h
 ../src/ompl/geometric/planners/kpiece/KPIECE1.h
 ../src/ompl/geometric/planners/kpiece/BKPIECE1.h

src/ompl/geometric/planners/prm/ConnectionStrategy.h

@@ -70,6 +70,12 @@ namespace ompl
             {
             }
 
+            /** \brief Set the nearest neighbors datastructure to use */
+            void setNearestNeighbors(const boost::shared_ptr< NearestNeighbors<Milestone> > &nn)
+            {
+                nn_ = nn;
+            }
+
             /** \brief Given a milestone \e m, find the nearest
                 neighbors attempts of connection should be made to,
                 according to the connection strategy */

src/ompl/geometric/planners/prm/BasicPRM.h → src/ompl/geometric/planners/prm/PRM.h

@@ -54,7 +54,7 @@ namespace ompl
     {
 
         /**
-           @anchor gBasicPRM
+           @anchor gPRM
 
            @par Short description
            PRM is a planner that constructs a roadmap of milestones
@@ -76,7 +76,7 @@ namespace ompl
         */
 
         /** \brief Probabilistic RoadMap planner */
-        class BasicPRM : public base::Planner
+        class PRM : public base::Planner
         {
         public:
 
@@ -130,9 +130,9 @@ namespace ompl
             typedef boost::function2<bool, const Vertex&, const Vertex&> ConnectionFilter;
 
             /** \brief Constructor */
-            BasicPRM(const base::SpaceInformationPtr &si);
+            PRM(const base::SpaceInformationPtr &si);
 
-            virtual ~BasicPRM(void)
+            virtual ~PRM(void)
             {
                 freeMemory();
             }
@@ -160,6 +160,11 @@ namespace ompl
                 method will also improve the roadmap, as needed.*/
             virtual void growRoadmap(double growTime);
 
+            /** \brief Attempt to connect disjoint components in the
+                roadmap using random bounding motions (the PRM
+                expansion step) */
+            virtual void expandRoadmap(double expandTime);
+
             virtual bool solve(const base::PlannerTerminationCondition &ptc);
 
             virtual void clear(void);

src/ompl/geometric/planners/prm/src/BasicPRM.cpp → src/ompl/geometric/planners/prm/src/PRM.cpp

@@ -34,7 +34,7 @@
 
 /* Author: Ioan Sucan, James D. Marble */
 
-#include "ompl/geometric/planners/prm/BasicPRM.h"
+#include "ompl/geometric/planners/prm/PRM.h"
 #include "ompl/geometric/planners/prm/ConnectionStrategy.h"
 #include "ompl/base/GoalSampleableRegion.h"
 #include "ompl/datastructures/NearestNeighborsGNAT.h"
@@ -53,7 +53,7 @@
 static const unsigned int FIND_VALID_STATE_ATTEMPTS_WITHOUT_TIME_CHECK = 2;
 
 
-ompl::geometric::BasicPRM::BasicPRM(const base::SpaceInformationPtr &si) :
+ompl::geometric::PRM::PRM(const base::SpaceInformationPtr &si) :
     base::Planner(si, "PRM"),
     stateProperty_(boost::get(vertex_state_t(), g_)),
     weightProperty_(boost::get(boost::edge_weight, g_)),
@@ -63,26 +63,28 @@ ompl::geometric::BasicPRM::BasicPRM(const base::SpaceInformationPtr &si) :
     maxEdgeID_(0)
 {
     specs_.recognizedGoal = base::GOAL_SAMPLEABLE_REGION;
-    connectionStrategy_ = KStrategy<Vertex>(10, nn_);
-    connectionFilter_ = boost::lambda::constant(true);
 }
 
-void ompl::geometric::BasicPRM::setup(void)
+void ompl::geometric::PRM::setup(void)
 {
     Planner::setup();
     if (!nn_)
-        nn_.reset(new NearestNeighborsGNAT<Vertex>());
-    nn_->setDistanceFunction(boost::bind(&BasicPRM::distanceFunction, this, _1, _2));
+            nn_.reset(new NearestNeighborsGNAT<Vertex>());
+    nn_->setDistanceFunction(boost::bind(&PRM::distanceFunction, this, _1, _2));
+    if (!connectionStrategy_)
+        connectionStrategy_ = KStrategy<Vertex>(10, nn_);
+    if (!connectionFilter_)
+        connectionFilter_ = boost::lambda::constant(true);
 }
 
-void ompl::geometric::BasicPRM::setProblemDefinition(const base::ProblemDefinitionPtr &pdef)
+void ompl::geometric::PRM::setProblemDefinition(const base::ProblemDefinitionPtr &pdef)
 {
     Planner::setProblemDefinition(pdef);
     startM_.clear();
     goalM_.clear();
 }
 
-void ompl::geometric::BasicPRM::clear(void)
+void ompl::geometric::PRM::clear(void)
 {
     Planner::clear();
     sampler_.reset();
@@ -94,15 +96,22 @@ void ompl::geometric::BasicPRM::clear(void)
     maxEdgeID_ = 0;
 }
 
-void ompl::geometric::BasicPRM::freeMemory(void)
+void ompl::geometric::PRM::freeMemory(void)
 {
     foreach (Vertex v, boost::vertices(g_))
         si_->freeState(stateProperty_[v]);
     g_.clear();
 }
 
-void ompl::geometric::BasicPRM::growRoadmap(double growTime)
+void ompl::geometric::PRM::expandRoadmap(double expandTime)
 {
+}
+
+void ompl::geometric::PRM::growRoadmap(double growTime)
+{
+    if (!isSetup())
+        setup();
+
     time::point endTime = time::now() + time::seconds(growTime);
     base::State *workState = si_->allocState();
     while (time::now() < endTime)
@@ -125,7 +134,7 @@ void ompl::geometric::BasicPRM::growRoadmap(double growTime)
     si_->freeState(workState);
 }
 
-void ompl::geometric::BasicPRM::growRoadmap(const std::vector<Vertex> &start,
+void ompl::geometric::PRM::growRoadmap(const std::vector<Vertex> &start,
                                             const std::vector<Vertex> &goal,
                                             const base::PlannerTerminationCondition &ptc,
                                             base::State *workState)
@@ -153,7 +162,7 @@ void ompl::geometric::BasicPRM::growRoadmap(const std::vector<Vertex> &start,
     }
 }
 
-bool ompl::geometric::BasicPRM::haveSolution(const std::vector<Vertex> &starts, const std::vector<Vertex> &goals, std::pair<Vertex, Vertex> *endpoints)
+bool ompl::geometric::PRM::haveSolution(const std::vector<Vertex> &starts, const std::vector<Vertex> &goals, std::pair<Vertex, Vertex> *endpoints)
 {
     base::Goal *g = pdef_->getGoal().get();
     foreach (Vertex start, starts)
@@ -174,7 +183,7 @@ bool ompl::geometric::BasicPRM::haveSolution(const std::vector<Vertex> &starts,
     return false;
 }
 
-bool ompl::geometric::BasicPRM::solve(const base::PlannerTerminationCondition &ptc)
+bool ompl::geometric::PRM::solve(const base::PlannerTerminationCondition &ptc)
 {
     checkValidity();
     base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
@@ -256,7 +265,7 @@ bool ompl::geometric::BasicPRM::solve(const base::PlannerTerminationCondition &p
     return goal->isAchieved();
 }
 
-ompl::geometric::BasicPRM::Vertex ompl::geometric::BasicPRM::addMilestone(base::State *state)
+ompl::geometric::PRM::Vertex ompl::geometric::PRM::addMilestone(base::State *state)
 {
     Vertex m = boost::add_vertex(g_);
     stateProperty_[m] = state;
@@ -286,19 +295,19 @@ ompl::geometric::BasicPRM::Vertex ompl::geometric::BasicPRM::addMilestone(base::
     return m;
 }
 
-void ompl::geometric::BasicPRM::uniteComponents(Vertex m1, Vertex m2)
+void ompl::geometric::PRM::uniteComponents(Vertex m1, Vertex m2)
 {
     disjointSets_.union_set(m1, m2);
 }
 
-void ompl::geometric::BasicPRM::constructSolution(const Vertex start, const Vertex goal)
+void ompl::geometric::PRM::constructSolution(const Vertex start, const Vertex goal)
 {
     PathGeometric *p = new PathGeometric(si_);
 
     boost::vector_property_map<Vertex> prev(boost::num_vertices(g_));
 
     boost::astar_search(g_, start,
-            boost::bind(&BasicPRM::distanceFunction, this, _1, goal),
+            boost::bind(&PRM::distanceFunction, this, _1, goal),
             boost::predecessor_map(prev));
 
     if (prev[goal] == goal)
@@ -312,7 +321,7 @@ void ompl::geometric::BasicPRM::constructSolution(const Vertex start, const Vert
     pdef_->getGoal()->setSolutionPath(base::PathPtr(p));
 }
 
-void ompl::geometric::BasicPRM::getPlannerData(base::PlannerData &data) const
+void ompl::geometric::PRM::getPlannerData(base::PlannerData &data) const
 {
     Planner::getPlannerData(data);
 

tests/geometric/2dmap/2dmap.cpp

@@ -51,7 +51,7 @@
 #include "ompl/geometric/planners/rrt/pRRT.h"
 #include "ompl/geometric/planners/rrt/LazyRRT.h"
 #include "ompl/geometric/planners/est/EST.h"
-#include "ompl/geometric/planners/prm/BasicPRM.h"
+#include "ompl/geometric/planners/prm/PRM.h"
 
 #include "../../base/PlannerTest.h"
 
@@ -351,13 +351,13 @@ class ESTTest : public TestPlanner
 
 };
 
-class BasicPRMTest : public TestPlanner
+class PRMTest : public TestPlanner
 {
 protected:
 
     base::PlannerPtr newPlanner(const base::SpaceInformationPtr &si)
     {
-        geometric::BasicPRM *prm = new geometric::BasicPRM(si);
+        geometric::PRM *prm = new geometric::PRM(si);
         return base::PlannerPtr(prm);
     }
 
@@ -598,15 +598,15 @@ TEST_F(PlanTest, geometric_LazyRRT)
     EXPECT_TRUE(avglength < 100.0);
 }
 
-TEST_F(PlanTest, geometric_BasicPRM)
+TEST_F(PlanTest, geometric_PRM)
 {
     double success    = 0.0;
     double avgruntime = 0.0;
     double avglength  = 0.0;
 
     simpleTest();
 
-    TestPlanner *p = new BasicPRMTest();
+    TestPlanner *p = new PRMTest();
     runPlanTest(p, &success, &avgruntime, &avglength);
     delete p;