Commits

Mark Moll committed 3b51f53

moved allocation of default nearest neighbor data structure to SelfConfig class

Comments (0)

Files changed (12)

src/ompl/contrib/rrt_star/src/RRTstar.cpp

         ballRadiusConst_ = maxDistance_ * sqrt((double)si_->getStateSpace()->getDimension());
 
     if (!nn_)
-        nn_.reset(NearestNeighbors<Motion*>::getDefault(si_->getStateSpace()));
+        nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(si_->getStateSpace()));
     nn_->setDistanceFunction(boost::bind(&RRTstar::distanceFunction, this, _1, _2));
 }
 

src/ompl/control/planners/rrt/src/RRT.cpp

 
 #include "ompl/control/planners/rrt/RRT.h"
 #include "ompl/base/goals/GoalSampleableRegion.h"
+#include "ompl/tools/config/SelfConfig.h"
 #include <limits>
 
 ompl::control::RRT::RRT(const SpaceInformationPtr &si) : base::Planner(si, "RRT")
 {
     base::Planner::setup();
     if (!nn_)
-        nn_.reset(NearestNeighbors<Motion*>::getDefault(si_->getStateSpace()));
+        nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(si_->getStateSpace()));
     nn_->setDistanceFunction(boost::bind(&RRT::distanceFunction, this, _1, _2));
 }
 

src/ompl/control/planners/syclop/src/SyclopRRT.cpp

 
 #include "ompl/control/planners/syclop/SyclopRRT.h"
 #include "ompl/base/goals/GoalSampleableRegion.h"
+#include "ompl/tools/config/SelfConfig.h"
 
 void ompl::control::SyclopRRT::setup(void)
 {
     // the default regionalNN check from the discretization
     if (!nn_ && !regionalNN_)
     {
-        nn_.reset(NearestNeighbors<Motion*>::getDefault(si_->getStateSpace()));
+        nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(si_->getStateSpace()));
         nn_->setDistanceFunction(boost::bind(&SyclopRRT::distanceFunction, this, _1, _2));
     }
 }

src/ompl/datastructures/NearestNeighbors.h

         /** \brief Get all the elements in the datastructure */
         virtual void list(std::vector<_T> &data) const = 0;
 
-        /** \brief Select a default nearest neighbor datastructure for the given
-             space */
-        static NearestNeighbors<_T>* getDefault(const base::StateSpacePtr space);
-
     protected:
 
         /** \brief The used distance function */
     };
 }
 
-
-#include "ompl/datastructures/NearestNeighborsGNAT.h"
-#include "ompl/datastructures/NearestNeighborsSqrtApprox.h"
-
-namespace ompl
-{
-    template<typename _T>
-    NearestNeighbors<_T>* NearestNeighbors<_T>::getDefault(const base::StateSpacePtr space)
-    {
-        if (space->isMetricSpace())
-            return new NearestNeighborsGNAT<_T>();
-        else
-            return new NearestNeighborsSqrtApprox<_T>();
-    }
-}
-
 #endif

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

 #include "ompl/geometric/planners/prm/ConnectionStrategy.h"
 #include "ompl/base/goals/GoalSampleableRegion.h"
 #include "ompl/datastructures/PDF.h"
+#include "ompl/tools/config/SelfConfig.h"
 #include <boost/lambda/bind.hpp>
 #include <boost/graph/astar_search.hpp>
 #include <boost/graph/incremental_components.hpp>
 {
     Planner::setup();
     if (!nn_)
-        nn_.reset(NearestNeighbors<Vertex>::getDefault(si_->getStateSpace()));
+        nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(si_->getStateSpace()));
     nn_->setDistanceFunction(boost::bind(&PRM::distanceFunction, this, _1, _2));
     if (!connectionStrategy_)
     {

src/ompl/geometric/planners/rrt/src/LazyRRT.cpp

     sc.configurePlannerRange(maxDistance_);
 
     if (!nn_)
-        nn_.reset(NearestNeighbors<Motion*>::getDefault(si_->getStateSpace()));
+        nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(si_->getStateSpace()));
     nn_->setDistanceFunction(boost::bind(&LazyRRT::distanceFunction, this, _1, _2));
 }
 

src/ompl/geometric/planners/rrt/src/RRT.cpp

     sc.configurePlannerRange(maxDistance_);
 
     if (!nn_)
-        nn_.reset(NearestNeighbors<Motion*>::getDefault(si_->getStateSpace()));
+        nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(si_->getStateSpace()));
     nn_->setDistanceFunction(boost::bind(&RRT::distanceFunction, this, _1, _2));
 }
 

src/ompl/geometric/planners/rrt/src/RRTConnect.cpp

     sc.configurePlannerRange(maxDistance_);
 
     if (!tStart_)
-        tStart_.reset(NearestNeighbors<Motion*>::getDefault(si_->getStateSpace()));
+        tStart_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(si_->getStateSpace()));
     if (!tGoal_)
-        tGoal_.reset(NearestNeighbors<Motion*>::getDefault(si_->getStateSpace()));
+        tGoal_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(si_->getStateSpace()));
     tStart_->setDistanceFunction(boost::bind(&RRTConnect::distanceFunction, this, _1, _2));
     tGoal_->setDistanceFunction(boost::bind(&RRTConnect::distanceFunction, this, _1, _2));
 }

src/ompl/geometric/planners/rrt/src/TRRT.cpp

 
     // Create the nearest neighbor function the first time setup is run
     if (!nearestNeighbors_)
-        nearestNeighbors_.reset(NearestNeighbors<Motion*>::getDefault(si_->getStateSpace()));
+        nearestNeighbors_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(si_->getStateSpace()));
 
     // Set the distance function
     nearestNeighbors_->setDistanceFunction(boost::bind(&TRRT::distanceFunction, this, _1, _2));

src/ompl/geometric/planners/rrt/src/pRRT.cpp

     sc.configurePlannerRange(maxDistance_);
 
     if (!nn_)
-        nn_.reset(NearestNeighbors<Motion*>::getDefault(si_->getStateSpace()));
+        nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(si_->getStateSpace()));
     nn_->setDistanceFunction(boost::bind(&pRRT::distanceFunction, this, _1, _2));
 }
 

src/ompl/tools/config/SelfConfig.h

 
 #include "ompl/config.h"
 #include "ompl/base/SpaceInformation.h"
+#include "ompl/datastructures/NearestNeighborsSqrtApprox.h"
+#include "ompl/datastructures/NearestNeighborsGNAT.h"
 #include <iostream>
 #include <string>
 
             /** \brief Print the computed configuration parameters */
             void print(std::ostream &out = std::cout) const;
 
+            /** \brief Select a default nearest neighbor datastructure for the given
+                space */
+            template<typename _T>
+            static NearestNeighbors<_T>* getDefaultNearestNeighbors(
+                    const base::StateSpacePtr space)
+            {
+                if (space->isMetricSpace())
+                    return new NearestNeighborsGNAT<_T>();
+                else
+                    return new NearestNeighborsSqrtApprox<_T>();
+            }
+
         private:
 
             /// @cond IGNORE

tests/datastructures/nearestneighbors.cpp

 #include <boost/unordered_set.hpp>
 
 #include "ompl/config.h"
-#include "ompl/datastructures/NearestNeighbors.h"
+#include "ompl/datastructures/NearestNeighborsSqrtApprox.h"
+#include "ompl/datastructures/NearestNeighborsGNAT.h"
 #if OMPL_HAVE_FLANN
 #include "ompl/datastructures/NearestNeighborsFLANN.h"
 #endif