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_SPACE_INFORMATION_
00038 #define OMPL_BASE_SPACE_INFORMATION_
00039
00040 #include "ompl/base/State.h"
00041 #include "ompl/base/StateValidityChecker.h"
00042 #include "ompl/base/MotionValidator.h"
00043 #include "ompl/base/StateSpace.h"
00044 #include "ompl/base/ValidStateSampler.h"
00045
00046 #include "ompl/util/ClassForward.h"
00047 #include "ompl/util/Console.h"
00048 #include "ompl/util/Exception.h"
00049
00050 #include <boost/noncopyable.hpp>
00051 #include <boost/function.hpp>
00052 #include <boost/bind.hpp>
00053
00054 #include <utility>
00055 #include <cstdlib>
00056 #include <vector>
00057 #include <iostream>
00058
00060 namespace ompl
00061 {
00062
00067 namespace base
00068 {
00069
00071 ClassForward(SpaceInformation);
00072
00079 typedef boost::function1<bool, const State*> StateValidityCheckerFn;
00080
00081
00085 class SpaceInformation : private boost::noncopyable
00086 {
00087 public:
00088
00090 SpaceInformation(const StateSpacePtr &space);
00091
00092 virtual ~SpaceInformation(void)
00093 {
00094 }
00095
00097 bool isValid(const State *state) const
00098 {
00099 return stateValidityChecker_->isValid(state);
00100 }
00101
00103 const StateSpacePtr& getStateSpace(void) const
00104 {
00105 return stateSpace_;
00106 }
00107
00112 bool equalStates(const State *state1, const State *state2) const
00113 {
00114 return stateSpace_->equalStates(state1, state2);
00115 }
00116
00118 bool satisfiesBounds(const State *state) const
00119 {
00120 return stateSpace_->satisfiesBounds(state);
00121 }
00122
00124 double distance(const State *state1, const State *state2) const
00125 {
00126 return stateSpace_->distance(state1, state2);
00127 }
00128
00130 void enforceBounds(State *state) const
00131 {
00132 stateSpace_->enforceBounds(state);
00133 }
00134
00136 void printState(const State *state, std::ostream &out = std::cout) const
00137 {
00138 stateSpace_->printState(state, out);
00139 }
00140
00149 void setStateValidityChecker(const StateValidityCheckerPtr &svc)
00150 {
00151 stateValidityChecker_ = svc;
00152 setup_ = false;
00153 }
00154
00160 void setStateValidityChecker(const StateValidityCheckerFn &svc);
00161
00163 const StateValidityCheckerPtr& getStateValidityChecker(void) const
00164 {
00165 return stateValidityChecker_;
00166 }
00167
00171 void setMotionValidator(const MotionValidatorPtr &mv)
00172 {
00173 motionValidator_ = mv;
00174 setup_ = false;
00175 }
00176
00178 const MotionValidatorPtr& getMotionValidator(void) const
00179 {
00180 return motionValidator_;
00181 }
00182
00189 void setStateValidityCheckingResolution(double resolution)
00190 {
00191 stateSpace_->setLongestValidSegmentFraction(resolution);
00192 setup_ = false;
00193 }
00194
00199 double getStateValidityCheckingResolution(void) const
00200 {
00201 return stateSpace_->getLongestValidSegmentFraction();
00202 }
00203
00204
00208 unsigned int getStateDimension(void) const
00209 {
00210 return stateSpace_->getDimension();
00211 }
00212
00217 State* allocState(void) const
00218 {
00219 return stateSpace_->allocState();
00220 }
00221
00223 void allocStates(std::vector<State*> &states) const
00224 {
00225 for (unsigned int i = 0 ; i < states.size() ; ++i)
00226 states[i] = stateSpace_->allocState();
00227 }
00228
00230 void freeState(State *state) const
00231 {
00232 stateSpace_->freeState(state);
00233 }
00234
00236 void freeStates(std::vector<State*> &states) const
00237 {
00238 for (unsigned int i = 0 ; i < states.size() ; ++i)
00239 stateSpace_->freeState(states[i]);
00240 }
00241
00243 void copyState(State *destination, const State *source) const
00244 {
00245 stateSpace_->copyState(destination, source);
00246 }
00247
00249 State* cloneState(const State *source) const
00250 {
00251 State *copy = stateSpace_->allocState();
00252 stateSpace_->copyState(copy, source);
00253 return copy;
00254 }
00255
00263 StateSamplerPtr allocStateSampler(void) const
00264 {
00265 return stateSpace_->allocStateSampler();
00266 }
00267
00271 ValidStateSamplerPtr allocValidStateSampler(void) const;
00272
00273
00276 void setValidStateSamplerAllocator(const ValidStateSamplerAllocator &vssa)
00277 {
00278 vssa_ = vssa;
00279 setup_ = false;
00280 }
00281
00290 double getMaximumExtent(void) const
00291 {
00292 return stateSpace_->getMaximumExtent();
00293 }
00294
00302 bool searchValidNearby(State *state, const State *near, double distance, unsigned int attempts) const;
00303
00311 bool searchValidNearby(const ValidStateSamplerPtr &sampler, State *state, const State *near, double distance) const;
00312
00319 unsigned int randomBounceMotion(const StateSamplerPtr &sss, const State *start, unsigned int steps, std::vector<State*> &states, bool alloc) const;
00320
00327 bool checkMotion(const State *s1, const State *s2, std::pair<State*, double> &lastValid) const
00328 {
00329 return motionValidator_->checkMotion(s1, s2, lastValid);
00330 }
00331
00332
00334 bool checkMotion(const State *s1, const State *s2) const
00335 {
00336 return motionValidator_->checkMotion(s1, s2);
00337 }
00338
00344 bool checkMotion(const std::vector<State*> &states, unsigned int count, unsigned int &firstInvalidStateIndex) const;
00345
00347 bool checkMotion(const std::vector<State*> &states, unsigned int count) const;
00348
00357 unsigned int getMotionStates(const State *s1, const State *s2, std::vector<State*> &states, unsigned int count, bool endpoints, bool alloc) const;
00358
00365 double probabilityOfValidState(unsigned int attempts) const;
00366
00368 double averageValidMotionLength(unsigned int attempts) const;
00369
00373 virtual void printSettings(std::ostream &out = std::cout) const;
00374
00376 virtual void printProperties(std::ostream &out = std::cout) const;
00377
00382 virtual void setup(void);
00383
00385 bool isSetup(void) const;
00386
00387 protected:
00388
00390 StateSpacePtr stateSpace_;
00391
00393 StateValidityCheckerPtr stateValidityChecker_;
00394
00396 MotionValidatorPtr motionValidator_;
00397
00399 bool setup_;
00400
00402 ValidStateSamplerAllocator vssa_;
00403
00405 msg::Interface msg_;
00406 };
00407
00408 }
00409
00410 }
00411
00412 #endif