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/goals/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), samplingAttempts_(0), 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 logDebug("Starting goal sampling thread");
00059 terminateSamplingThread_ = false;
00060 samplingThread_ = new boost::thread(&GoalLazySamples::goalSamplingThread, this);
00061 }
00062 }
00063
00064 void ompl::base::GoalLazySamples::stopSampling(void)
00065 {
00066 if (isSampling())
00067 {
00068 logDebug("Attempting to stop goal sampling thread...");
00069 terminateSamplingThread_ = true;
00070 samplingThread_->join();
00071 delete samplingThread_;
00072 samplingThread_ = NULL;
00073 }
00074 else
00075 if (samplingThread_)
00076 {
00077 samplingThread_->join();
00078 delete samplingThread_;
00079 samplingThread_ = NULL;
00080 }
00081 }
00082
00083 void ompl::base::GoalLazySamples::goalSamplingThread(void)
00084 {
00085 if (!si_->isSetup())
00086 {
00087 logDebug("Waiting for space information to be set up before the sampling thread can begin computation...");
00088
00089 while (!terminateSamplingThread_ && !si_->isSetup())
00090 boost::this_thread::sleep(time::seconds(0.01));
00091 }
00092 unsigned int prevsa = samplingAttempts_;
00093 if (!terminateSamplingThread_ && samplerFunc_)
00094 {
00095 logDebug("Beginning sampling thread computation");
00096 ScopedState<> s(si_);
00097 while (!terminateSamplingThread_ && samplerFunc_(this, s.get()))
00098 {
00099 ++samplingAttempts_;
00100 if (si_->satisfiesBounds(s.get()) && si_->isValid(s.get()))
00101 addStateIfDifferent(s.get(), minDist_);
00102 }
00103 }
00104 else
00105 logWarn("Goal sampling thread never did any work.%s",
00106 samplerFunc_ ? (si_->isSetup() ? "" : " Space information not set up.") : " No sampling function set.");
00107 terminateSamplingThread_ = true;
00108 logDebug("Stopped goal sampling thread after %u sampling attempts", samplingAttempts_ - prevsa);
00109 }
00110
00111 bool ompl::base::GoalLazySamples::isSampling(void) const
00112 {
00113 return terminateSamplingThread_ == false && samplingThread_ != NULL;
00114 }
00115
00116 bool ompl::base::GoalLazySamples::couldSample(void) const
00117 {
00118 return canSample() || isSampling();
00119 }
00120
00121 void ompl::base::GoalLazySamples::clear(void)
00122 {
00123 boost::mutex::scoped_lock slock(lock_);
00124 GoalStates::clear();
00125 }
00126
00127 double ompl::base::GoalLazySamples::distanceGoal(const State *st) const
00128 {
00129 boost::mutex::scoped_lock slock(lock_);
00130 return GoalStates::distanceGoal(st);
00131 }
00132
00133 void ompl::base::GoalLazySamples::sampleGoal(base::State *st) const
00134 {
00135 boost::mutex::scoped_lock slock(lock_);
00136 GoalStates::sampleGoal(st);
00137 }
00138
00139 void ompl::base::GoalLazySamples::setNewStateCallback(const NewStateCallbackFn &callback)
00140 {
00141 callback_ = callback;
00142 }
00143
00144 void ompl::base::GoalLazySamples::addState(const State* st)
00145 {
00146 boost::mutex::scoped_lock slock(lock_);
00147 GoalStates::addState(st);
00148 }
00149
00150 const ompl::base::State* ompl::base::GoalLazySamples::getState(unsigned int index) const
00151 {
00152 boost::mutex::scoped_lock slock(lock_);
00153 return GoalStates::getState(index);
00154 }
00155
00156 bool ompl::base::GoalLazySamples::hasStates(void) const
00157 {
00158 boost::mutex::scoped_lock slock(lock_);
00159 return GoalStates::hasStates();
00160 }
00161
00162 std::size_t ompl::base::GoalLazySamples::getStateCount(void) const
00163 {
00164 boost::mutex::scoped_lock slock(lock_);
00165 return GoalStates::getStateCount();
00166 }
00167
00168 bool ompl::base::GoalLazySamples::addStateIfDifferent(const State* st, double minDistance)
00169 {
00170 const base::State *newState = NULL;
00171 bool added = false;
00172 {
00173 boost::mutex::scoped_lock slock(lock_);
00174 if (GoalStates::distanceGoal(st) > minDistance)
00175 {
00176 GoalStates::addState(st);
00177 added = true;
00178 if (callback_)
00179 newState = states_.back();
00180 }
00181 }
00182
00183
00184 if (newState)
00185 callback_(newState);
00186 return added;
00187 }