Loading...
Searching...
No Matches
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)
55}
56
57ompl::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_);
196}
197
199{
200 std::lock_guard<std::mutex> slock(lock_);
202}
203
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}
void addState(const State *st) override
Add a goal state.
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...
void clear() override
Clear all goal states.
void goalSamplingThread()
The function that samples goals by calling samplerFunc_ in a separate thread.
void sampleGoal(State *st) const override
Sample a state in the goal region.
void startSampling()
Start the goal sampling thread.
bool isSampling() const
Return true if the sampling thread is active.
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 stopSampling()
Stop the goal sampling thread.
bool couldSample() const override
Return true if GoalStates::couldSample() is true or if the sampling thread is active,...
void setNewStateCallback(const NewStateCallbackFn &callback)
Set the callback function to be called when a new state is added to the list of possible samples....
std::size_t getStateCount() const override
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...
const State * getState(unsigned int index) const override
Return a pointer to the indexth state in the state list.
unsigned int maxSampleCount() const override
Return the maximum number of samples that can be asked for before repeating.
bool hasStates() const override
Check if there are any states in this goal region.
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....
Definition of a set of goal states.
Definition: GoalStates.h:50
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
virtual bool hasStates() const
Check if there are any states in this goal region.
Definition: GoalStates.cpp:122
unsigned int maxSampleCount() const override
Return the maximum number of samples that can be asked for before repeating.
Definition: GoalStates.cpp:94
virtual std::size_t getStateCount() const
Return the number of valid goal states.
Definition: GoalStates.cpp:117
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.
Definition: GoalStates.cpp:81
virtual void clear()
Clear all goal states.
Definition: GoalStates.cpp:47
virtual const State * getState(unsigned int index) const
Return a pointer to the indexth state in the state list.
Definition: GoalStates.cpp:109
GoalType type_
Goal type.
Definition: Goal.h:146
Definition of a scoped state.
Definition: ScopedState.h:57
A shared pointer wrapper for ompl::base::SpaceInformation.
Definition of an abstract state.
Definition: State.h:50
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
@ GOAL_LAZY_SAMPLES
This bit is set if casting to goal states (ompl::base::GoalLazySamples) is possible.
Definition: GoalTypes.h:65
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...
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition: Time.h:64
STL namespace.