All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
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 <boost/bind.hpp>
40 #include <boost/thread.hpp>
41 #include <boost/lambda/bind.hpp>
42 #include <utility>
43 
44 namespace ompl
45 {
46  namespace base
47  {
48 
50  class PlannerTerminationCondition::PlannerTerminationConditionImpl
51  {
52  public:
53  PlannerTerminationConditionImpl(const PlannerTerminationConditionFn &fn, double period) :
54  fn_(fn),
55  period_(period),
56  terminate_(false),
57  thread_(NULL),
58  evalValue_(false),
59  signalThreadStop_(false)
60  {
61  if (period_ > 0.0)
62  startEvalThread();
63  }
64 
65  ~PlannerTerminationConditionImpl()
66  {
67  stopEvalThread();
68  }
69 
70  bool eval() const
71  {
72  if (terminate_)
73  return true;
74  if (period_ > 0.0)
75  return evalValue_;
76  return fn_();
77  }
78 
79  void terminate() const
80  {
81  // it is ok to have unprotected write here
82  terminate_ = true;
83  }
84 
85  private:
86 
88  void startEvalThread(void)
89  {
90  if (!thread_)
91  {
92  signalThreadStop_ = false;
93  evalValue_ = false;
94  thread_ = new boost::thread(boost::bind(&PlannerTerminationConditionImpl::periodicEval, this));
95  }
96  }
97 
99  void stopEvalThread(void)
100  {
101  signalThreadStop_ = true;
102  if (thread_)
103  {
104  thread_->join();
105  delete thread_;
106  thread_ = NULL;
107  }
108  }
109 
111  void periodicEval(void)
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  boost::this_thread::sleep(s);
132  }
133  }
134  }
135 
138 
140  double period_;
141 
143  mutable bool terminate_;
144 
146  boost::thread *thread_;
147 
149  bool evalValue_;
150 
152  bool signalThreadStop_;
153  };
154 
156  }
157 }
158 
160 impl_(new PlannerTerminationConditionImpl(fn, -1.0))
161 {
162 }
163 
165 impl_(new PlannerTerminationConditionImpl(fn, period))
166 {
167 }
168 
170 {
171  impl_->terminate();
172 }
173 
175 {
176  return impl_->eval();
177 }
178 
180 {
181  return PlannerTerminationCondition(boost::lambda::constant(false));
182 }
183 
185 {
186  return PlannerTerminationCondition(boost::lambda::constant(true));
187 }
188 
190 namespace ompl
191 {
192  namespace base
193  {
194  static bool plannerOrTerminationConditionAux(const PlannerTerminationCondition &c1, const PlannerTerminationCondition &c2)
195  {
196  return c1() || c2();
197  }
198 
199  static bool plannerAndTerminationConditionAux(const PlannerTerminationCondition &c1, const PlannerTerminationCondition &c2)
200  {
201  return c1() && c2();
202  }
203 
204  // return true if a certain point in time has passed
205  static bool timePassed(const time::point &endTime)
206  {
207  return time::now() > endTime;
208  }
209  }
210 }
212 
214 {
215  return PlannerTerminationCondition(boost::bind(&plannerOrTerminationConditionAux, c1, c2));
216 }
217 
219 {
220  return PlannerTerminationCondition(boost::bind(&plannerAndTerminationConditionAux, c1, c2));
221 }
222 
224 {
225  return PlannerTerminationCondition(boost::bind(&timePassed, time::now() + time::seconds(duration)));
226 }
227 
229 {
230  if (interval > duration)
231  interval = duration;
232  return PlannerTerminationCondition(boost::bind(&timePassed, time::now() + time::seconds(duration)), interval);
233 }
bool eval(void) 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 plannerNonTerminatingCondition()
Simple termination condition that always returns false. The termination condition will never be met...
boost::posix_time::time_duration duration
Representation of a time duration.
Definition: Time.h:53
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
PlannerTerminationCondition plannerAndTerminationCondition(const PlannerTerminationCondition &c1, const PlannerTerminationCondition &c2)
Combine two termination conditions into one. Both termination conditions need to return true for this...
void terminate(void) const
Notify that the condition for termination should become true, regardless of what eval() returns...
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:62
PlannerTerminationCondition timedPlannerTerminationCondition(double duration)
Return a termination condition that will become true duration seconds in the future (wall-time) ...
PlannerTerminationCondition(const PlannerTerminationConditionFn &fn)
Construct a termination condition. By default, eval() will call the externally specified function fn ...
boost::posix_time::ptime point
Representation of a point in time.
Definition: Time.h:50
point now(void)
Get the current time point.
Definition: Time.h:56
boost::function< bool()> PlannerTerminationConditionFn
Signature for functions that decide whether termination conditions have been met for a planner...