Loading...
Searching...
No Matches
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.
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.