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/GoalLazySamples.h"
00038 #include "ompl/base/ScopedState.h"
00039 #include "ompl/util/Time.h"
00040
00041 ompl::base::GoalLazySamples::GoalLazySamples(const SpaceInformationPtr &si, const GoalSamplingFn &samplerFunc, bool autoStart, double minDist) :
00042 GoalStates(si), samplerFunc_(samplerFunc), terminateSamplingThread_(false), samplingThread_(NULL), lastStateAdded_(false), minDist_(minDist)
00043 {
00044 type_ = GOAL_LAZY_SAMPLES;
00045 if (autoStart)
00046 startSampling();
00047 }
00048
00049 ompl::base::GoalLazySamples::~GoalLazySamples(void)
00050 {
00051 stopSampling();
00052 }
00053
00054 void ompl::base::GoalLazySamples::startSampling(void)
00055 {
00056 if (samplingThread_ == NULL)
00057 {
00058 terminateSamplingThread_ = false;
00059 samplingThread_ = new boost::thread(&GoalLazySamples::goalSamplingThread, this);
00060 }
00061 }
00062
00063 void ompl::base::GoalLazySamples::stopSampling(void)
00064 {
00065 if (isSampling())
00066 {
00067 terminateSamplingThread_ = true;
00068 samplingThread_->join();
00069 delete samplingThread_;
00070 samplingThread_ = NULL;
00071 }
00072 }
00073
00074 void ompl::base::GoalLazySamples::goalSamplingThread(void)
00075 {
00076
00077 while (!terminateSamplingThread_ && !si_->isSetup())
00078 boost::this_thread::sleep(time::seconds(0.01));
00079
00080 if (!terminateSamplingThread_ && samplerFunc_)
00081 {
00082 ScopedState<> s(si_);
00083 while (!terminateSamplingThread_ && samplerFunc_(this, s.get()))
00084 addStateIfDifferent(s.get(), minDist_);
00085 }
00086 terminateSamplingThread_ = true;
00087 }
00088
00089 bool ompl::base::GoalLazySamples::isSampling(void) const
00090 {
00091 return terminateSamplingThread_ == false && samplingThread_ != NULL;
00092 }
00093
00094 bool ompl::base::GoalLazySamples::canSample(void) const
00095 {
00096 return maxSampleCount() > 0 || (terminateSamplingThread_ == false && samplingThread_ != NULL);
00097 }
00098
00099 void ompl::base::GoalLazySamples::clear(void)
00100 {
00101 boost::mutex::scoped_lock slock(lock_);
00102 GoalStates::clear();
00103 }
00104
00105 double ompl::base::GoalLazySamples::distanceGoal(const State *st) const
00106 {
00107 boost::mutex::scoped_lock slock(lock_);
00108 return GoalStates::distanceGoal(st);
00109 }
00110
00111 void ompl::base::GoalLazySamples::sampleGoal(base::State *st) const
00112 {
00113 boost::mutex::scoped_lock slock(lock_);
00114 GoalStates::sampleGoal(st);
00115 }
00116
00117 void ompl::base::GoalLazySamples::addState(const State* st)
00118 {
00119 boost::mutex::scoped_lock slock(lock_);
00120 GoalStates::addState(st);
00121 }
00122
00123 bool ompl::base::GoalLazySamples::addStateIfDifferent(const State* st, double minDistance)
00124 {
00125 boost::mutex::scoped_lock slock(lock_);
00126 if (GoalStates::distanceGoal(st) > minDistance)
00127 {
00128 GoalStates::addState(st);
00129 lastStateAdded_ = true;
00130 }
00131 else
00132 lastStateAdded_ = false;
00133 return lastStateAdded_;
00134 }