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 <thread>
40#include <utility>
41
42namespace ompl
43{
44 namespace base
45 {
47 class PlannerTerminationCondition::PlannerTerminationConditionImpl
48 {
49 public:
50 PlannerTerminationConditionImpl(PlannerTerminationConditionFn fn, double period)
51 : fn_(std::move(fn))
52 , period_(period)
53 , terminate_(false)
54 , thread_(nullptr)
55 , evalValue_(false)
56 , signalThreadStop_(false)
57 {
58 if (period_ > 0.0)
59 startEvalThread();
60 }
61
62 ~PlannerTerminationConditionImpl()
63 {
64 stopEvalThread();
65 }
66
67 bool eval() const
68 {
69 if (terminate_)
70 return true;
71 if (period_ > 0.0)
72 return evalValue_;
73 return fn_();
74 }
75
76 void terminate() const
77 {
78 // it is ok to have unprotected write here
79 terminate_ = true;
80 }
81
82 private:
84 void startEvalThread()
85 {
86 if (thread_ == nullptr)
87 {
88 signalThreadStop_ = false;
89 evalValue_ = false;
90 thread_ = new std::thread([this]
91 {
92 periodicEval();
93 });
94 }
95 }
96
98 void stopEvalThread()
99 {
100 signalThreadStop_ = true;
101 if (thread_ != nullptr)
102 {
103 thread_->join();
104 delete thread_;
105 thread_ = nullptr;
106 }
107 }
108
110 void periodicEval()
111 {
112 // we want to check for termination at least once every ms;
113 // even though we may evaluate the condition itself more rarely
114
115 unsigned int count = 1;
116 time::duration s = time::seconds(period_);
117 if (period_ > 0.001)
118 {
119 count = 0.5 + period_ / 0.001;
120 s = time::seconds(period_ / (double)count);
121 }
122
123 while (!terminate_ && !signalThreadStop_)
124 {
125 evalValue_ = fn_();
126 for (unsigned int i = 0; i < count; ++i)
127 {
128 if (terminate_ || signalThreadStop_)
129 break;
130 std::this_thread::sleep_for(s);
131 }
132 }
133 }
134
138
140 double period_;
141
144 mutable bool terminate_;
145
147 std::thread *thread_;
148
150 bool evalValue_;
151
153 bool signalThreadStop_;
154 };
155
157 }
158}
159
161 : impl_(std::make_shared<PlannerTerminationConditionImpl>(fn, -1.0))
162{
163}
164
166 double period)
167 : impl_(std::make_shared<PlannerTerminationConditionImpl>(fn, period))
168{
169}
170
172{
173 impl_->terminate();
174}
175
177{
178 return impl_->eval();
179}
180
182{
184 {
185 return false;
186 });
187}
188
190{
192 {
193 return true;
194 });
195}
196
199{
200 return PlannerTerminationCondition([c1, c2]
201 {
202 return c1() || c2();
203 });
204}
205
208{
209 return PlannerTerminationCondition([c1, c2]
210 {
211 return c1() && c2();
212 });
213}
214
216{
218}
219
221{
222 const time::point endTime(time::now() + duration);
223 return PlannerTerminationCondition([endTime]
224 {
225 return time::now() > endTime;
226 });
227}
228
230{
231 if (interval > duration)
232 interval = duration;
233 const time::point endTime(time::now() + time::seconds(duration));
234 return PlannerTerminationCondition([endTime]
235 {
236 return time::now() > endTime;
237 },
238 interval);
239}
240
243{
244 return PlannerTerminationCondition([pdef]
245 {
246 return pdef->hasExactSolution();
247 });
248}
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.
std::function< bool()> PlannerTerminationConditionFn
Signature for functions that decide whether termination conditions have been met for a planner,...
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.
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
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
std::chrono::system_clock::duration duration
Representation of a time duration.
Definition: Time.h:55
Main namespace. Contains everything in this library.
STL namespace.