00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include "ompl/base/PlannerTerminationCondition.h"
00038 #include "ompl/util/Time.h"
00039 #include <boost/bind.hpp>
00040 #include <boost/lambda/bind.hpp>
00041 #include <utility>
00042
00043 void ompl::base::PlannerTerminationCondition::terminate(void) const
00044 {
00045 terminate_ = true;
00046 }
00047
00048 bool ompl::base::PlannerTerminationCondition::eval(void) const
00049 {
00050 return fn_();
00051 }
00052
00053 ompl::base::PlannerNonTerminatingCondition::PlannerNonTerminatingCondition(void) : PlannerTerminationCondition(boost::lambda::constant(false))
00054 {
00055 }
00056
00057 ompl::base::PlannerAlwaysTerminatingCondition::PlannerAlwaysTerminatingCondition(void) : PlannerTerminationCondition(boost::lambda::constant(true))
00058 {
00059 }
00060
00062 namespace ompl
00063 {
00064 namespace base
00065 {
00066 static bool plannerOrTerminationCondition(const PlannerTerminationCondition &c1, const PlannerTerminationCondition &c2)
00067 {
00068 return c1() || c2();
00069 }
00070
00071 static bool plannerAndTerminationCondition(const PlannerTerminationCondition &c1, const PlannerTerminationCondition &c2)
00072 {
00073 return c1() && c2();
00074 }
00075
00076
00077 static bool timePassed(const time::point &endTime)
00078 {
00079 return time::now() > endTime;
00080 }
00081 }
00082 }
00084
00085 ompl::base::PlannerOrTerminationCondition::PlannerOrTerminationCondition(const PlannerTerminationCondition &c1, const PlannerTerminationCondition &c2) :
00086 PlannerTerminationCondition(boost::bind(&plannerOrTerminationCondition, c1, c2))
00087 {
00088 }
00089
00090 ompl::base::PlannerAndTerminationCondition::PlannerAndTerminationCondition(const PlannerTerminationCondition &c1, const PlannerTerminationCondition &c2) :
00091 PlannerTerminationCondition(boost::bind(&plannerAndTerminationCondition, c1, c2))
00092 {
00093 }
00094
00095 bool ompl::base::PlannerThreadedTerminationCondition::eval(void) const
00096 {
00097 return evalValue_;
00098 }
00099
00100 void ompl::base::PlannerThreadedTerminationCondition::startEvalThread(void)
00101 {
00102 if (!thread_)
00103 thread_ = new boost::thread(boost::bind(&PlannerThreadedTerminationCondition::periodicEval, this));
00104 }
00105
00106 void ompl::base::PlannerThreadedTerminationCondition::stopEvalThread(void)
00107 {
00108 if (thread_)
00109 {
00110 thread_->interrupt();
00111 thread_->join();
00112 delete thread_;
00113 thread_ = NULL;
00114 }
00115 }
00116
00117 bool ompl::base::PlannerThreadedTerminationCondition::computeEval(void)
00118 {
00119 return fn_();
00120 }
00121
00122 ompl::base::PlannerThreadedTerminationCondition::PlannerThreadedTerminationCondition(const PlannerTerminationConditionFn &fn, double period) :
00123 PlannerTerminationCondition(fn), thread_(NULL), evalValue_(terminate_), period_(period)
00124 {
00125 startEvalThread();
00126 }
00127
00128 ompl::base::PlannerThreadedTerminationCondition::~PlannerThreadedTerminationCondition(void)
00129 {
00130 terminate_ = true;
00131 stopEvalThread();
00132 }
00133
00134 void ompl::base::PlannerThreadedTerminationCondition::periodicEval(void)
00135 {
00136 time::duration s = time::seconds(period_);
00137 do
00138 {
00139 evalValue_ = computeEval();
00140 if ((*this)())
00141 break;
00142 boost::this_thread::sleep(s);
00143 } while (!(*this)());
00144 }
00145
00146 ompl::base::PlannerTerminationCondition ompl::base::timedPlannerTerminationCondition(double duration)
00147 {
00148 return PlannerTerminationCondition(boost::bind(&timePassed, time::now() + time::seconds(duration)));
00149 }
00150
00151 ompl::base::PlannerThreadedTerminationCondition ompl::base::timedPlannerTerminationCondition(double duration, double interval)
00152 {
00153 if (interval > duration)
00154 interval = duration;
00155 return PlannerThreadedTerminationCondition(boost::bind(&timePassed, time::now() + time::seconds(duration)), interval);
00156 }