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
41bool 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}
bool sampleNear(State *state, const State *near, double distance) override
Sample a state near another, within specified distance. Return false, in case of failure.
double getStdDev() const
Get the standard deviation used when sampling.
BridgeTestValidStateSampler(const SpaceInformation *si)
Constructor.
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.
void declareParam(const std::string &name, const typename SpecificParam< T >::SetterFn &setter, const typename SpecificParam< T >::GetterFn &getter=[] { return T();})
This function declares a parameter name, and specifies the setter and getter functions.
Definition: GenericParam.h:231
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:50
Abstract definition of a state sampler.
ParamSet params_
The parameters for this instance of the valid state sampler.
std::string name_
The name of the sampler.