37 #include "ompl/tools/config/SelfConfig.h"
38 #include "ompl/tools/config/MagicConstants.h"
40 #include <boost/thread/mutex.hpp>
41 #include <boost/shared_ptr.hpp>
42 #include <boost/weak_ptr.hpp>
54 class SelfConfig::SelfConfigImpl
60 SelfConfigImpl(
const base::SpaceInformationPtr &si) :
61 wsi_(si), probabilityOfValidState_(-1.0), averageValidMotionLength_(-1.0)
67 base::SpaceInformationPtr si = wsi_.lock();
69 if (si && probabilityOfValidState_ < 0.0)
71 return probabilityOfValidState_;
76 base::SpaceInformationPtr si = wsi_.lock();
78 if (si && averageValidMotionLength_ < 0.0)
80 return averageValidMotionLength_;
91 if (range < std::numeric_limits<double>::epsilon())
93 base::SpaceInformationPtr si = wsi_.lock();
97 OMPL_DEBUG(
"%sPlanner range detected to be %lf", context.c_str(), range);
100 OMPL_ERROR(
"%sUnable to detect planner range. SpaceInformation instance has expired.", context.c_str());
106 base::SpaceInformationPtr si = wsi_.lock();
110 OMPL_INFORM(
"%sAttempting to use default projection.", context.c_str());
111 proj = si->getStateSpace()->getDefaultProjection();
114 throw Exception(
"No projection evaluator specified");
118 void print(std::ostream &out)
const
120 base::SpaceInformationPtr si = wsi_.lock();
123 out <<
"Configuration parameters for space '" << si->getStateSpace()->getName() <<
"'" << std::endl;
124 out <<
" - probability of a valid state is " << probabilityOfValidState_ << std::endl;
125 out <<
" - average length of a valid motion is " << averageValidMotionLength_ << std::endl;
128 out <<
"EXPIRED" << std::endl;
131 bool expired(
void)
const
133 return wsi_.expired();
138 void checkSetup(
const base::SpaceInformationPtr &si)
145 probabilityOfValidState_ = -1.0;
146 averageValidMotionLength_ = -1.0;
151 probabilityOfValidState_ = -1.0;
152 averageValidMotionLength_ = -1.0;
158 boost::weak_ptr<base::SpaceInformation> wsi_;
160 double probabilityOfValidState_;
161 double averageValidMotionLength_;
172 context_(context.empty() ?
"" : context +
": ")
174 typedef std::map<base::SpaceInformation*, boost::shared_ptr<SelfConfigImpl> > ConfigMap;
176 static ConfigMap SMAP;
177 static boost::mutex LOCK;
179 boost::mutex::scoped_lock smLock(LOCK);
182 ConfigMap::iterator dit = SMAP.begin();
183 while (dit != SMAP.end())
185 if (dit->second->expired())
191 ConfigMap::const_iterator it = SMAP.find(si.get());
193 if (it != SMAP.end())
194 impl_ = it->second.get();
197 impl_ =
new SelfConfigImpl(si);
198 SMAP[si.get()].reset(impl_);
202 ompl::tools::SelfConfig::~SelfConfig(
void)
210 boost::mutex::scoped_lock iLock(impl_->lock_);
211 return impl_->getProbabilityOfValidState();
216 boost::mutex::scoped_lock iLock(impl_->lock_);
217 return impl_->getAverageValidMotionLength();
222 boost::mutex::scoped_lock iLock(impl_->lock_);
223 impl_->configureValidStateSamplingAttempts(attempts);
228 boost::mutex::scoped_lock iLock(impl_->lock_);
229 impl_->configurePlannerRange(range, context_);
234 boost::mutex::scoped_lock iLock(impl_->lock_);
235 return impl_->configureProjectionEvaluator(proj, context_);
240 boost::mutex::scoped_lock iLock(impl_->lock_);
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
A boost shared pointer wrapper for ompl::base::ProjectionEvaluator.
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
static const unsigned int TEST_STATE_COUNT
When multiple states need to be generated as part of the computation of various information (usually ...
static const double MAX_MOTION_LENGTH_AS_SPACE_EXTENT_FRACTION
For planners: if default values are to be used for the maximum length of motions, this constant defin...
static const unsigned int MAX_VALID_SAMPLE_ATTEMPTS
When multiple attempts are needed to generate valid samples, this value defines the default number of...
#define OMPL_INFORM(fmt,...)
Log a formatted information string.