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/util/Console.h"
00045 #include "ompl/util/Time.h"
00046 #include "ompl/util/ClassForward.h"
00047 #include <boost/function.hpp>
00048 #include <boost/concept_check.hpp>
00049 #include <boost/noncopyable.hpp>
00050 #include <string>
00051
00052 namespace ompl
00053 {
00054
00055 namespace base
00056 {
00057
00061 enum PlannerType
00062 {
00064 PLAN_UNKNOWN = 0,
00065
00067 PLAN_TO_GOAL_STATE = 1,
00068
00070 PLAN_TO_GOAL_STATES = 2,
00071
00073 PLAN_TO_GOAL_SAMPLEABLE_REGION = 4 | PLAN_TO_GOAL_STATES | PLAN_TO_GOAL_STATE,
00074
00076 PLAN_TO_GOAL_REGION = 8 | PLAN_TO_GOAL_SAMPLEABLE_REGION,
00077
00079 PLAN_TO_GOAL_ANY = 32768 | PLAN_TO_GOAL_REGION
00080 };
00081
00083 ClassForward(Planner);
00084
00100 class PlannerInputStates
00101 {
00102 public:
00103
00105 PlannerInputStates(const PlannerPtr &planner) : planner_(planner.get())
00106 {
00107 tempState_ = NULL;
00108 update();
00109 }
00110
00112 PlannerInputStates(const Planner *planner) : planner_(planner)
00113 {
00114 tempState_ = NULL;
00115 update();
00116 }
00117
00121 PlannerInputStates(void) : planner_(NULL)
00122 {
00123 tempState_ = NULL;
00124 clear();
00125 }
00126
00128 ~PlannerInputStates(void)
00129 {
00130 clear();
00131 }
00132
00134 void clear(void);
00135
00141 bool update(void);
00142
00151 bool use(const SpaceInformationPtr &si, const ProblemDefinitionPtr &pdef);
00152
00161 bool use(const SpaceInformation *si, const ProblemDefinition *pdef);
00162
00165 void checkValidity(void) const;
00166
00169 const State* nextStart(void);
00170
00179 const State* nextGoal(const PlannerTerminationCondition &ptc);
00180
00182 const State* nextGoal(void);
00183
00185 bool haveMoreStartStates(void) const;
00186
00188 bool haveMoreGoalStates(void) const;
00189
00193 unsigned int getSeenStartStatesCount(void) const
00194 {
00195 return addedStartStates_;
00196 }
00197
00199 unsigned int getSampledGoalsCount(void) const
00200 {
00201 return sampledGoalsCount_;
00202 }
00203
00204 private:
00205
00206 const Planner *planner_;
00207
00208 unsigned int addedStartStates_;
00209 unsigned int sampledGoalsCount_;
00210 State *tempState_;
00211
00212 const ProblemDefinition *pdef_;
00213 const SpaceInformation *si_;
00214 };
00215
00217 class Planner : private boost::noncopyable
00218 {
00219
00220 public:
00221
00223 Planner(const SpaceInformationPtr &si, const std::string &name);
00224
00226 virtual ~Planner(void)
00227 {
00228 }
00229
00231 template<class T>
00232 T* as(void)
00233 {
00235 BOOST_CONCEPT_ASSERT((boost::Convertible<T*, Planner*>));
00236
00237 return static_cast<T*>(this);
00238 }
00239
00241 template<class T>
00242 const T* as(void) const
00243 {
00245 BOOST_CONCEPT_ASSERT((boost::Convertible<T*, Planner*>));
00246
00247 return static_cast<const T*>(this);
00248 }
00249
00251 const SpaceInformationPtr& getSpaceInformation(void) const;
00252
00254 const ProblemDefinitionPtr& getProblemDefinition(void) const;
00255
00257 const PlannerInputStates& getPlannerInputStates(void) const;
00258
00263 virtual void setProblemDefinition(const ProblemDefinitionPtr &pdef);
00264
00277 virtual bool solve(const PlannerTerminationCondition &ptc) = 0;
00278
00281 bool solve(const PlannerTerminationConditionFn &ptc, double checkInterval);
00282
00286 bool solve(double solveTime);
00287
00291 virtual void clear(void);
00292
00299 virtual void getPlannerData(PlannerData &data) const;
00300
00302 const std::string& getName(void) const;
00303
00305 void setName(const std::string &name);
00306
00310 PlannerType getType(void) const;
00311
00316 virtual void setup(void);
00317
00322 virtual void checkValidity(void);
00323
00325 bool isSetup(void) const;
00326
00327 protected:
00328
00330 SpaceInformationPtr si_;
00331
00333 ProblemDefinitionPtr pdef_;
00334
00336 PlannerInputStates pis_;
00337
00339 std::string name_;
00340
00342 PlannerType type_;
00343
00345 bool setup_;
00346
00348 msg::Interface msg_;
00349 };
00350
00352 typedef boost::function1<PlannerPtr, const SpaceInformationPtr&> PlannerAllocator;
00353 }
00354 }
00355
00356 #endif