ompl/base/Planner.h
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2010, 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 /* Author: Ioan Sucan */ 00036 00037 #ifndef OMPL_BASE_PLANNER_ 00038 #define OMPL_BASE_PLANNER_ 00039 00040 #include "ompl/base/SpaceInformation.h" 00041 #include "ompl/base/ProblemDefinition.h" 00042 #include "ompl/base/PlannerData.h" 00043 #include "ompl/base/PlannerStatus.h" 00044 #include "ompl/base/PlannerTerminationCondition.h" 00045 #include "ompl/base/GenericParam.h" 00046 #include "ompl/util/Console.h" 00047 #include "ompl/util/Time.h" 00048 #include "ompl/util/ClassForward.h" 00049 #include "ompl/util/Deprecation.h" 00050 #include <boost/function.hpp> 00051 #include <boost/concept_check.hpp> 00052 #include <boost/noncopyable.hpp> 00053 #include <boost/lexical_cast.hpp> 00054 #include <string> 00055 #include <map> 00056 00057 namespace ompl 00058 { 00059 00060 namespace base 00061 { 00062 00064 00065 OMPL_CLASS_FORWARD(Planner); 00067 00083 class PlannerInputStates 00084 { 00085 public: 00086 00088 PlannerInputStates(const PlannerPtr &planner) : planner_(planner.get()) 00089 { 00090 tempState_ = NULL; 00091 update(); 00092 } 00093 00095 PlannerInputStates(const Planner *planner) : planner_(planner) 00096 { 00097 tempState_ = NULL; 00098 update(); 00099 } 00100 00104 PlannerInputStates() : planner_(NULL) 00105 { 00106 tempState_ = NULL; 00107 clear(); 00108 } 00109 00111 ~PlannerInputStates() 00112 { 00113 clear(); 00114 } 00115 00117 void clear(); 00118 00122 void restart(); 00123 00129 bool update(); 00130 00136 bool use(const ProblemDefinitionPtr &pdef); 00137 00143 bool use(const ProblemDefinition *pdef); 00144 00147 void checkValidity() const; 00148 00151 const State* nextStart(); 00152 00161 const State* nextGoal(const PlannerTerminationCondition &ptc); 00162 00164 const State* nextGoal(); 00165 00167 bool haveMoreStartStates() const; 00168 00170 bool haveMoreGoalStates() const; 00171 00175 unsigned int getSeenStartStatesCount() const 00176 { 00177 return addedStartStates_; 00178 } 00179 00181 unsigned int getSampledGoalsCount() const 00182 { 00183 return sampledGoalsCount_; 00184 } 00185 00186 private: 00187 00188 const Planner *planner_; 00189 00190 unsigned int addedStartStates_; 00191 unsigned int sampledGoalsCount_; 00192 State *tempState_; 00193 00194 const ProblemDefinition *pdef_; 00195 const SpaceInformation *si_; 00196 }; 00197 00199 struct PlannerSpecs 00200 { 00201 PlannerSpecs() : recognizedGoal(GOAL_ANY), multithreaded(false), approximateSolutions(false), 00202 optimizingPaths(false), directed(false), provingSolutionNonExistence(false), 00203 canReportIntermediateSolutions(false) 00204 { 00205 } 00206 00208 GoalType recognizedGoal; 00209 00211 bool multithreaded; 00212 00214 bool approximateSolutions; 00215 00218 bool optimizingPaths; 00219 00222 bool directed; 00223 00225 bool provingSolutionNonExistence; 00226 00228 bool canReportIntermediateSolutions; 00229 }; 00230 00232 class Planner : private boost::noncopyable 00233 { 00234 00235 public: 00236 00238 Planner(const SpaceInformationPtr &si, const std::string &name); 00239 00241 virtual ~Planner() 00242 { 00243 } 00244 00246 template<class T> 00247 T* as() 00248 { 00250 BOOST_CONCEPT_ASSERT((boost::Convertible<T*, Planner*>)); 00251 00252 return static_cast<T*>(this); 00253 } 00254 00256 template<class T> 00257 const T* as() const 00258 { 00260 BOOST_CONCEPT_ASSERT((boost::Convertible<T*, Planner*>)); 00261 00262 return static_cast<const T*>(this); 00263 } 00264 00266 const SpaceInformationPtr& getSpaceInformation() const; 00267 00269 const ProblemDefinitionPtr& getProblemDefinition() const; 00270 00272 const PlannerInputStates& getPlannerInputStates() const; 00273 00278 virtual void setProblemDefinition(const ProblemDefinitionPtr &pdef); 00279 00292 virtual PlannerStatus solve(const PlannerTerminationCondition &ptc) = 0; 00293 00296 PlannerStatus solve(const PlannerTerminationConditionFn &ptc, double checkInterval); 00297 00301 PlannerStatus solve(double solveTime); 00302 00306 virtual void clear(); 00307 00314 virtual void getPlannerData(PlannerData &data) const; 00315 00317 const std::string& getName() const; 00318 00320 void setName(const std::string &name); 00321 00323 const PlannerSpecs& getSpecs() const; 00324 00329 virtual void setup(); 00330 00335 virtual void checkValidity(); 00336 00338 bool isSetup() const; 00339 00341 ParamSet& params() 00342 { 00343 return params_; 00344 } 00345 00347 const ParamSet& params() const 00348 { 00349 return params_; 00350 } 00351 00353 typedef boost::function<std::string ()> PlannerProgressProperty; 00354 00356 typedef std::map<std::string, PlannerProgressProperty> PlannerProgressProperties; 00357 00359 const PlannerProgressProperties& getPlannerProgressProperties() const 00360 { 00361 return plannerProgressProperties_; 00362 } 00363 00365 virtual void printProperties(std::ostream &out) const; 00366 00368 virtual void printSettings(std::ostream &out) const; 00369 00370 protected: 00371 00373 template<typename T, typename PlannerType, typename SetterType, typename GetterType> 00374 void declareParam(const std::string &name, const PlannerType &planner, const SetterType& setter, const GetterType& getter, const std::string &rangeSuggestion = "") 00375 { 00376 params_.declareParam<T>(name, boost::bind(setter, planner, _1), boost::bind(getter, planner)); 00377 if (!rangeSuggestion.empty()) 00378 params_[name].setRangeSuggestion(rangeSuggestion); 00379 } 00380 00382 template<typename T, typename PlannerType, typename SetterType> 00383 void declareParam(const std::string &name, const PlannerType &planner, const SetterType& setter, const std::string &rangeSuggestion = "") 00384 { 00385 params_.declareParam<T>(name, boost::bind(setter, planner, _1)); 00386 if (!rangeSuggestion.empty()) 00387 params_[name].setRangeSuggestion(rangeSuggestion); 00388 } 00389 00391 void addPlannerProgressProperty(const std::string& progressPropertyName, const PlannerProgressProperty& prop) 00392 { 00393 plannerProgressProperties_[progressPropertyName] = prop; 00394 } 00395 00397 SpaceInformationPtr si_; 00398 00400 ProblemDefinitionPtr pdef_; 00401 00403 PlannerInputStates pis_; 00404 00406 std::string name_; 00407 00409 PlannerSpecs specs_; 00410 00412 ParamSet params_; 00413 00415 PlannerProgressProperties plannerProgressProperties_; 00416 00418 bool setup_; 00419 }; 00420 00422 typedef boost::function<PlannerPtr(const SpaceInformationPtr&)> PlannerAllocator; 00423 } 00424 } 00425 00426 #endif