void ompl::base::SpaceInformation::samplesPerSecond(double &uniform, double &near, double &gaussian,
unsigned int attempts) const
{
StateSamplerPtr ss = allocStateSampler();
std::vector<State *> states(attempts + 1);
allocStates(states);
time::point start = time::now();
for (unsigned int i = 0; i < attempts; ++i)
ss->sampleUniform(states[i]);
uniform = (double)attempts / time::seconds(time::now() - start);
double d = getMaximumExtent() / 10.0;
ss->sampleUniform(states[attempts]);
start = time::now();
for (unsigned int i = 1; i <= attempts; ++i)
ss->sampleUniformNear(states[i - 1], states[i], d);
near = (double)attempts / time::seconds(time::now() - start);
start = time::now();
for (unsigned int i = 1; i <= attempts; ++i)
ss->sampleGaussian(states[i - 1], states[i], d);
gaussian = (double)attempts / time::seconds(time::now() - start);
freeStates(states);
}
void ompl::control::SpaceInformation::setup()
{
base::SpaceInformation::setup();
if (!statePropagator_)
throw Exception("State propagator not defined");
if (minSteps_ > maxSteps_)
throw Exception("The minimum number of steps cannot be larger than the maximum number of steps");
if (minSteps_ == 0 && maxSteps_ == 0)
{
minSteps_ = 1;
maxSteps_ = 10;
OMPL_WARN("Assuming propagation will always have between %d and %d steps", minSteps_, maxSteps_);
}
if (minSteps_ < 1)
throw Exception("The minimum number of steps must be at least 1");
if (stepSize_ < std::numeric_limits<double>::epsilon())
{
stepSize_ = getStateValidityCheckingResolution() * getMaximumExtent();
if (stepSize_ < std::numeric_limits<double>::epsilon())
throw Exception("The propagation step size must be larger than 0");
OMPL_WARN("The propagation step size is assumed to be %f", stepSize_);
}
controlSpace_->setup();
if (controlSpace_->getDimension() <= 0)
throw Exception("The dimension of the control space we plan in must be > 0");
}