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(bool flag)
00044 {
00045 terminate_ = flag;
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, boost::cref(c1), boost::cref(c2)))
00087 {
00088 }
00089
00090 ompl::base::PlannerAndTerminationCondition::PlannerAndTerminationCondition(const PlannerTerminationCondition &c1, const PlannerTerminationCondition &c2) :
00091 PlannerTerminationCondition(boost::bind(&plannerAndTerminationCondition, boost::cref(c1), boost::cref(c2)))
00092 {
00093 }
00094
00095 bool ompl::base::PlannerThreadedTerminationCondition::eval(void) const
00096 {
00097 return evalValue_;
00098 }
00099
00100 void ompl::base::PlannerThreadedTerminationCondition::terminate(bool flag)
00101 {
00102 PlannerTerminationCondition::terminate(flag);
00103 if (terminate_)
00104 stopEvalThread();
00105 else
00106 startEvalThread();
00107 }
00108
00109 void ompl::base::PlannerThreadedTerminationCondition::startEvalThread(void)
00110 {
00111 if (!thread_)
00112 thread_ = new boost::thread(boost::bind(&PlannerThreadedTerminationCondition::periodicEval, this));
00113 }
00114
00115 void ompl::base::PlannerThreadedTerminationCondition::stopEvalThread(void)
00116 {
00117 if (thread_)
00118 {
00119 thread_->interrupt();
00120 thread_->join();
00121 delete thread_;
00122 thread_ = NULL;
00123 }
00124 }
00125
00126 bool ompl::base::PlannerThreadedTerminationCondition::computeEval(void)
00127 {
00128 return fn_();
00129 }
00130
00131 ompl::base::PlannerThreadedTerminationCondition::PlannerThreadedTerminationCondition(const PlannerTerminationConditionFn &fn, double period) :
00132 PlannerTerminationCondition(fn), thread_(NULL), evalValue_(terminate_), period_(period)
00133 {
00134 startEvalThread();
00135 }
00136
00137 ompl::base::PlannerThreadedTerminationCondition::~PlannerThreadedTerminationCondition(void)
00138 {
00139 terminate_ = true;
00140 stopEvalThread();
00141 }
00142
00143 void ompl::base::PlannerThreadedTerminationCondition::periodicEval(void)
00144 {
00145 time::duration s = time::seconds(period_);
00146 do
00147 {
00148 evalValue_ = computeEval();
00149 if ((*this)())
00150 break;
00151 boost::this_thread::sleep(s);
00152 } while (!(*this)());
00153 }
00154
00155 ompl::base::PlannerTerminationCondition ompl::base::timedPlannerTerminationCondition(double duration)
00156 {
00157 return PlannerTerminationCondition(boost::bind(&timePassed, time::now() + time::seconds(duration)));
00158 }