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");
68  terminateSamplingThread_ = false;
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_);
78  if (!terminateSamplingThread_)
79  {
80  OMPL_DEBUG("Attempting to stop goal sampling thread...");
81  terminateSamplingThread_ = true;
82  }
83  }
84 
85  /* Join thread */
86  if (samplingThread_ != nullptr)
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  {
116  ++samplingAttempts_;
117  if (si_->satisfiesBounds(s.get()) && si_->isValid(s.get()))
118  {
119  OMPL_DEBUG("Adding goal state");
120  addStateIfDifferent(s.get(), minDist_);
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_);
134  terminateSamplingThread_ = true;
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_ && 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 != nullptr)
221  callback_(newState);
222  return added;
223 }
bool couldSample() const override
Return true if GoalStates::couldSample() is true or if the sampling thread is active,...
@ GOAL_LAZY_SAMPLES
This bit is set if casting to goal states (ompl::base::GoalLazySamples) is possible.
Definition: GoalTypes.h:65
GoalType type_
Goal type.
Definition: Goal.h:146
void clear() override
Clear all goal states.
bool isSampling() const
Return true if the sampling thread is active.
A shared pointer wrapper for ompl::base::SpaceInformation.
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...
bool hasStates() const override
Check if there are any states in this goal region.
Definition of an abstract state.
Definition: State.h:50
virtual void clear()
Clear all goal states.
Definition: GoalStates.cpp:47
void addState(const State *st) override
Add a goal state.
void sampleGoal(State *st) const override
Sample a state in the goal region.
Definition: GoalStates.cpp:81
std::size_t getStateCount() const override
Return the number of valid goal states.
const State * getState(unsigned int index) const override
Return a pointer to the indexth state in the state list.
double distanceGoal(const State *st) const override
Compute the distance to the goal (heuristic). This function is the one used in computing the distance...
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
Definition of a set of goal states.
Definition: GoalStates.h:50
virtual const State * getState(unsigned int index) const
Return a pointer to the indexth state in the state list.
Definition: GoalStates.cpp:109
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 addState(const State *st)
Add a goal state.
Definition: GoalStates.cpp:99
void sampleGoal(State *st) const override
Sample a state in the goal region.
void goalSamplingThread()
The function that samples goals by calling samplerFunc_ in a separate thread.
unsigned int maxSampleCount() const override
Return the maximum number of samples that can be asked for before repeating.
Definition: GoalStates.cpp:94
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
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.
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....
unsigned int maxSampleCount() const override
Return the maximum number of samples that can be asked for before repeating.
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...
virtual std::size_t getStateCount() const
Return the number of valid goal states.
Definition: GoalStates.cpp:117
virtual bool hasStates() const
Check if there are any states in this goal region.
Definition: GoalStates.cpp:122
Definition of a scoped state.
Definition: ScopedState.h:57
void stopSampling()
Stop the goal sampling thread.
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition: Time.h:64