Loading...
Searching...
No Matches
PlannerTerminationCondition.cpp
1/*********************************************************************
2* Software License Agreement (BSD License)
3*
4* Copyright (c) 2011, 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/PlannerTerminationCondition.h"
38#include "ompl/util/Time.h"
39#include <atomic>
40#include <thread>
41#include <utility>
42
43namespace ompl
44{
45 namespace base
46 {
48 class PlannerTerminationCondition::PlannerTerminationConditionImpl
49 {
50 public:
51 PlannerTerminationConditionImpl(PlannerTerminationConditionFn fn, double period)
52 : fn_(std::move(fn))
53 , period_(period)
54 , terminate_(false)
55 , thread_(nullptr)
56 , evalValue_(false)
57 , signalThreadStop_(false)
58 {
59 if (period_ > 0.0)
60 startEvalThread();
61 }
62
63 ~PlannerTerminationConditionImpl()
64 {
65 stopEvalThread();
66 }
67
68 bool eval() const
69 {
70 if (terminate_)
71 return true;
72 if (period_ > 0.0)
73 return evalValue_;
74 return fn_();
75 }
76
77 void terminate() const
78 {
79 // it is ok to have unprotected write here
80 terminate_ = true;
81 }
82
83 private:
85 void startEvalThread()
86 {
87 if (thread_ == nullptr)
88 {
89 signalThreadStop_ = false;
90 evalValue_ = false;
91 thread_ = new std::thread([this]
92 {
93 periodicEval();
94 });
95 }
96 }
97
99 void stopEvalThread()
100 {
101 signalThreadStop_ = true;
102 if (thread_ != nullptr)
103 {
104 thread_->join();
105 delete thread_;
106 thread_ = nullptr;
107 }
108 }
109
111 void periodicEval()
112 {
113 // we want to check for termination at least once every ms;
114 // even though we may evaluate the condition itself more rarely
115
116 unsigned int count = 1;
117 time::duration s = time::seconds(period_);
118 if (period_ > 0.001)
119 {
120 count = 0.5 + period_ / 0.001;
121 s = time::seconds(period_ / (double)count);
122 }
123
124 while (!terminate_ && !signalThreadStop_)
125 {
126 evalValue_ = fn_();
127 for (unsigned int i = 0; i < count; ++i)
128 {
129 if (terminate_ || signalThreadStop_)
130 break;
131 std::this_thread::sleep_for(s);
132 }
133 }
134 }
135
139
141 double period_;
142
145 mutable bool terminate_;
146
148 std::thread *thread_;
149
151 std::atomic<bool> evalValue_;
152
154 std::atomic<bool> signalThreadStop_;
155 };
156
158 }
159}
160
162 : impl_(std::make_shared<PlannerTerminationConditionImpl>(fn, -1.0))
163{
164}
165
167 double period)
168 : impl_(std::make_shared<PlannerTerminationConditionImpl>(fn, period))
169{
170}
171
173{
174 impl_->terminate();
175}
176
178{
179 return impl_->eval();
180}
181
189
197
206
209{
210 return PlannerTerminationCondition([c1, c2]
211 {
212 return c1() && c2();
213 });
214}
215
220
222{
223 const time::point endTime(time::now() + duration);
224 return PlannerTerminationCondition([endTime]
225 {
226 return time::now() > endTime;
227 });
228}
229
231{
232 if (interval > duration)
233 interval = duration;
234 const time::point endTime(time::now() + time::seconds(duration));
235 return PlannerTerminationCondition([endTime]
236 {
237 return time::now() > endTime;
238 },
239 interval);
240}
241
244{
245 return PlannerTerminationCondition([pdef]
246 {
247 return pdef->hasExactSolution();
248 });
249}
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
PlannerTerminationCondition(const PlannerTerminationConditionFn &fn)
Construct a termination condition. By default, eval() will call the externally specified function fn ...
bool eval() const
The implementation of some termination condition. By default, this just calls fn_()
void terminate() const
Notify that the condition for termination should become true, regardless of what eval() returns....
A shared pointer wrapper for ompl::base::ProblemDefinition.
PlannerTerminationCondition plannerAlwaysTerminatingCondition()
Simple termination condition that always returns true. The termination condition will always be met.
PlannerTerminationCondition plannerAndTerminationCondition(const PlannerTerminationCondition &c1, const PlannerTerminationCondition &c2)
Combine two termination conditions into one. Both termination conditions need to return true for this...
PlannerTerminationCondition exactSolnPlannerTerminationCondition(const ompl::base::ProblemDefinitionPtr &pdef)
Return a termination condition that will become true as soon as the problem definition has an exact s...
PlannerTerminationCondition plannerOrTerminationCondition(const PlannerTerminationCondition &c1, const PlannerTerminationCondition &c2)
Combine two termination conditions into one. If either termination condition returns true,...
PlannerTerminationCondition plannerNonTerminatingCondition()
Simple termination condition that always returns false. The termination condition will never be met.
std::function< bool()> PlannerTerminationConditionFn
Signature for functions that decide whether termination conditions have been met for a planner,...
PlannerTerminationCondition timedPlannerTerminationCondition(double duration)
Return a termination condition that will become true duration seconds in the future (wall-time)
std::chrono::system_clock::time_point point
Representation of a point in time.
Definition Time.h:52
std::chrono::system_clock::duration duration
Representation of a time duration.
Definition Time.h:55
point now()
Get the current time point.
Definition Time.h:58
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition Time.h:64
Main namespace. Contains everything in this library.
STL namespace.