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 
43 namespace 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 
183 {
185  {
186  return false;
187  });
188 }
189 
191 {
193  {
194  return true;
195  });
196 }
197 
199  const PlannerTerminationCondition &c2)
200 {
201  return PlannerTerminationCondition([c1, c2]
202  {
203  return c1() || c2();
204  });
205 }
206 
209 {
210  return PlannerTerminationCondition([c1, c2]
211  {
212  return c1() && c2();
213  });
214 }
215 
217 {
219 }
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 }
PlannerTerminationCondition plannerAlwaysTerminatingCondition()
Simple termination condition that always returns true. The termination condition will always be met.
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 timedPlannerTerminationCondition(double duration)
Return a termination condition that will become true duration seconds in the future (wall-time)
std::function< bool()> PlannerTerminationConditionFn
Signature for functions that decide whether termination conditions have been met for a planner,...
std::chrono::system_clock::time_point point
Representation of a point in time.
Definition: Time.h:116
point now()
Get the current time point.
Definition: Time.h:122
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
A shared pointer wrapper for ompl::base::ProblemDefinition.
std::chrono::system_clock::duration duration
Representation of a time duration.
Definition: Time.h:119
bool eval() const
The implementation of some termination condition. By default, this just calls fn_()
PlannerTerminationCondition plannerOrTerminationCondition(const PlannerTerminationCondition &c1, const PlannerTerminationCondition &c2)
Combine two termination conditions into one. If either termination condition returns true,...
PlannerTerminationCondition plannerAndTerminationCondition(const PlannerTerminationCondition &c1, const PlannerTerminationCondition &c2)
Combine two termination conditions into one. Both termination conditions need to return true for this...
PlannerTerminationCondition plannerNonTerminatingCondition()
Simple termination condition that always returns false. The termination condition will never be met.
void terminate() const
Notify that the condition for termination should become true, regardless of what eval() returns....
PlannerTerminationCondition(const PlannerTerminationConditionFn &fn)
Construct a termination condition. By default, eval() will call the externally specified function fn ...
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition: Time.h:128
Main namespace. Contains everything in this library.
Definition: AppBase.h:21