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