37 #include "ompl/base/goals/GoalLazySamples.h"
38 #include "ompl/base/ScopedState.h"
39 #include "ompl/util/Time.h"
42 GoalStates(si), samplerFunc_(samplerFunc), terminateSamplingThread_(false), samplingThread_(NULL), samplingAttempts_(0), minDist_(minDist)
49 ompl::base::GoalLazySamples::~GoalLazySamples(
void)
56 if (samplingThread_ == NULL)
59 terminateSamplingThread_ =
false;
68 OMPL_DEBUG(
"Attempting to stop goal sampling thread...");
69 terminateSamplingThread_ =
true;
70 samplingThread_->join();
71 delete samplingThread_;
72 samplingThread_ = NULL;
77 samplingThread_->join();
78 delete samplingThread_;
79 samplingThread_ = NULL;
87 OMPL_DEBUG(
"Waiting for space information to be set up before the sampling thread can begin computation...");
89 while (!terminateSamplingThread_ && !si_->isSetup())
92 unsigned int prevsa = samplingAttempts_;
93 if (!terminateSamplingThread_ && samplerFunc_)
95 OMPL_DEBUG(
"Beginning sampling thread computation");
97 while (!terminateSamplingThread_ && samplerFunc_(
this, s.
get()))
100 if (si_->satisfiesBounds(s.
get()) && si_->isValid(s.
get()))
101 addStateIfDifferent(s.
get(), minDist_);
105 OMPL_WARN(
"Goal sampling thread never did any work.%s",
106 samplerFunc_ ? (si_->isSetup() ?
"" :
" Space information not set up.") :
" No sampling function set.");
107 terminateSamplingThread_ =
true;
108 OMPL_DEBUG(
"Stopped goal sampling thread after %u sampling attempts", samplingAttempts_ - prevsa);
113 return terminateSamplingThread_ ==
false && samplingThread_ != NULL;
118 return canSample() || isSampling();
123 boost::mutex::scoped_lock slock(lock_);
129 boost::mutex::scoped_lock slock(lock_);
135 boost::mutex::scoped_lock slock(lock_);
141 callback_ = callback;
146 boost::mutex::scoped_lock slock(lock_);
152 boost::mutex::scoped_lock slock(lock_);
158 boost::mutex::scoped_lock slock(lock_);
164 boost::mutex::scoped_lock slock(lock_);
173 boost::mutex::scoped_lock slock(lock_);
179 newState = states_.back();