ompl/base/src/PlannerTerminationCondition.cpp
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2011, Rice University
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Rice University nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 /* Author: Ioan Sucan */
00036 
00037 #include "ompl/base/PlannerTerminationCondition.h"
00038 #include "ompl/util/Time.h"
00039 #include <boost/bind.hpp>
00040 #include <boost/thread.hpp>
00041 #include <boost/lambda/bind.hpp>
00042 #include <utility>
00043 
00044 namespace ompl
00045 {
00046     namespace base
00047     {
00048 
00050         class PlannerTerminationCondition::PlannerTerminationConditionImpl
00051         {
00052         public:
00053             PlannerTerminationConditionImpl(const PlannerTerminationConditionFn &fn, double period) :
00054             fn_(fn),
00055             period_(period),
00056             terminate_(false),
00057             thread_(NULL),
00058             evalValue_(false),
00059             signalThreadStop_(false)
00060             {
00061                 if (period_ > 0.0)
00062                     startEvalThread();
00063             }
00064 
00065             ~PlannerTerminationConditionImpl()
00066             {
00067                 stopEvalThread();
00068             }
00069 
00070             bool eval() const
00071             {
00072                 if (terminate_)
00073                     return true;
00074                 if (period_ > 0.0)
00075                     return evalValue_;
00076                 return fn_();
00077             }
00078 
00079             void terminate() const
00080             {
00081                 // it is ok to have unprotected write here
00082                 terminate_ = true;
00083             }
00084 
00085         private:
00086 
00088             void startEvalThread()
00089             {
00090                 if (!thread_)
00091                 {
00092                     signalThreadStop_ = false;
00093                     evalValue_ = false;
00094                     thread_ = new boost::thread(boost::bind(&PlannerTerminationConditionImpl::periodicEval, this));
00095                 }
00096             }
00097 
00099             void stopEvalThread()
00100             {
00101                 signalThreadStop_ = true;
00102                 if (thread_)
00103                 {
00104                     thread_->join();
00105                     delete thread_;
00106                     thread_ = NULL;
00107                 }
00108             }
00109 
00111             void periodicEval()
00112             {
00113                 // we want to check for termination at least once every ms;
00114                 // even though we may evaluate the condition itself more rarely
00115 
00116                 unsigned int count = 1;
00117                 time::duration s = time::seconds(period_);
00118                 if (period_ > 0.001)
00119                 {
00120                     count = 0.5 + period_ / 0.001;
00121                     s = time::seconds(period_ / (double) count);
00122                 }
00123 
00124                 while (!terminate_ && !signalThreadStop_)
00125                 {
00126                     evalValue_ = fn_();
00127                     for (unsigned int i = 0 ; i < count ; ++i)
00128                     {
00129                         if (terminate_ || signalThreadStop_)
00130                             break;
00131                         boost::this_thread::sleep(s);
00132                     }
00133                 }
00134             }
00135 
00137             PlannerTerminationConditionFn fn_;
00138 
00140             double                        period_;
00141 
00143             mutable bool                  terminate_;
00144 
00146             boost::thread                *thread_;
00147 
00149             bool                          evalValue_;
00150 
00152             bool                          signalThreadStop_;
00153         };
00154 
00156     }
00157 }
00158 
00159 ompl::base::PlannerTerminationCondition::PlannerTerminationCondition(const PlannerTerminationConditionFn &fn) :
00160 impl_(new PlannerTerminationConditionImpl(fn, -1.0))
00161 {
00162 }
00163 
00164 ompl::base::PlannerTerminationCondition::PlannerTerminationCondition(const PlannerTerminationConditionFn &fn, double period) :
00165 impl_(new PlannerTerminationConditionImpl(fn, period))
00166 {
00167 }
00168 
00169 void ompl::base::PlannerTerminationCondition::terminate() const
00170 {
00171     impl_->terminate();
00172 }
00173 
00174 bool ompl::base::PlannerTerminationCondition::eval() const
00175 {
00176     return impl_->eval();
00177 }
00178 
00179 ompl::base::PlannerTerminationCondition ompl::base::plannerNonTerminatingCondition()
00180 {
00181     return PlannerTerminationCondition(boost::lambda::constant(false));
00182 }
00183 
00184 ompl::base::PlannerTerminationCondition ompl::base::plannerAlwaysTerminatingCondition()
00185 {
00186     return PlannerTerminationCondition(boost::lambda::constant(true));
00187 }
00188 
00190 namespace ompl
00191 {
00192     namespace base
00193     {
00194         static bool plannerOrTerminationConditionAux(const PlannerTerminationCondition &c1, const PlannerTerminationCondition &c2)
00195         {
00196             return c1() || c2();
00197         }
00198 
00199         static bool plannerAndTerminationConditionAux(const PlannerTerminationCondition &c1, const PlannerTerminationCondition &c2)
00200         {
00201             return c1() && c2();
00202         }
00203 
00204         // return true if a certain point in time has passed
00205         static bool timePassed(const time::point &endTime)
00206         {
00207             return time::now() > endTime;
00208         }
00209     }
00210 }
00212 
00213 ompl::base::PlannerTerminationCondition ompl::base::plannerOrTerminationCondition(const PlannerTerminationCondition &c1, const PlannerTerminationCondition &c2)
00214 {
00215     return PlannerTerminationCondition(boost::bind(&plannerOrTerminationConditionAux, c1, c2));
00216 }
00217 
00218 ompl::base::PlannerTerminationCondition ompl::base::plannerAndTerminationCondition(const PlannerTerminationCondition &c1, const PlannerTerminationCondition &c2)
00219 {
00220     return PlannerTerminationCondition(boost::bind(&plannerAndTerminationConditionAux, c1, c2));
00221 }
00222 
00223 ompl::base::PlannerTerminationCondition ompl::base::timedPlannerTerminationCondition(double duration)
00224 {
00225     return PlannerTerminationCondition(boost::bind(&timePassed, time::now() + time::seconds(duration)));
00226 }
00227 
00228 ompl::base::PlannerTerminationCondition ompl::base::timedPlannerTerminationCondition(double duration, double interval)
00229 {
00230     if (interval > duration)
00231         interval = duration;
00232     return PlannerTerminationCondition(boost::bind(&timePassed, time::now() + time::seconds(duration)), interval);
00233 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines