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 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
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096
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
00111
00112
00113
00114
00115
00116
00117
00118
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 }