ompl/base/goals/src/GoalLazySamples.cpp
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2010, Rice University
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Rice University nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 /* Author: Ioan Sucan */
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()
00050 {
00051     stopSampling();
00052 }
00053 
00054 void ompl::base::GoalLazySamples::startSampling()
00055 {
00056     if (samplingThread_ == NULL)
00057     {
00058         OMPL_DEBUG("Starting goal sampling thread");
00059         terminateSamplingThread_ = false;
00060         samplingThread_ = new boost::thread(&GoalLazySamples::goalSamplingThread, this);
00061     }
00062 }
00063 
00064 void ompl::base::GoalLazySamples::stopSampling()
00065 {
00066     if (isSampling())
00067     {
00068         OMPL_DEBUG("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         { // join a finished thread
00077             samplingThread_->join();
00078             delete samplingThread_;
00079             samplingThread_ = NULL;
00080         }
00081 }
00082 
00083 void ompl::base::GoalLazySamples::goalSamplingThread()
00084 {
00085     if (!si_->isSetup())
00086     {
00087         OMPL_DEBUG("Waiting for space information to be set up before the sampling thread can begin computation...");
00088         // wait for everything to be set up before performing computation
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         OMPL_DEBUG("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         OMPL_WARN("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     OMPL_DEBUG("Stopped goal sampling thread after %u sampling attempts", samplingAttempts_ - prevsa);
00109 }
00110 
00111 bool ompl::base::GoalLazySamples::isSampling() const
00112 {
00113     return terminateSamplingThread_ == false && samplingThread_ != NULL;
00114 }
00115 
00116 bool ompl::base::GoalLazySamples::couldSample() const
00117 {
00118     return canSample() || isSampling();
00119 }
00120 
00121 void ompl::base::GoalLazySamples::clear()
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() const
00157 {
00158     boost::mutex::scoped_lock slock(lock_);
00159     return GoalStates::hasStates();
00160 }
00161 
00162 std::size_t ompl::base::GoalLazySamples::getStateCount() 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     // the lock is released at this; if needed, issue a call to the callback
00184     if (newState)
00185         callback_(newState);
00186     return added;
00187 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines