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/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
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
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
00109
00110
00111
00112
00113
00114
00115
00116
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 }