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); }
void ompl::base::GoalLazySamples::stopSampling() { if (isSampling()) { OMPL_DEBUG("Attempting to stop goal sampling thread..."); terminateSamplingThread_ = true; samplingThread_->join(); delete samplingThread_; samplingThread_ = nullptr; } else if (samplingThread_) { // join a finished thread samplingThread_->join(); delete samplingThread_; samplingThread_ = nullptr; } }
bool ompl::base::GoalLazySamples::couldSample() const { return canSample() || isSampling(); }