00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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 <boost/function.hpp>
00050 #include <boost/concept_check.hpp>
00051 #include <boost/noncopyable.hpp>
00052 #include <boost/lexical_cast.hpp>
00053 #include <string>
00054 #include <map>
00055
00056 namespace ompl
00057 {
00058
00059 namespace base
00060 {
00061
00063
00064 ClassForward(Planner);
00066
00082 class PlannerInputStates
00083 {
00084 public:
00085
00087 PlannerInputStates(const PlannerPtr &planner) : planner_(planner.get())
00088 {
00089 tempState_ = NULL;
00090 update();
00091 }
00092
00094 PlannerInputStates(const Planner *planner) : planner_(planner)
00095 {
00096 tempState_ = NULL;
00097 update();
00098 }
00099
00103 PlannerInputStates(void) : planner_(NULL)
00104 {
00105 tempState_ = NULL;
00106 clear();
00107 }
00108
00110 ~PlannerInputStates(void)
00111 {
00112 clear();
00113 }
00114
00116 void clear(void);
00117
00121 void restart(void);
00122
00128 bool update(void);
00129
00138 bool use(const SpaceInformationPtr &si, const ProblemDefinitionPtr &pdef);
00139
00148 bool use(const SpaceInformation *si, const ProblemDefinition *pdef);
00149
00152 void checkValidity(void) const;
00153
00156 const State* nextStart(void);
00157
00166 const State* nextGoal(const PlannerTerminationCondition &ptc);
00167
00169 const State* nextGoal(void);
00170
00172 bool haveMoreStartStates(void) const;
00173
00175 bool haveMoreGoalStates(void) const;
00176
00180 unsigned int getSeenStartStatesCount(void) const
00181 {
00182 return addedStartStates_;
00183 }
00184
00186 unsigned int getSampledGoalsCount(void) const
00187 {
00188 return sampledGoalsCount_;
00189 }
00190
00191 private:
00192
00193 const Planner *planner_;
00194
00195 unsigned int addedStartStates_;
00196 unsigned int sampledGoalsCount_;
00197 State *tempState_;
00198
00199 const ProblemDefinition *pdef_;
00200 const SpaceInformation *si_;
00201 };
00202
00204 struct PlannerSpecs
00205 {
00206 PlannerSpecs(void) : recognizedGoal(GOAL_ANY), multithreaded(false), approximateSolutions(false), optimizingPaths(false), directed(false), provingSolutionNonExistence(false)
00207 {
00208 }
00209
00211 GoalType recognizedGoal;
00212
00214 bool multithreaded;
00215
00217 bool approximateSolutions;
00218
00221 bool optimizingPaths;
00222
00225 bool directed;
00226
00228 bool provingSolutionNonExistence;
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(void)
00242 {
00243 }
00244
00246 template<class T>
00247 T* as(void)
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(void) const
00258 {
00260 BOOST_CONCEPT_ASSERT((boost::Convertible<T*, Planner*>));
00261
00262 return static_cast<const T*>(this);
00263 }
00264
00266 const SpaceInformationPtr& getSpaceInformation(void) const;
00267
00269 const ProblemDefinitionPtr& getProblemDefinition(void) const;
00270
00272 const PlannerInputStates& getPlannerInputStates(void) 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(void);
00307
00314 virtual void getPlannerData(PlannerData &data) const;
00315
00317 const std::string& getName(void) const;
00318
00320 void setName(const std::string &name);
00321
00323 const PlannerSpecs& getSpecs(void) const;
00324
00329 virtual void setup(void);
00330
00335 virtual void checkValidity(void);
00336
00338 bool isSetup(void) const;
00339
00341 ParamSet& params(void)
00342 {
00343 return params_;
00344 }
00345
00347 const ParamSet& params(void) const
00348 {
00349 return params_;
00350 }
00351
00353 virtual void printProperties(std::ostream &out) const;
00354
00356 virtual void printSettings(std::ostream &out) const;
00357
00358 protected:
00359
00361 template<typename T, typename PlannerType, typename SetterType, typename GetterType>
00362 void declareParam(const std::string &name, const PlannerType &planner, const SetterType& setter, const GetterType& getter)
00363 {
00364 params_.declareParam<T>(name, boost::bind(setter, planner, _1), boost::bind(getter, planner));
00365 }
00366
00368 template<typename T, typename PlannerType, typename SetterType>
00369 void declareParam(const std::string &name, const PlannerType &planner, const SetterType& setter)
00370 {
00371 params_.declareParam<T>(name, boost::bind(setter, planner, _1));
00372 }
00373
00375 SpaceInformationPtr si_;
00376
00378 ProblemDefinitionPtr pdef_;
00379
00381 PlannerInputStates pis_;
00382
00384 std::string name_;
00385
00387 PlannerSpecs specs_;
00388
00390 ParamSet params_;
00391
00393 bool setup_;
00394 };
00395
00397 typedef boost::function<PlannerPtr(const SpaceInformationPtr&)> PlannerAllocator;
00398 }
00399 }
00400
00401 #endif