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/lambda/bind.hpp>
41 #include <utility>
42 
44 {
45  terminate_ = true;
46 }
47 
49 {
50  return fn_();
51 }
52 
53 ompl::base::PlannerNonTerminatingCondition::PlannerNonTerminatingCondition(void) : PlannerTerminationCondition(boost::lambda::constant(false))
54 {
55 }
56 
57 ompl::base::PlannerAlwaysTerminatingCondition::PlannerAlwaysTerminatingCondition(void) : PlannerTerminationCondition(boost::lambda::constant(true))
58 {
59 }
60 
62 namespace ompl
63 {
64  namespace base
65  {
66  static bool plannerOrTerminationCondition(const PlannerTerminationCondition &c1, const PlannerTerminationCondition &c2)
67  {
68  return c1() || c2();
69  }
70 
71  static bool plannerAndTerminationCondition(const PlannerTerminationCondition &c1, const PlannerTerminationCondition &c2)
72  {
73  return c1() && c2();
74  }
75 
76  // return true if a certain point in time has passed
77  static bool timePassed(const time::point &endTime)
78  {
79  return time::now() > endTime;
80  }
81  }
82 }
84 
85 ompl::base::PlannerOrTerminationCondition::PlannerOrTerminationCondition(const PlannerTerminationCondition &c1, const PlannerTerminationCondition &c2) :
86  PlannerTerminationCondition(boost::bind(&plannerOrTerminationCondition, c1, c2))
87 {
88 }
89 
90 ompl::base::PlannerAndTerminationCondition::PlannerAndTerminationCondition(const PlannerTerminationCondition &c1, const PlannerTerminationCondition &c2) :
91  PlannerTerminationCondition(boost::bind(&plannerAndTerminationCondition, c1, c2))
92 {
93 }
94 
96 {
97  return evalValue_;
98 }
99 
101 {
102  if (!thread_)
103  thread_ = new boost::thread(boost::bind(&PlannerThreadedTerminationCondition::periodicEval, this));
104 }
105 
107 {
108  if (thread_)
109  {
110  thread_->interrupt();
111  thread_->join();
112  delete thread_;
113  thread_ = NULL;
114  }
115 }
116 
118 {
119  return fn_();
120 }
121 
123  PlannerTerminationCondition(fn), thread_(NULL), evalValue_(terminate_), period_(period)
124 {
125  startEvalThread();
126 }
127 
128 ompl::base::PlannerThreadedTerminationCondition::~PlannerThreadedTerminationCondition(void)
129 {
130  terminate_ = true;
131  stopEvalThread();
132 }
133 
135 {
136  time::duration s = time::seconds(period_);
137  do
138  {
139  evalValue_ = computeEval();
140  if ((*this)())
141  break;
142  boost::this_thread::sleep(s);
143  } while (!(*this)());
144 }
145 
147 {
148  return PlannerTerminationCondition(boost::bind(&timePassed, time::now() + time::seconds(duration)));
149 }
150 
152 {
153  if (interval > duration)
154  interval = duration;
155  return PlannerThreadedTerminationCondition(boost::bind(&timePassed, time::now() + time::seconds(duration)), interval);
156 }