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/PlannerTerminationCondition.h"
00044 #include "ompl/base/GenericParam.h"
00045 #include "ompl/util/Console.h"
00046 #include "ompl/util/Time.h"
00047 #include "ompl/util/ClassForward.h"
00048 #include <boost/function.hpp>
00049 #include <boost/concept_check.hpp>
00050 #include <boost/noncopyable.hpp>
00051 #include <boost/lexical_cast.hpp>
00052 #include <string>
00053 #include <map>
00054
00055 namespace ompl
00056 {
00057
00058 namespace base
00059 {
00060
00062
00063 ClassForward(Planner);
00065
00081 class PlannerInputStates
00082 {
00083 public:
00084
00086 PlannerInputStates(const PlannerPtr &planner) : planner_(planner.get())
00087 {
00088 tempState_ = NULL;
00089 update();
00090 }
00091
00093 PlannerInputStates(const Planner *planner) : planner_(planner)
00094 {
00095 tempState_ = NULL;
00096 update();
00097 }
00098
00102 PlannerInputStates(void) : planner_(NULL)
00103 {
00104 tempState_ = NULL;
00105 clear();
00106 }
00107
00109 ~PlannerInputStates(void)
00110 {
00111 clear();
00112 }
00113
00115 void clear(void);
00116
00120 void restart(void);
00121
00127 bool update(void);
00128
00137 bool use(const SpaceInformationPtr &si, const ProblemDefinitionPtr &pdef);
00138
00147 bool use(const SpaceInformation *si, const ProblemDefinition *pdef);
00148
00151 void checkValidity(void) const;
00152
00155 const State* nextStart(void);
00156
00165 const State* nextGoal(const PlannerTerminationCondition &ptc);
00166
00168 const State* nextGoal(void);
00169
00171 bool haveMoreStartStates(void) const;
00172
00174 bool haveMoreGoalStates(void) const;
00175
00179 unsigned int getSeenStartStatesCount(void) const
00180 {
00181 return addedStartStates_;
00182 }
00183
00185 unsigned int getSampledGoalsCount(void) const
00186 {
00187 return sampledGoalsCount_;
00188 }
00189
00190 private:
00191
00192 const Planner *planner_;
00193
00194 unsigned int addedStartStates_;
00195 unsigned int sampledGoalsCount_;
00196 State *tempState_;
00197
00198 const ProblemDefinition *pdef_;
00199 const SpaceInformation *si_;
00200 };
00201
00203 struct PlannerSpecs
00204 {
00205 PlannerSpecs(void) : recognizedGoal(GOAL_ANY), multithreaded(false), approximateSolutions(false), optimizingPaths(false)
00206 {
00207 }
00208
00210 GoalType recognizedGoal;
00211
00213 bool multithreaded;
00214
00216 bool approximateSolutions;
00217
00220 bool optimizingPaths;
00221 };
00222
00224 class Planner : private boost::noncopyable
00225 {
00226
00227 public:
00228
00230 Planner(const SpaceInformationPtr &si, const std::string &name);
00231
00233 virtual ~Planner(void)
00234 {
00235 }
00236
00238 template<class T>
00239 T* as(void)
00240 {
00242 BOOST_CONCEPT_ASSERT((boost::Convertible<T*, Planner*>));
00243
00244 return static_cast<T*>(this);
00245 }
00246
00248 template<class T>
00249 const T* as(void) const
00250 {
00252 BOOST_CONCEPT_ASSERT((boost::Convertible<T*, Planner*>));
00253
00254 return static_cast<const T*>(this);
00255 }
00256
00258 const SpaceInformationPtr& getSpaceInformation(void) const;
00259
00261 const ProblemDefinitionPtr& getProblemDefinition(void) const;
00262
00264 const PlannerInputStates& getPlannerInputStates(void) const;
00265
00270 virtual void setProblemDefinition(const ProblemDefinitionPtr &pdef);
00271
00284 virtual bool solve(const PlannerTerminationCondition &ptc) = 0;
00285
00288 bool solve(const PlannerTerminationConditionFn &ptc, double checkInterval);
00289
00293 bool solve(double solveTime);
00294
00298 virtual void clear(void);
00299
00306 virtual void getPlannerData(PlannerData &data) const;
00307
00309 const std::string& getName(void) const;
00310
00312 void setName(const std::string &name);
00313
00315 const PlannerSpecs& getSpecs(void) const;
00316
00321 virtual void setup(void);
00322
00327 virtual void checkValidity(void);
00328
00330 bool isSetup(void) const;
00331
00333 ParamSet& params(void)
00334 {
00335 return params_;
00336 }
00337
00339 const ParamSet& params(void) const
00340 {
00341 return params_;
00342 }
00343
00345 virtual void printProperties(std::ostream &out) const;
00346
00348 virtual void printSettings(std::ostream &out) const;
00349
00350 protected:
00351
00353 template<typename T, typename PlannerType, typename SetterType, typename GetterType>
00354 void declareParam(const std::string &name, const PlannerType &planner, const SetterType& setter, const GetterType& getter)
00355 {
00356 params_.declareParam<T>(name, msg_, boost::bind(setter, planner, _1), boost::bind(getter, planner));
00357 }
00358
00360 template<typename T, typename PlannerType, typename SetterType>
00361 void declareParam(const std::string &name, const PlannerType &planner, const SetterType& setter)
00362 {
00363 params_.declareParam<T>(name, msg_, boost::bind(setter, planner, _1));
00364 }
00365
00367 SpaceInformationPtr si_;
00368
00370 ProblemDefinitionPtr pdef_;
00371
00373 PlannerInputStates pis_;
00374
00376 std::string name_;
00377
00379 PlannerSpecs specs_;
00380
00382 ParamSet params_;
00383
00385 bool setup_;
00386
00388 msg::Interface msg_;
00389 };
00390
00392 typedef boost::function<PlannerPtr(const SpaceInformationPtr&)> PlannerAllocator;
00393 }
00394 }
00395
00396 #endif