ompl/geometric/planners/cforest/CForest.h
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2014, Rice University 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Rice University nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 /* Authors: Javier V. Gómez, Ioan Sucan, Mark Moll */ 00036 00037 #ifndef OMPL_GEOMETRIC_PLANNERS_CFOREST_CFOREST_ 00038 #define OMPL_GEOMETRIC_PLANNERS_CFOREST_CFOREST_ 00039 00040 00041 #include "ompl/geometric/planners/cforest/CForestStateSpaceWrapper.h" 00042 #include "ompl/geometric/planners/PlannerIncludes.h" 00043 #include "ompl/tools/config/SelfConfig.h" 00044 00045 #include <boost/thread.hpp> 00046 00047 #include <vector> 00048 00049 namespace ompl 00050 { 00051 00052 namespace geometric 00053 { 00054 00078 class CForest : public base::Planner 00079 { 00080 public: 00081 00082 CForest(const base::SpaceInformationPtr &si); 00083 00084 virtual ~CForest(); 00085 00086 virtual void getPlannerData(base::PlannerData &data) const; 00087 00088 virtual void clear(); 00089 00091 template <class T> 00092 void addPlannerInstance() 00093 { 00094 base::CForestStateSpaceWrapper *cfspace = new base::CForestStateSpaceWrapper(this, si_->getStateSpace().get()); 00095 base::StateSpacePtr space(cfspace); 00096 base::SpaceInformationPtr si(new base::SpaceInformation(space)); 00097 si->setStateValidityChecker(si_->getStateValidityChecker()); 00098 si->setMotionValidator(si_->getMotionValidator()); 00099 base::PlannerPtr planner(new T(si)); 00100 cfspace->setPlanner(planner.get()); 00101 addPlannerInstanceInternal(planner); 00102 } 00103 00105 template <class T> 00106 void addPlannerInstances(std::size_t num = 2) 00107 { 00108 planners_.reserve(planners_.size() + num); 00109 for (std::size_t i = 0 ; i < num; ++i) 00110 { 00111 addPlannerInstance<T>(); 00112 } 00113 } 00114 00116 void clearPlannerInstances() 00117 { 00118 planners_.clear(); 00119 } 00121 base::PlannerPtr& getPlannerInstance(const std::size_t idx) 00122 { 00123 return planners_[idx]; 00124 } 00125 00126 virtual void setup(); 00127 00128 virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc); 00129 00130 void addSampler(base::StateSamplerPtr sampler) 00131 { 00132 addSamplerMutex_.lock(); 00133 samplers_.push_back(sampler); 00134 addSamplerMutex_.unlock(); 00135 } 00136 00138 void setPrune(const bool prune) 00139 { 00140 prune_ = prune; 00141 } 00142 00144 bool getPrune() const 00145 { 00146 return prune_; 00147 } 00148 00150 void setNumThreads(unsigned int numThreads = 0); 00151 00153 unsigned int getNumThreads() 00154 { 00155 return numThreads_; 00156 } 00157 00159 std::string getBestCost() const; 00160 00162 std::string getNumPathsShared() const; 00163 00165 std::string getNumStatesShared() const; 00166 00167 private: 00168 00170 void addPlannerInstanceInternal(const base::PlannerPtr &planner); 00171 00173 void newSolutionFound(const base::Planner *planner, const std::vector<const base::State *> &states, const base::Cost cost); 00174 00175 protected: 00176 00178 void solve(base::Planner *planner, const base::PlannerTerminationCondition &ptc); 00179 00181 base::OptimizationObjectivePtr opt_; 00182 00184 std::vector<base::PlannerPtr> planners_; 00185 00187 std::vector<base::StateSamplerPtr> samplers_; 00188 00190 boost::unordered_set<const base::State *> statesShared_; 00191 00193 base::Cost bestCost_; 00194 00196 unsigned int numPathsShared_; 00197 00199 unsigned int numStatesShared_; 00200 00202 boost::mutex newSolutionFoundMutex_; 00203 00205 boost::mutex addSamplerMutex_; 00206 00208 bool prune_; 00209 00211 unsigned int numThreads_; 00212 }; 00213 } 00214 } 00215 00216 #endif