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
243ompl::base::exactSolnPlannerTerminationCondition(const ompl::base::ProblemDefinitionPtr& pdef)
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....
This namespace contains sampling based planning routines shared by both planning under geometric cons...
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.