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/StateManifold.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 <cstdlib>
00055 #include <vector>
00056 #include <iostream>
00057
00059 namespace ompl
00060 {
00061
00066 namespace base
00067 {
00068
00070 ClassForward(SpaceInformation);
00071
00078 typedef boost::function1<bool, const State*> StateValidityCheckerFn;
00079
00080
00084 class SpaceInformation : private boost::noncopyable
00085 {
00086 public:
00087
00090 SpaceInformation(const StateManifoldPtr &manifold);
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 StateManifoldPtr& getStateManifold(void) const
00104 {
00105 return stateManifold_;
00106 }
00107
00112 bool equalStates(const State *state1, const State *state2) const
00113 {
00114 return stateManifold_->equalStates(state1, state2);
00115 }
00116
00118 bool satisfiesBounds(const State *state) const
00119 {
00120 return stateManifold_->satisfiesBounds(state);
00121 }
00122
00124 double distance(const State *state1, const State *state2) const
00125 {
00126 return stateManifold_->distance(state1, state2);
00127 }
00128
00130 void enforceBounds(State *state) const
00131 {
00132 stateManifold_->enforceBounds(state);
00133 }
00134
00136 void printState(const State *state, std::ostream &out = std::cout) const
00137 {
00138 stateManifold_->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 stateManifold_->setLongestValidSegmentFraction(resolution);
00192 setup_ = false;
00193 }
00194
00199 double getStateValidityCheckingResolution(void) const
00200 {
00201 return stateManifold_->getLongestValidSegmentFraction();
00202 }
00203
00204
00208 unsigned int getStateDimension(void) const
00209 {
00210 return stateManifold_->getDimension();
00211 }
00212
00217 State* allocState(void) const
00218 {
00219 return stateManifold_->allocState();
00220 }
00221
00223 void freeState(State *state) const
00224 {
00225 stateManifold_->freeState(state);
00226 }
00227
00229 void copyState(State *destination, const State *source) const
00230 {
00231 stateManifold_->copyState(destination, source);
00232 }
00233
00235 State* cloneState(const State *source) const
00236 {
00237 State *copy = stateManifold_->allocState();
00238 stateManifold_->copyState(copy, source);
00239 return copy;
00240 }
00241
00249 ManifoldStateSamplerPtr allocManifoldStateSampler(void) const
00250 {
00251 return stateManifold_->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 stateManifold_->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 ManifoldStateSamplerPtr &mss, 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
00348 virtual void printSettings(std::ostream &out = std::cout) const;
00349
00354 virtual void setup(void);
00355
00357 bool isSetup(void) const;
00358
00359 protected:
00360
00362 StateManifoldPtr stateManifold_;
00363
00365 StateValidityCheckerPtr stateValidityChecker_;
00366
00368 MotionValidatorPtr motionValidator_;
00369
00371 bool setup_;
00372
00374 ValidStateSamplerAllocator vssa_;
00375
00377 msg::Interface msg_;
00378 };
00379
00380 }
00381
00382 }
00383
00384 #endif