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 #include "ompl/base/samplers/ObstacleBasedValidStateSampler.h"
00038 #include "ompl/base/SpaceInformation.h"
00039
00040 ompl::base::ObstacleBasedValidStateSampler::ObstacleBasedValidStateSampler(const SpaceInformation *si) :
00041 ValidStateSampler(si), sampler_(si->allocStateSampler())
00042 {
00043 name_ = "obstacle";
00044 }
00045
00046 bool ompl::base::ObstacleBasedValidStateSampler::sample(State *state)
00047 {
00048
00049 unsigned int attempts = 0;
00050 bool valid = true;
00051 do
00052 {
00053 sampler_->sampleUniform(state);
00054 valid = si_->isValid(state);
00055 ++attempts;
00056 } while (valid && attempts < attempts_);
00057 if (valid)
00058 return false;
00059
00060
00061 State *temp = si_->allocState();
00062 attempts = 0;
00063 valid = false;
00064 do
00065 {
00066 sampler_->sampleUniform(temp);
00067 valid = si_->isValid(temp);
00068 ++attempts;
00069 } while (!valid && attempts < attempts_);
00070
00071
00072
00073 if (valid)
00074 {
00075 std::pair<State*, double> fail(state, 0.0);
00076 si_->checkMotion(temp, state, fail);
00077 }
00078
00079 si_->freeState(temp);
00080
00081 return valid;
00082 }
00083
00084 bool ompl::base::ObstacleBasedValidStateSampler::sampleNear(State *state, const State *near, const double distance)
00085 {
00086
00087 unsigned int attempts = 0;
00088 bool valid = true;
00089 do
00090 {
00091 sampler_->sampleUniformNear(state, near, distance);
00092 valid = si_->isValid(state);
00093 ++attempts;
00094 } while (valid && attempts < attempts_);
00095 if (valid)
00096 return false;
00097
00098
00099 State *temp = si_->allocState();
00100 attempts = 0;
00101 valid = false;
00102 do
00103 {
00104 sampler_->sampleUniform(temp);
00105 valid = si_->isValid(temp);
00106 ++attempts;
00107 } while (!valid && attempts < attempts_);
00108
00109
00110
00111 if (valid)
00112 {
00113 std::pair<State*, double> fail(state, 0.0);
00114 si_->checkMotion(temp, state, fail);
00115 }
00116
00117 si_->freeState(temp);
00118
00119 return valid;
00120 }