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