GoalLazySamples.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2010, Rice University
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Rice University nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
37 #include "ompl/base/goals/GoalLazySamples.h"
38 #include "ompl/base/ScopedState.h"
39 #include "ompl/util/Time.h"
40 
41 ompl::base::GoalLazySamples::GoalLazySamples(const SpaceInformationPtr &si, const GoalSamplingFn &samplerFunc, bool autoStart, double minDist) :
42  GoalStates(si), samplerFunc_(samplerFunc), terminateSamplingThread_(false), samplingThread_(NULL), samplingAttempts_(0), minDist_(minDist)
43 {
45  if (autoStart)
46  startSampling();
47 }
48 
49 ompl::base::GoalLazySamples::~GoalLazySamples()
50 {
51  stopSampling();
52 }
53 
55 {
56  if (samplingThread_ == NULL)
57  {
58  OMPL_DEBUG("Starting goal sampling thread");
60  samplingThread_ = new boost::thread(&GoalLazySamples::goalSamplingThread, this);
61  }
62 }
63 
65 {
66  if (isSampling())
67  {
68  OMPL_DEBUG("Attempting to stop goal sampling thread...");
70  samplingThread_->join();
71  delete samplingThread_;
72  samplingThread_ = NULL;
73  }
74  else
75  if (samplingThread_)
76  { // join a finished thread
77  samplingThread_->join();
78  delete samplingThread_;
79  samplingThread_ = NULL;
80  }
81 }
82 
84 {
85  if (!si_->isSetup())
86  {
87  OMPL_DEBUG("Waiting for space information to be set up before the sampling thread can begin computation...");
88  // wait for everything to be set up before performing computation
89  while (!terminateSamplingThread_ && !si_->isSetup())
90  boost::this_thread::sleep(time::seconds(0.01));
91  }
92  unsigned int prevsa = samplingAttempts_;
94  {
95  OMPL_DEBUG("Beginning sampling thread computation");
96  ScopedState<> s(si_);
97  while (!terminateSamplingThread_ && samplerFunc_(this, s.get()))
98  {
100  if (si_->satisfiesBounds(s.get()) && si_->isValid(s.get()))
102  }
103  }
104  else
105  OMPL_WARN("Goal sampling thread never did any work.%s",
106  samplerFunc_ ? (si_->isSetup() ? "" : " Space information not set up.") : " No sampling function set.");
108  OMPL_DEBUG("Stopped goal sampling thread after %u sampling attempts", samplingAttempts_ - prevsa);
109 }
110 
112 {
113  return terminateSamplingThread_ == false && samplingThread_ != NULL;
114 }
115 
117 {
118  return canSample() || isSampling();
119 }
120 
122 {
123  boost::mutex::scoped_lock slock(lock_);
125 }
126 
128 {
129  boost::mutex::scoped_lock slock(lock_);
130  return GoalStates::distanceGoal(st);
131 }
132 
134 {
135  boost::mutex::scoped_lock slock(lock_);
137 }
138 
140 {
141  callback_ = callback;
142 }
143 
145 {
146  boost::mutex::scoped_lock slock(lock_);
148 }
149 
151 {
152  boost::mutex::scoped_lock slock(lock_);
153  return GoalStates::getState(index);
154 }
155 
157 {
158  boost::mutex::scoped_lock slock(lock_);
159  return GoalStates::hasStates();
160 }
161 
163 {
164  boost::mutex::scoped_lock slock(lock_);
165  return GoalStates::getStateCount();
166 }
167 
168 bool ompl::base::GoalLazySamples::addStateIfDifferent(const State *st, double minDistance)
169 {
170  const base::State *newState = NULL;
171  bool added = false;
172  {
173  boost::mutex::scoped_lock slock(lock_);
174  if (GoalStates::distanceGoal(st) > minDistance)
175  {
177  added = true;
178  if (callback_)
179  newState = states_.back();
180  }
181  }
182 
183  // the lock is released at this; if needed, issue a call to the callback
184  if (newState)
185  callback_(newState);
186  return added;
187 }
virtual void sampleGoal(State *st) const
Sample a state in the goal region.
void goalSamplingThread()
The function that samples goals by calling samplerFunc_ in a separate thread.
Definition of a scoped state.
Definition: ScopedState.h:56
virtual void clear()
Clear all goal states.
Definition: GoalStates.cpp:48
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...
boost::mutex lock_
Lock for updating the set of states.
virtual std::size_t getStateCount() const
Return the number of valid goal states.
GoalSamplingFn samplerFunc_
Function that produces samples.
virtual const State * getState(unsigned int index) const
Return a pointer to the indexth state in the state list.
double minDist_
Samples returned by the sampling thread are added to the list of states only if they are at least min...
Definition of a set of goal states.
Definition: GoalStates.h:50
virtual bool couldSample() 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.
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...
void startSampling()
Start the goal sampling thread.
bool isSampling() const
Return true if the sampling thread is active.
virtual void addState(const State *st)
Add a goal state.
Definition: GoalStates.cpp:95
virtual bool hasStates() const
Check if there are any states in this goal region.
virtual const State * getState(unsigned int index) const
Return a pointer to the indexth state in the state list.
Definition: GoalStates.cpp:105
StateType * get()
Returns a pointer to the contained state.
Definition: ScopedState.h:396
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition: Time.h:62
virtual double distanceGoal(const State *st) const
Compute the distance to the goal (heuristic). This function is the one used in computing the distance...
GoalType type_
Goal type.
Definition: Goal.h:147
bool canSample() const
Return true if maxSampleCount() > 0, since in this case samples can certainly be produced.
void setNewStateCallback(const NewStateCallbackFn &callback)
Set the callback function to be called when a new state is added to the list of possible samples...
virtual void clear()
Clear all goal states.
std::vector< State * > states_
The goal states. Only ones that are valid are considered by the motion planner.
Definition: GoalStates.h:91
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...
A boost shared pointer wrapper for ompl::base::SpaceInformation.
virtual void sampleGoal(State *st) const
Sample a state in the goal region.
Definition: GoalStates.cpp:82
Definition of an abstract state.
Definition: State.h:50
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
SpaceInformationPtr si_
The space information for this goal.
Definition: Goal.h:150
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
virtual void addState(const State *st)
Add a goal 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...
Definition: GoalStates.cpp:60
boost::thread * samplingThread_
Additional thread for sampling goal states.
bool terminateSamplingThread_
Flag used to notify the sampling thread to terminate sampling.
void stopSampling()
Stop the goal sampling thread.
unsigned int samplingAttempts_
The number of times the sampling function was called and it returned true.
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...
virtual bool hasStates() const
Check if there are any states in this goal region.
Definition: GoalStates.cpp:118
This bit is set if casting to goal states (ompl::base::GoalLazySamples) is possible.
Definition: GoalTypes.h:64
NewStateCallbackFn callback_
If defined, this function is called when a new state is added to the list of possible samples...
virtual std::size_t getStateCount() const
Return the number of valid goal states.
Definition: GoalStates.cpp:113