All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
src/ompl/tools/config/src/SelfConfig.cpp
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2011, 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/tools/config/SelfConfig.h"
00038 #include "ompl/tools/config/MagicConstants.h"
00039 #include "ompl/util/Console.h"
00040 #include <boost/thread/mutex.hpp>
00041 #include <boost/shared_ptr.hpp>
00042 #include <algorithm>
00043 #include <limits>
00044 #include <cmath>
00045 #include <map>
00046 
00048 namespace ompl
00049 {
00050     namespace tools
00051     {
00052 
00053         class SelfConfig::SelfConfigImpl
00054         {
00055             friend class SelfConfig;
00056 
00057         public:
00058 
00059             SelfConfigImpl(const base::SpaceInformationPtr &si) :
00060                 si_(si), probabilityOfValidState_(-1.0), averageValidMotionLength_(-1.0)
00061             {
00062             }
00063 
00064             double getProbabilityOfValidState(void)
00065             {
00066                 checkSetup();
00067                 if (probabilityOfValidState_ < 0.0)
00068                     probabilityOfValidState_ = si_->probabilityOfValidState(magic::TEST_STATE_COUNT);
00069                 return probabilityOfValidState_;
00070             }
00071 
00072             double getAverageValidMotionLength(void)
00073             {
00074                 checkSetup();
00075                 if (averageValidMotionLength_ < 0.0)
00076                     averageValidMotionLength_ = si_->averageValidMotionLength(magic::TEST_STATE_COUNT);
00077                 return averageValidMotionLength_;
00078             }
00079 
00080             void configureValidStateSamplingAttempts(unsigned int &attempts)
00081             {
00082                 if (attempts == 0)
00083                     attempts = magic::MAX_VALID_SAMPLE_ATTEMPTS;
00084 
00085                 /*
00086                 static const double log_of_0_9 = -0.105360516;
00087                 if (attempts == 0)
00088                 {
00089                     double p = 1.0 - getProbabilityOfValidState();
00090                     if (p > 0.0)
00091                         attempts = std::min((unsigned int)std::max((int)ceil(log_of_0_9 / log(p)), 1),
00092                                             magic::MAX_VALID_SAMPLE_ATTEMPTS);
00093                     else
00094                         attempts = 1;
00095                     msg_.debug("Number of attempts made at sampling a valid state in space %s is computed to be %u",
00096                                si_->getStateSpace()->getName().c_str(), attempts);
00097                 }
00098                 */
00099             }
00100 
00101             void configurePlannerRange(double &range)
00102             {
00103                 if (range < std::numeric_limits<double>::epsilon())
00104                 {
00105                     range = si_->getMaximumExtent() * magic::MAX_MOTION_LENGTH_AS_SPACE_EXTENT_FRACTION;
00106                     logDebug("Planner range detected to be %lf", range);
00107                 }
00108 
00109                 /*
00110                 if (range < std::numeric_limits<double>::epsilon())
00111                 {
00112                     range = getAverageValidMotionLength() / 2.0;
00113                     double b = si_->getMaximumExtent() * magic::MAX_MOTION_LENGTH_AS_SPACE_EXTENT_FRACTION;
00114                     if (range < std::numeric_limits<double>::epsilon())
00115                         range = b;
00116                     else
00117                         range = std::min(range, b);
00118                     msg_.debug("Planner range detected to be %lf", range);
00119                 }
00120                 */
00121             }
00122 
00123             void configureProjectionEvaluator(base::ProjectionEvaluatorPtr &proj)
00124             {
00125                 checkSetup();
00126                 if (!proj)
00127                 {
00128                     logInform("Attempting to use default projection.");
00129                     proj = si_->getStateSpace()->getDefaultProjection();
00130                 }
00131                 if (!proj)
00132                     throw Exception("No projection evaluator specified");
00133                 proj->setup();
00134             }
00135 
00136             void print(std::ostream &out) const
00137             {
00138                 out << "Configuration parameters for space '" << si_->getStateSpace()->getName() << "'" << std::endl;
00139                 out << "   - probability of a valid state is " << probabilityOfValidState_ << std::endl;
00140                 out << "   - average length of a valid motion is " << averageValidMotionLength_ << std::endl;
00141             }
00142 
00143         private:
00144 
00145             void checkSetup(void)
00146             {
00147                 if (!si_->isSetup())
00148                 {
00149                     si_->setup();
00150                     probabilityOfValidState_ = -1.0;
00151                     averageValidMotionLength_ = -1.0;
00152                 }
00153             }
00154 
00155             base::SpaceInformationPtr si_;
00156             double                    probabilityOfValidState_;
00157             double                    averageValidMotionLength_;
00158 
00159             boost::mutex              lock_;
00160         };
00161 
00162     }
00163 }
00164 
00166 
00167 ompl::tools::SelfConfig::SelfConfig(const base::SpaceInformationPtr &si, const std::string &context) : context_(context)
00168 {
00169     typedef std::map<base::SpaceInformation*, boost::shared_ptr<SelfConfigImpl> > ConfigMap;
00170 
00171     static ConfigMap    SMAP;
00172     static boost::mutex LOCK;
00173 
00174     boost::mutex::scoped_lock smLock(LOCK);
00175     ConfigMap::const_iterator it = SMAP.find(si.get());
00176 
00177     if (it != SMAP.end())
00178         impl_ = it->second.get();
00179     else
00180     {
00181         impl_ = new SelfConfigImpl(si);
00182         SMAP[si.get()].reset(impl_);
00183     }
00184 }
00185 
00186 /* ------------------------------------------------------------------------ */
00187 
00188 double ompl::tools::SelfConfig::getProbabilityOfValidState(void)
00189 {
00190     boost::mutex::scoped_lock iLock(impl_->lock_);
00191     return impl_->getProbabilityOfValidState();
00192 }
00193 
00194 double ompl::tools::SelfConfig::getAverageValidMotionLength(void)
00195 {
00196     boost::mutex::scoped_lock iLock(impl_->lock_);
00197     return impl_->getAverageValidMotionLength();
00198 }
00199 
00200 void ompl::tools::SelfConfig::configureProjectionEvaluator(base::ProjectionEvaluatorPtr &proj)
00201 {
00202     boost::mutex::scoped_lock iLock(impl_->lock_);
00203     return impl_->configureProjectionEvaluator(proj);
00204 }
00205 
00206 void ompl::tools::SelfConfig::configureValidStateSamplingAttempts(unsigned int &attempts)
00207 {
00208     boost::mutex::scoped_lock iLock(impl_->lock_);
00209     impl_->configureValidStateSamplingAttempts(attempts);
00210 }
00211 
00212 void ompl::tools::SelfConfig::configurePlannerRange(double &range)
00213 {
00214     boost::mutex::scoped_lock iLock(impl_->lock_);
00215     impl_->configurePlannerRange(range);
00216 }
00217 
00218 void ompl::tools::SelfConfig::print(std::ostream &out) const
00219 {
00220     boost::mutex::scoped_lock iLock(impl_->lock_);
00221     impl_->print(out);
00222 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines