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 <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
188
196
205
208{
209 return PlannerTerminationCondition([c1, c2]
210 {
211 return c1() && c2();
212 });
213}
214
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.
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.