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
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines