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/util/Console.h"
00044 #include "ompl/util/Time.h"
00045 #include "ompl/util/ClassForward.h"
00046 #include <boost/function.hpp>
00047 #include <boost/concept_check.hpp>
00048 #include <boost/noncopyable.hpp>
00049 #include <string>
00050
00051 namespace ompl
00052 {
00053
00054 namespace base
00055 {
00056
00060 enum PlannerType
00061 {
00063 PLAN_UNKNOWN = 0,
00064
00066 PLAN_TO_GOAL_STATE = 1,
00067
00069 PLAN_TO_GOAL_STATES = 2,
00070
00072 PLAN_TO_GOAL_SAMPLEABLE_REGION = 4 | PLAN_TO_GOAL_STATES | PLAN_TO_GOAL_STATE,
00073
00075 PLAN_TO_GOAL_REGION = 8 | PLAN_TO_GOAL_SAMPLEABLE_REGION,
00076
00078 PLAN_TO_GOAL_ANY = 32768 | PLAN_TO_GOAL_REGION
00079 };
00080
00088 typedef boost::function0<bool> PlannerTerminationCondition;
00089
00090
00092 ClassForward(Planner);
00093
00109 class PlannerInputStates
00110 {
00111 public:
00112
00114 PlannerInputStates(const PlannerPtr &planner) : planner_(planner.get())
00115 {
00116 tempState_ = NULL;
00117 update();
00118 }
00119
00121 PlannerInputStates(const Planner *planner) : planner_(planner)
00122 {
00123 tempState_ = NULL;
00124 update();
00125 }
00126
00130 PlannerInputStates(void) : planner_(NULL)
00131 {
00132 tempState_ = NULL;
00133 clear();
00134 }
00135
00137 ~PlannerInputStates(void)
00138 {
00139 clear();
00140 }
00141
00143 void clear(void);
00144
00150 bool update(void);
00151
00160 bool use(const SpaceInformationPtr &si, const ProblemDefinitionPtr &pdef);
00161
00170 bool use(const SpaceInformation *si, const ProblemDefinition *pdef);
00171
00174 void checkValidity(void) const;
00175
00178 const State* nextStart(void);
00179
00188 const State* nextGoal(const PlannerTerminationCondition &ptc);
00189
00191 const State* nextGoal(void);
00192
00194 bool haveMoreStartStates(void) const;
00195
00197 bool haveMoreGoalStates(void) const;
00198
00202 unsigned int getSeenStartStatesCount(void) const
00203 {
00204 return addedStartStates_;
00205 }
00206
00208 unsigned int getSampledGoalsCount(void) const
00209 {
00210 return sampledGoalsCount_;
00211 }
00212
00213 private:
00214
00215 const Planner *planner_;
00216
00217 unsigned int addedStartStates_;
00218 unsigned int sampledGoalsCount_;
00219 State *tempState_;
00220
00221 const ProblemDefinition *pdef_;
00222 const SpaceInformation *si_;
00223 };
00224
00226 class Planner : private boost::noncopyable
00227 {
00228
00229 public:
00230
00232 Planner(const SpaceInformationPtr &si, const std::string &name);
00233
00235 virtual ~Planner(void)
00236 {
00237 }
00238
00240 template<class T>
00241 T* as(void)
00242 {
00244 BOOST_CONCEPT_ASSERT((boost::Convertible<T*, Planner*>));
00245
00246 return static_cast<T*>(this);
00247 }
00248
00250 template<class T>
00251 const T* as(void) const
00252 {
00254 BOOST_CONCEPT_ASSERT((boost::Convertible<T*, Planner*>));
00255
00256 return static_cast<const T*>(this);
00257 }
00258
00260 const SpaceInformationPtr& getSpaceInformation(void) const;
00261
00263 const ProblemDefinitionPtr& getProblemDefinition(void) const;
00264
00266 const PlannerInputStates& getPlannerInputStates(void) const;
00267
00272 virtual void setProblemDefinition(const ProblemDefinitionPtr &pdef);
00273
00286 virtual bool solve(const PlannerTerminationCondition &ptc) = 0;
00287
00290 bool solve(const PlannerTerminationCondition &ptc, double checkInterval);
00291
00295 bool solve(double solveTime);
00296
00300 virtual void clear(void);
00301
00308 virtual void getPlannerData(PlannerData &data) const;
00309
00311 const std::string& getName(void) const;
00312
00314 void setName(const std::string &name);
00315
00319 PlannerType getType(void) const;
00320
00325 virtual void setup(void);
00326
00331 virtual void checkValidity(void);
00332
00334 bool isSetup(void) const;
00335
00336 protected:
00337
00339 SpaceInformationPtr si_;
00340
00342 ProblemDefinitionPtr pdef_;
00343
00345 PlannerInputStates pis_;
00346
00348 std::string name_;
00349
00351 PlannerType type_;
00352
00354 bool setup_;
00355
00357 msg::Interface msg_;
00358 };
00359
00361 typedef boost::function1<PlannerPtr, const SpaceInformationPtr&> PlannerAllocator;
00362 }
00363 }
00364
00365 #endif