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 
42 namespace 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_)
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_)
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 
198  const PlannerTerminationCondition &c2)
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 }
239 
242 {
243  return PlannerTerminationCondition([pdef]
244  {
245  return pdef->hasExactSolution();
246  });
247 }
248 
249 namespace ompl
250 {
251  namespace base
252  {
254  : maxCalls_(numIterations), timesCalled_(0u)
255  {
256  }
257 
259  {
260  ++timesCalled_;
261 
262  return (timesCalled_ > maxCalls_);
263  }
264 
266  {
267  timesCalled_ = 0u;
268  }
269 
270  IterationTerminationCondition::operator PlannerTerminationCondition()
271  {
272  return PlannerTerminationCondition([this]
273  {
274  return eval();
275  });
276  }
277  }
278 }
void reset()
Reset the number of times the IterationTeriminationCondition has been called.
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...
A shared pointer wrapper for ompl::base::ProblemDefinition.
bool eval() const
The implementation of some termination condition. By default, this just calls fn_() ...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
STL namespace.
PlannerTerminationCondition plannerAndTerminationCondition(const PlannerTerminationCondition &c1, const PlannerTerminationCondition &c2)
Combine two termination conditions into one. Both termination conditions need to return true for this...
std::function< bool()> PlannerTerminationConditionFn
Signature for functions that decide whether termination conditions have been met for a planner...
PlannerTerminationCondition plannerAlwaysTerminatingCondition()
Simple termination condition that always returns true. The termination condition will always be met...
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition: Time.h:76
PlannerTerminationCondition timedPlannerTerminationCondition(double duration)
Return a termination condition that will become true duration seconds in the future (wall-time) ...
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
PlannerTerminationCondition exactSolnPlannerTerminationCondition(ompl::base::ProblemDefinitionPtr pdef)
Return a termination condition that will become true as soon as the problem definition has an exact s...
std::chrono::system_clock::duration duration
Representation of a time duration.
Definition: Time.h:67
PlannerTerminationCondition(const PlannerTerminationConditionFn &fn)
Construct a termination condition. By default, eval() will call the externally specified function fn ...
IterationTerminationCondition(unsigned int numIterations)
Construct a termination condition that can be evaluated numIterations times before returning true...
point now()
Get the current time point.
Definition: Time.h:70
std::chrono::system_clock::time_point point
Representation of a point in time.
Definition: Time.h:64
bool eval()
Increment the number of times eval has been called and check if the planner should now terminate...
void terminate() const
Notify that the condition for termination should become true, regardless of what eval() returns...