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 freeState(State *state) const
00224 {
00225 stateSpace_->freeState(state);
00226 }
00227
00229 void copyState(State *destination, const State *source) const
00230 {
00231 stateSpace_->copyState(destination, source);
00232 }
00233
00235 State* cloneState(const State *source) const
00236 {
00237 State *copy = stateSpace_->allocState();
00238 stateSpace_->copyState(copy, source);
00239 return copy;
00240 }
00241
00249 StateSamplerPtr allocStateSampler(void) const
00250 {
00251 return stateSpace_->allocStateSampler();
00252 }
00253
00257 ValidStateSamplerPtr allocValidStateSampler(void) const;
00258
00259
00262 void setValidStateSamplerAllocator(const ValidStateSamplerAllocator &vssa)
00263 {
00264 vssa_ = vssa;
00265 setup_ = false;
00266 }
00267
00276 double getMaximumExtent(void) const
00277 {
00278 return stateSpace_->getMaximumExtent();
00279 }
00280
00288 bool searchValidNearby(State *state, const State *near, double distance, unsigned int attempts) const;
00289
00297 bool searchValidNearby(const ValidStateSamplerPtr &sampler, State *state, const State *near, double distance) const;
00298
00305 unsigned int randomBounceMotion(const StateSamplerPtr &sss, const State *start, unsigned int steps, std::vector<State*> &states, bool alloc) const;
00306
00313 bool checkMotion(const State *s1, const State *s2, std::pair<State*, double> &lastValid) const
00314 {
00315 return motionValidator_->checkMotion(s1, s2, lastValid);
00316 }
00317
00318
00320 bool checkMotion(const State *s1, const State *s2) const
00321 {
00322 return motionValidator_->checkMotion(s1, s2);
00323 }
00324
00330 bool checkMotion(const std::vector<State*> &states, unsigned int count, unsigned int &firstInvalidStateIndex) const;
00331
00333 bool checkMotion(const std::vector<State*> &states, unsigned int count) const;
00334
00343 unsigned int getMotionStates(const State *s1, const State *s2, std::vector<State*> &states, unsigned int count, bool endpoints, bool alloc) const;
00344
00351 double probabilityOfValidState(unsigned int attempts) const;
00352
00354 double averageValidMotionLength(unsigned int attempts) const;
00355
00359 virtual void printSettings(std::ostream &out = std::cout) const;
00360
00362 virtual void printProperties(std::ostream &out = std::cout) const;
00363
00368 virtual void setup(void);
00369
00371 bool isSetup(void) const;
00372
00373 protected:
00374
00376 StateSpacePtr stateSpace_;
00377
00379 StateValidityCheckerPtr stateValidityChecker_;
00380
00382 MotionValidatorPtr motionValidator_;
00383
00385 bool setup_;
00386
00388 ValidStateSamplerAllocator vssa_;
00389
00391 msg::Interface msg_;
00392 };
00393
00394 }
00395
00396 }
00397
00398 #endif