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_based";
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 do
00064 {
00065 sampler_->sampleUniform(temp);
00066 valid = si_->isValid(temp);
00067 ++attempts;
00068 } while (!valid && attempts < attempts_);
00069
00070
00071
00072 if (valid)
00073 {
00074 std::pair<State*, double> fail(state, 0.0);
00075 si_->checkMotion(temp, state, fail);
00076 }
00077
00078 si_->freeState(temp);
00079
00080 return valid;
00081 }
00082
00083 bool ompl::base::ObstacleBasedValidStateSampler::sampleNear(State *state, const State *near, const double distance)
00084 {
00085
00086 unsigned int attempts = 0;
00087 bool valid = true;
00088 do
00089 {
00090 sampler_->sampleUniformNear(state, near, distance);
00091 valid = si_->isValid(state);
00092 ++attempts;
00093 } while (valid && attempts < attempts_);
00094 if (valid)
00095 return false;
00096
00097
00098 State *temp = si_->allocState();
00099 attempts = 0;
00100 do
00101 {
00102 sampler_->sampleUniform(temp);
00103 valid = si_->isValid(temp);
00104 ++attempts;
00105 } while (!valid && attempts < attempts_);
00106
00107
00108
00109 if (valid)
00110 {
00111 std::pair<State*, double> fail(state, 0.0);
00112 si_->checkMotion(temp, state, fail);
00113 }
00114
00115 si_->freeState(temp);
00116
00117 return valid;
00118 }