All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
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                     msg_.debug("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                     msg_.inform("Attempting to use default projection.");
00129                     proj = si_->getStateSpace()->getDefaultProjection();
00130                 }
00131                 if (!proj)
00132                     throw Exception(msg_.getPrefix(), "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             msg::Interface            msg_;
00161         };
00162 
00163     }
00164 }
00165 
00167 
00168 ompl::tools::SelfConfig::SelfConfig(const base::SpaceInformationPtr &si, const std::string &context) : context_(context)
00169 {
00170     typedef std::map<base::SpaceInformation*, boost::shared_ptr<SelfConfigImpl> > ConfigMap;
00171 
00172     static ConfigMap    SMAP;
00173     static boost::mutex LOCK;
00174 
00175     boost::mutex::scoped_lock smLock(LOCK);
00176     ConfigMap::const_iterator it = SMAP.find(si.get());
00177 
00178     if (it != SMAP.end())
00179         impl_ = it->second.get();
00180     else
00181     {
00182         impl_ = new SelfConfigImpl(si);
00183         SMAP[si.get()].reset(impl_);
00184     }
00185 }
00186 
00187 /* ------------------------------------------------------------------------ */
00188 
00189 #define SET_CONTEXT                                                \
00190     boost::mutex::scoped_lock iLock(impl_->lock_);                \
00191     impl_->msg_.setPrefix(context_)
00192 
00193 double ompl::tools::SelfConfig::getProbabilityOfValidState(void)
00194 {
00195     SET_CONTEXT;
00196     return impl_->getProbabilityOfValidState();
00197 }
00198 
00199 double ompl::tools::SelfConfig::getAverageValidMotionLength(void)
00200 {
00201     SET_CONTEXT;
00202     return impl_->getAverageValidMotionLength();
00203 }
00204 
00205 void ompl::tools::SelfConfig::configureProjectionEvaluator(base::ProjectionEvaluatorPtr &proj)
00206 {
00207     SET_CONTEXT;
00208     return impl_->configureProjectionEvaluator(proj);
00209 }
00210 
00211 void ompl::tools::SelfConfig::configureValidStateSamplingAttempts(unsigned int &attempts)
00212 {
00213     SET_CONTEXT;
00214     impl_->configureValidStateSamplingAttempts(attempts);
00215 }
00216 
00217 void ompl::tools::SelfConfig::configurePlannerRange(double &range)
00218 {
00219     SET_CONTEXT;
00220     impl_->configurePlannerRange(range);
00221 }
00222 
00223 void ompl::tools::SelfConfig::print(std::ostream &out) const
00224 {
00225     SET_CONTEXT;
00226     impl_->print(out);
00227 }
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends