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 msg_.debug("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 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 }