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 <utility>
38 
39 #include "ompl/base/ScopedState.h"
40 #include "ompl/base/goals/GoalLazySamples.h"
41 #include "ompl/util/Time.h"
42 
44  double minDist)
45  : GoalStates(si)
46  , samplerFunc_(std::move(samplerFunc))
47  , terminateSamplingThread_(false)
48  , samplingThread_(nullptr)
49  , samplingAttempts_(0)
50  , minDist_(minDist)
51 {
53  if (autoStart)
54  startSampling();
55 }
56 
57 ompl::base::GoalLazySamples::~GoalLazySamples()
58 {
59  stopSampling();
60 }
61 
63 {
64  std::lock_guard<std::mutex> slock(lock_);
65  if (samplingThread_ == nullptr)
66  {
67  OMPL_DEBUG("Starting goal sampling thread");
69  samplingThread_ = new std::thread(&GoalLazySamples::goalSamplingThread, this);
70  }
71 }
72 
74 {
75  /* Set termination flag */
76  {
77  std::lock_guard<std::mutex> slock(lock_);
79  {
80  OMPL_DEBUG("Attempting to stop goal sampling thread...");
82  }
83  }
84 
85  /* Join thread */
86  if (samplingThread_)
87  {
88  samplingThread_->join();
89  delete samplingThread_;
90  samplingThread_ = nullptr;
91  }
92 }
93 
95 {
96  {
97  /* Wait for startSampling() to finish assignment
98  * samplingThread_ */
99  std::lock_guard<std::mutex> slock(lock_);
100  }
101 
102  if (!si_->isSetup()) // this looks racy
103  {
104  OMPL_DEBUG("Waiting for space information to be set up before the sampling thread can begin computation...");
105  // wait for everything to be set up before performing computation
106  while (!terminateSamplingThread_ && !si_->isSetup())
107  std::this_thread::sleep_for(time::seconds(0.01));
108  }
109  unsigned int prevsa = samplingAttempts_;
110  if (isSampling() && samplerFunc_)
111  {
112  OMPL_DEBUG("Beginning sampling thread computation");
113  ScopedState<> s(si_);
114  while (isSampling() && samplerFunc_(this, s.get()))
115  {
117  if (si_->satisfiesBounds(s.get()) && si_->isValid(s.get()))
118  {
119  OMPL_DEBUG("Adding goal state");
121  }
122  else
123  {
124  OMPL_DEBUG("Invalid goal candidate");
125  }
126  }
127  }
128  else
129  OMPL_WARN("Goal sampling thread never did any work.%s",
130  samplerFunc_ ? (si_->isSetup() ? "" : " Space information not set up.") : " No sampling function "
131  "set.");
132  {
133  std::lock_guard<std::mutex> slock(lock_);
135  }
136 
137  OMPL_DEBUG("Stopped goal sampling thread after %u sampling attempts", samplingAttempts_ - prevsa);
138 }
139 
141 {
142  std::lock_guard<std::mutex> slock(lock_);
143  return terminateSamplingThread_ == false && samplingThread_ != nullptr;
144 }
145 
147 {
148  return canSample() || isSampling();
149 }
150 
152 {
153  std::lock_guard<std::mutex> slock(lock_);
155 }
156 
158 {
159  std::lock_guard<std::mutex> slock(lock_);
160  return GoalStates::distanceGoal(st);
161 }
162 
164 {
165  std::lock_guard<std::mutex> slock(lock_);
167 }
168 
170 {
171  callback_ = callback;
172 }
173 
175 {
176  std::lock_guard<std::mutex> slock(lock_);
178 }
179 
181 {
182  std::lock_guard<std::mutex> slock(lock_);
183  return GoalStates::getState(index);
184 }
185 
187 {
188  std::lock_guard<std::mutex> slock(lock_);
189  return GoalStates::hasStates();
190 }
191 
193 {
194  std::lock_guard<std::mutex> slock(lock_);
195  return GoalStates::getStateCount();
196 }
197 
199 {
200  std::lock_guard<std::mutex> slock(lock_);
202 }
203 
204 bool ompl::base::GoalLazySamples::addStateIfDifferent(const State *st, double minDistance)
205 {
206  const base::State *newState = nullptr;
207  bool added = false;
208  {
209  std::lock_guard<std::mutex> slock(lock_);
210  if (GoalStates::distanceGoal(st) > minDistance)
211  {
213  added = true;
214  if (callback_)
215  newState = states_.back();
216  }
217  }
218 
219  // the lock is released at this; if needed, issue a call to the callback
220  if (newState)
221  callback_(newState);
222  return added;
223 }
double distanceGoal(const State *st) const override
Compute the distance to the goal (heuristic). This function is the one used in computing the distance...
void addState(const State *st) override
Add a goal state.
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:47
GoalSamplingFn samplerFunc_
Function that produces samples.
STL namespace.
unsigned int maxSampleCount() const override
Return the maximum number of samples that can be asked for before repeating.
double minDist_
Samples returned by the sampling thread are added to the list of states only if they are at least min...
std::thread * samplingThread_
Additional thread for sampling goal states.
Definition of a set of goal states.
Definition: GoalStates.h:49
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.
std::vector< State * > states_
The goal states. Only ones that are valid are considered by the motion planner.
Definition: GoalStates.h:88
virtual void addState(const State *st)
Add a goal state.
Definition: GoalStates.cpp:99
virtual const State * getState(unsigned int index) const
Return a pointer to the indexth state in the state list.
Definition: GoalStates.cpp:109
bool hasStates() const override
Check if there are any states in this goal region.
StateType * get()
Returns a pointer to the contained state.
Definition: ScopedState.h:394
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition: Time.h:76
std::function< bool(const GoalLazySamples *, State *)> GoalSamplingFn
Goal sampling function. Returns false when no further calls should be made to it. Fills its second ar...
unsigned int maxSampleCount() const override
Return the maximum number of samples that can be asked for before repeating.
Definition: GoalStates.cpp:94
void sampleGoal(State *st) const override
Sample a state in the goal region.
Definition: GoalStates.cpp:81
GoalType type_
Goal type.
Definition: Goal.h:146
void sampleGoal(State *st) const override
Sample a state in the goal region.
void clear() override
Clear all goal states.
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...
A shared pointer wrapper for ompl::base::SpaceInformation.
Definition of an abstract state.
Definition: State.h:49
std::size_t getStateCount() const override
Return the number of valid goal states.
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
SpaceInformationPtr si_
The space information for this goal.
Definition: Goal.h:149
std::mutex lock_
Lock for updating the set of states.
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
bool couldSample() const override
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 terminateSamplingThread_
Flag used to notify the sampling thread to terminate sampling.
void stopSampling()
Stop the goal sampling thread.
GoalLazySamples(const SpaceInformationPtr &si, 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...
unsigned int samplingAttempts_
The number of times the sampling function was called and it returned true.
const State * getState(unsigned int index) const override
Return a pointer to the indexth state in the state list.
virtual bool hasStates() const
Check if there are any states in this goal region.
Definition: GoalStates.cpp:122
This bit is set if casting to goal states (ompl::base::GoalLazySamples) is possible.
Definition: GoalTypes.h:65
double distanceGoal(const State *st) const override
Compute the distance to the goal (heuristic). This function is the one used in computing the distance...
Definition: GoalStates.cpp:59
NewStateCallbackFn callback_
If defined, this function is called when a new state is added to the list of possible samples...
std::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 std::size_t getStateCount() const
Return the number of valid goal states.
Definition: GoalStates.cpp:117