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();
bool isSampling(void) const
Return true if the sampling thread is active.
void startSampling(void)
Start the goal sampling thread.
virtual bool hasStates(void) const
Check if there are any states in this goal region.
void stopSampling(void)
Stop the goal sampling thread.
void goalSamplingThread(void)
The function that samples goals by calling samplerFunc_ in a separate thread.
virtual void clear(void)
Clear all goal states.
Definition of a scoped state.
virtual double distanceGoal(const State *st) const
Compute the distance to the goal (heuristic). This function is the one used in computing the distance...
boost::function< void(const base::State *)> NewStateCallbackFn
When new samples are generated and added to the list of possible samples, a callback can be called...
virtual void sampleGoal(State *st) const
Sample a state in the goal region.
virtual std::size_t getStateCount(void) const
Return the number of valid goal states.
virtual bool couldSample(void) const
Return true if GoalStates::couldSample() is true or if the sampling thread is active, as in this case it is possible a sample can be produced at some point.
virtual const State * getState(unsigned int index) const
Return a pointer to the indexth state in the state list.
Definition of a set of goal states.
virtual std::size_t getStateCount(void) const
Return the number of valid goal states.
bool addStateIfDifferent(const State *st, double minDistance)
Add a state st if it further away that minDistance from previously added states. Return true if the s...
virtual const State * getState(unsigned int index) const
Return a pointer to the indexth state in the state list.
virtual void addState(const State *st)
Add a goal state.
duration seconds(double sec)
Return the time duration representing a given number of seconds.
virtual void sampleGoal(State *st) const
Sample a state in the goal region.
void setNewStateCallback(const NewStateCallbackFn &callback)
Set the callback function to be called when a new state is added to the list of possible samples...
StateType * get(void)
Returns a pointer to the contained state.
GoalLazySamples(const SpaceInformationPtr &si, const GoalSamplingFn &samplerFunc, bool autoStart=true, double minDist=std::numeric_limits< double >::epsilon())
Create a goal region that can be sampled in a lazy fashion. A function (samplerFunc) that produces sa...
Definition of an abstract state.
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
virtual void addState(const State *st)
Add a goal state.
virtual bool hasStates(void) const
Check if there are any states in this goal region.
virtual double distanceGoal(const State *st) const
Compute the distance to the goal (heuristic). This function is the one used in computing the distance...
virtual void clear(void)
Clear all goal states.
boost::function< bool(const GoalLazySamples *, State *)> GoalSamplingFn
Goal sampling function. Returns false when no further calls should be made to it. Fills its second ar...
This bit is set if casting to goal states (ompl::base::GoalLazySamples) is possible.