void ompl::base::GoalLazySamples::goalSamplingThread()
{
if (!si_->isSetup())
{
OMPL_DEBUG("Waiting for space information to be set up before the sampling thread can begin computation...");
// wait for everything to be set up before performing computation
while (!terminateSamplingThread_ && !si_->isSetup())
std::this_thread::sleep_for(time::seconds(0.01));
}
unsigned int prevsa = samplingAttempts_;
if (!terminateSamplingThread_ && samplerFunc_)
{
OMPL_DEBUG("Beginning sampling thread computation");
ScopedState<> s(si_);
while (!terminateSamplingThread_ && samplerFunc_(this, s.get()))
{
++samplingAttempts_;
if (si_->satisfiesBounds(s.get()) && si_->isValid(s.get()))
addStateIfDifferent(s.get(), minDist_);
}
}
else
OMPL_WARN("Goal sampling thread never did any work.%s",
samplerFunc_ ? (si_->isSetup() ? "" : " Space information not set up.") : " No sampling function "
"set.");
terminateSamplingThread_ = true;
OMPL_DEBUG("Stopped goal sampling thread after %u sampling attempts", samplingAttempts_ - prevsa);
}
void ompl::base::GoalLazySamples::goalSamplingThread()
{
{
/* Wait for startSampling() to finish assignment
* samplingThread_ */
std::lock_guard<std::mutex> slock(lock_);
}
if (!si_->isSetup()) // this looks racy
{
OMPL_DEBUG("Waiting for space information to be set up before the sampling thread can begin computation...");
// wait for everything to be set up before performing computation
while (!terminateSamplingThread_ && !si_->isSetup())
std::this_thread::sleep_for(time::seconds(0.01));
}
unsigned int prevsa = samplingAttempts_;
if (isSampling() && samplerFunc_)
{
OMPL_DEBUG("Beginning sampling thread computation");
ScopedState<> s(si_);
while (isSampling() && samplerFunc_(this, s.get()))
{
++samplingAttempts_;
if (si_->satisfiesBounds(s.get()) && si_->isValid(s.get()))
{
OMPL_DEBUG("Adding goal state");
addStateIfDifferent(s.get(), minDist_);
}
else
{
OMPL_DEBUG("Invalid goal candidate");
}
}
}
else
OMPL_WARN("Goal sampling thread never did any work.%s",
samplerFunc_ ? (si_->isSetup() ? "" : " Space information not set up.") : " No sampling function "
"set.");
{
std::lock_guard<std::mutex> slock(lock_);
terminateSamplingThread_ = true;
}
OMPL_DEBUG("Stopped goal sampling thread after %u sampling attempts", samplingAttempts_ - prevsa);
}
