BridgeTestValidStateSampler.cpp
1 #include "ompl/base/samplers/BridgeTestValidStateSampler.h"
2 #include "ompl/base/SpaceInformation.h"
3 #include "ompl/tools/config/MagicConstants.h"
4 
7  , sampler_(si->allocStateSampler())
8  , stddev_(si->getMaximumExtent() * magic::STD_DEV_AS_SPACE_EXTENT_FRACTION)
9 {
10  name_ = "bridge_test";
11  params_.declareParam<double>("standard_deviation", [this](double stddev) { setStdDev(stddev); },
12  [this] { return getStdDev(); });
13 }
14 
16 {
17  unsigned int attempts = 0;
18  bool valid = false;
19  State *endpoint = si_->allocState();
20  do
21  {
22  sampler_->sampleUniform(state);
23  bool v1 = si_->isValid(state);
24  if (!v1)
25  {
26  sampler_->sampleGaussian(endpoint, state, stddev_);
27  bool v2 = si_->isValid(endpoint);
28  if (!v2)
29  {
30  si_->getStateSpace()->interpolate(endpoint, state, 0.5, state);
31  valid = si_->isValid(state);
32  }
33  }
34  ++attempts;
35  } while (!valid && attempts < attempts_);
36 
37  si_->freeState(endpoint);
38  return valid;
39 }
40 
41 bool ompl::base::BridgeTestValidStateSampler::sampleNear(State *state, const State *near, const double distance)
42 {
43  unsigned int attempts = 0;
44  bool valid = false;
45  State *endpoint = si_->allocState();
46  do
47  {
48  sampler_->sampleUniformNear(state, near, distance);
49  bool v1 = si_->isValid(state);
50  if (!v1)
51  {
52  sampler_->sampleGaussian(endpoint, state, distance);
53  bool v2 = si_->isValid(endpoint);
54  if (!v2)
55  {
56  si_->getStateSpace()->interpolate(endpoint, state, 0.5, state);
57  valid = si_->isValid(state);
58  }
59  }
60  ++attempts;
61  } while (!valid && attempts < attempts_);
62 
63  si_->freeState(endpoint);
64  return valid;
65 }
BridgeTestValidStateSampler(const SpaceInformation *si)
Constructor.
double getStdDev() const
Get the standard deviation used when sampling.
bool sampleNear(State *state, const State *near, double distance) override
Sample a state near another, within specified distance. Return false, in case of failure.
Abstract definition of a state sampler.
The base class for space information. This contains all the information about the space planning is d...
Definition of an abstract state.
Definition: State.h:49
void declareParam(const std::string &name, const typename SpecificParam< T >::SetterFn &setter, const typename SpecificParam< T >::GetterFn &getter=typename SpecificParam< T >::GetterFn())
This function declares a parameter name, and specifies the setter and getter functions.
Definition: GenericParam.h:232
bool sample(State *state) override
Sample a state. Return false in case of failure.
void setStdDev(double stddev)
Set the standard deviation to use when sampling.
ParamSet params_
The parameters for this instance of the valid state sampler.
std::string name_
The name of the sampler.