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_PROBLEM_DEFINITION_
00038 #define OMPL_BASE_PROBLEM_DEFINITION_
00039
00040 #include "ompl/base/State.h"
00041 #include "ompl/base/Goal.h"
00042 #include "ompl/base/Path.h"
00043 #include "ompl/base/SpaceInformation.h"
00044 #include "ompl/base/SolutionNonExistenceProof.h"
00045 #include "ompl/util/Console.h"
00046 #include "ompl/util/ClassForward.h"
00047 #include "ompl/base/ScopedState.h"
00048
00049 #include <vector>
00050 #include <cstdlib>
00051 #include <iostream>
00052 #include <limits>
00053
00054 #include <boost/noncopyable.hpp>
00055
00056 namespace ompl
00057 {
00058 namespace base
00059 {
00060
00062
00063 ClassForward(ProblemDefinition);
00065
00070 struct PlannerSolution
00071 {
00073 PlannerSolution(const PathPtr &path, bool approximate = false, double difference = -1.0) :
00074 index_(-1), path_(path), length_(path->length()), approximate_(approximate), difference_(difference)
00075 {
00076 }
00077
00079 bool operator==(const PlannerSolution& p) const
00080 {
00081 return path_ == p.path_;
00082 }
00083
00085 bool operator<(const PlannerSolution &b) const
00086 {
00087 if (!approximate_ && b.approximate_)
00088 return true;
00089 if (approximate_ && !b.approximate_)
00090 return false;
00091 if (approximate_ && b.approximate_)
00092 return difference_ < b.difference_;
00093 return length_ < b.length_;
00094 }
00095
00097 int index_;
00098
00100 PathPtr path_;
00101
00103 double length_;
00104
00106 bool approximate_;
00107
00109 double difference_;
00110 };
00111
00115 class ProblemDefinition : private boost::noncopyable
00116 {
00117 public:
00118
00120 ProblemDefinition(const SpaceInformationPtr &si);
00121
00122 virtual ~ProblemDefinition(void)
00123 {
00124 clearStartStates();
00125 }
00126
00128 const SpaceInformationPtr& getSpaceInformation(void) const
00129 {
00130 return si_;
00131 }
00132
00134 void addStartState(const State *state)
00135 {
00136 startStates_.push_back(si_->cloneState(state));
00137 }
00138
00140 void addStartState(const ScopedState<> &state)
00141 {
00142 startStates_.push_back(si_->cloneState(state.get()));
00143 }
00144
00148 bool hasStartState(const State *state, unsigned int *startIndex = NULL);
00149
00151 void clearStartStates(void)
00152 {
00153 for (unsigned int i = 0 ; i < startStates_.size() ; ++i)
00154 si_->freeState(startStates_[i]);
00155 startStates_.clear();
00156 }
00157
00159 unsigned int getStartStateCount(void) const
00160 {
00161 return startStates_.size();
00162 }
00163
00165 const State* getStartState(unsigned int index) const
00166 {
00167 return startStates_[index];
00168 }
00169
00171 State* getStartState(unsigned int index)
00172 {
00173 return startStates_[index];
00174 }
00175
00177 void setGoal(const GoalPtr &goal)
00178 {
00179 goal_ = goal;
00180 }
00181
00183 void clearGoal(void)
00184 {
00185 goal_.reset();
00186 }
00187
00189 const GoalPtr& getGoal(void) const
00190 {
00191 return goal_;
00192 }
00193
00198 void getInputStates(std::vector<const State*> &states) const;
00199
00207 void setStartAndGoalStates(const State *start, const State *goal, const double threshold = std::numeric_limits<double>::epsilon());
00208
00210 void setGoalState(const State *goal, const double threshold = std::numeric_limits<double>::epsilon());
00211
00213 void setStartAndGoalStates(const ScopedState<> &start, const ScopedState<> &goal, const double threshold = std::numeric_limits<double>::epsilon())
00214 {
00215 setStartAndGoalStates(start.get(), goal.get(), threshold);
00216 }
00217
00219 void setGoalState(const ScopedState<> &goal, const double threshold = std::numeric_limits<double>::epsilon())
00220 {
00221 setGoalState(goal.get(), threshold);
00222 }
00223
00229 bool isTrivial(unsigned int *startIndex = NULL, double *distance = NULL) const;
00230
00243 PathPtr isStraightLinePathValid(void) const;
00244
00249 bool fixInvalidInputStates(double distStart, double distGoal, unsigned int attempts);
00250
00252 bool hasSolution(void) const;
00253
00257 bool hasApproximateSolution(void) const;
00258
00260 double getSolutionDifference(void) const;
00261
00266 PathPtr getSolutionPath(void) const;
00267
00272 void addSolutionPath(const PathPtr &path, bool approximate = false, double difference = -1.0) const;
00273
00275 std::size_t getSolutionCount(void) const;
00276
00278 std::vector<PlannerSolution> getSolutions(void) const;
00279
00281 void clearSolutionPaths(void) const;
00282
00284 void print(std::ostream &out = std::cout) const;
00285
00287 bool hasSolutionNonExistenceProof(void) const;
00288
00290 void clearSolutionNonExistenceProof(void);
00291
00293 const SolutionNonExistenceProofPtr& getSolutionNonExistenceProof(void) const;
00294
00296 void setSolutionNonExistenceProof(const SolutionNonExistenceProofPtr& nonExistenceProof);
00297
00298 protected:
00299
00301 bool fixInvalidInputState(State *state, double dist, bool start, unsigned int attempts);
00302
00304 SpaceInformationPtr si_;
00305
00307 std::vector<State*> startStates_;
00308
00310 GoalPtr goal_;
00311
00313 SolutionNonExistenceProofPtr nonExistenceProof_;
00314
00315 private:
00316
00318 ClassForward(PlannerSolutionSet);
00320
00322 PlannerSolutionSetPtr solutions_;
00323 };
00324 }
00325 }
00326
00327 #endif