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/SpaceInformation.h"
00043 #include "ompl/util/Console.h"
00044 #include "ompl/util/ClassForward.h"
00045 #include "ompl/base/ScopedState.h"
00046
00047 #include <vector>
00048 #include <cstdlib>
00049 #include <iostream>
00050 #include <limits>
00051
00052 #include <boost/noncopyable.hpp>
00053
00054 namespace ompl
00055 {
00056 namespace base
00057 {
00058
00060
00061 ClassForward(ProblemDefinition);
00063
00069 class ProblemDefinition : private boost::noncopyable
00070 {
00071 public:
00072
00074 ProblemDefinition(const SpaceInformationPtr &si) : si_(si)
00075 {
00076 }
00077
00078 virtual ~ProblemDefinition(void)
00079 {
00080 clearStartStates();
00081 }
00082
00084 const SpaceInformationPtr& getSpaceInformation(void) const
00085 {
00086 return si_;
00087 }
00088
00090 void addStartState(const State *state)
00091 {
00092 startStates_.push_back(si_->cloneState(state));
00093 }
00094
00096 void addStartState(const ScopedState<> &state)
00097 {
00098 startStates_.push_back(si_->cloneState(state.get()));
00099 }
00100
00104 bool hasStartState(const State *state, unsigned int *startIndex = NULL);
00105
00107 void clearStartStates(void)
00108 {
00109 for (unsigned int i = 0 ; i < startStates_.size() ; ++i)
00110 si_->freeState(startStates_[i]);
00111 startStates_.clear();
00112 }
00113
00115 unsigned int getStartStateCount(void) const
00116 {
00117 return startStates_.size();
00118 }
00119
00121 const State* getStartState(unsigned int index) const
00122 {
00123 return startStates_[index];
00124 }
00125
00127 State* getStartState(unsigned int index)
00128 {
00129 return startStates_[index];
00130 }
00131
00133 void setGoal(const GoalPtr &goal)
00134 {
00135 goal_ = goal;
00136 }
00137
00139 void clearGoal(void)
00140 {
00141 goal_.reset();
00142 }
00143
00145 const GoalPtr& getGoal(void) const
00146 {
00147 return goal_;
00148 }
00149
00154 void getInputStates(std::vector<const State*> &states) const;
00155
00163 void setStartAndGoalStates(const State *start, const State *goal, const double threshold = std::numeric_limits<double>::epsilon());
00164
00166 void setGoalState(const State *goal, const double threshold = std::numeric_limits<double>::epsilon());
00167
00169 void setStartAndGoalStates(const ScopedState<> &start, const ScopedState<> &goal, const double threshold = std::numeric_limits<double>::epsilon())
00170 {
00171 setStartAndGoalStates(start.get(), goal.get(), threshold);
00172 }
00173
00175 void setGoalState(const ScopedState<> &goal, const double threshold = std::numeric_limits<double>::epsilon())
00176 {
00177 setGoalState(goal.get(), threshold);
00178 }
00179
00185 bool isTrivial(unsigned int *startIndex = NULL, double *distance = NULL) const;
00186
00199 PathPtr isStraightLinePathValid(void) const;
00200
00205 bool fixInvalidInputStates(double distStart, double distGoal, unsigned int attempts);
00206
00208 void print(std::ostream &out = std::cout) const;
00209
00210 protected:
00211
00213 bool fixInvalidInputState(State *state, double dist, bool start, unsigned int attempts);
00214
00216 SpaceInformationPtr si_;
00217
00219 std::vector<State*> startStates_;
00220
00222 GoalPtr goal_;
00223
00225 msg::Interface msg_;
00226 };
00227 }
00228 }
00229
00230 #endif