37#include "ompl/tools/config/SelfConfig.h"
38#include "ompl/tools/config/MagicConstants.h"
39#include "ompl/geometric/planners/rrt/RRTConnect.h"
40#include "ompl/geometric/planners/rrt/RRT.h"
41#include "ompl/geometric/planners/kpiece/LBKPIECE1.h"
42#include "ompl/geometric/planners/kpiece/KPIECE1.h"
43#include "ompl/control/planners/rrt/RRT.h"
44#include "ompl/control/planners/kpiece/KPIECE1.h"
57 class SelfConfig::SelfConfigImpl
59 friend class SelfConfig;
62 SelfConfigImpl(
const base::SpaceInformationPtr &si)
63 : wsi_(si), probabilityOfValidState_(-1.0), averageValidMotionLength_(-1.0)
67 double getProbabilityOfValidState()
69 base::SpaceInformationPtr si = wsi_.lock();
71 if (si && probabilityOfValidState_ < 0.0)
72 probabilityOfValidState_ = si->probabilityOfValidState(magic::TEST_STATE_COUNT);
73 return probabilityOfValidState_;
76 double getAverageValidMotionLength()
78 base::SpaceInformationPtr si = wsi_.lock();
80 if (si && averageValidMotionLength_ < 0.0)
81 averageValidMotionLength_ = si->averageValidMotionLength(magic::TEST_STATE_COUNT);
82 return averageValidMotionLength_;
85 void configureValidStateSamplingAttempts(
unsigned int &attempts)
88 attempts = magic::MAX_VALID_SAMPLE_ATTEMPTS;
91 void configurePlannerRange(
double &range,
const std::string &context)
93 if (range < std::numeric_limits<double>::epsilon())
95 base::SpaceInformationPtr si = wsi_.lock();
98 range = si->getMaximumExtent() * magic::MAX_MOTION_LENGTH_AS_SPACE_EXTENT_FRACTION;
99 OMPL_DEBUG(
"%sPlanner range detected to be %lf", context.c_str(), range);
102 OMPL_ERROR(
"%sUnable to detect planner range. SpaceInformation instance has expired.",
107 void configureProjectionEvaluator(base::ProjectionEvaluatorPtr &proj,
const std::string &context)
109 base::SpaceInformationPtr si = wsi_.lock();
113 OMPL_INFORM(
"%sAttempting to use default projection.", context.c_str());
114 proj = si->getStateSpace()->getDefaultProjection();
117 throw Exception(
"No projection evaluator specified");
121 void print(std::ostream &out)
const
123 base::SpaceInformationPtr si = wsi_.lock();
126 out <<
"Configuration parameters for space '" << si->getStateSpace()->getName() <<
"'" << std::endl;
127 out <<
" - probability of a valid state is " << probabilityOfValidState_ << std::endl;
128 out <<
" - average length of a valid motion is " << averageValidMotionLength_ << std::endl;
131 out <<
"EXPIRED" << std::endl;
136 return wsi_.expired();
140 void checkSetup(
const base::SpaceInformationPtr &si)
147 probabilityOfValidState_ = -1.0;
148 averageValidMotionLength_ = -1.0;
153 probabilityOfValidState_ = -1.0;
154 averageValidMotionLength_ = -1.0;
160 std::weak_ptr<base::SpaceInformation> wsi_;
162 double probabilityOfValidState_;
163 double averageValidMotionLength_;
170std::mutex ompl::tools::SelfConfig::staticConstructorLock_;
174 : context_(context.empty() ?
"" : context +
": ")
176 using ConfigMap = std::map<base::SpaceInformation *, std::shared_ptr<SelfConfigImpl>>;
178 std::unique_lock<std::mutex> smLock(staticConstructorLock_);
180 static ConfigMap SMAP;
183 auto dit = SMAP.begin();
184 while (dit != SMAP.end())
186 if (dit->second->expired())
192 const auto it = SMAP.find(si.get());
194 if (it != SMAP.end())
195 impl_ = it->second.get();
198 impl_ =
new SelfConfigImpl(si);
199 SMAP[si.get()].reset(impl_);
203ompl::tools::SelfConfig::~SelfConfig() =
default;
209 std::lock_guard<std::mutex> iLock(impl_->lock_);
210 return impl_->getProbabilityOfValidState();
215 std::lock_guard<std::mutex> iLock(impl_->lock_);
216 return impl_->getAverageValidMotionLength();
221 std::lock_guard<std::mutex> iLock(impl_->lock_);
222 impl_->configureValidStateSamplingAttempts(attempts);
227 std::lock_guard<std::mutex> iLock(impl_->lock_);
228 impl_->configurePlannerRange(range, context_);
233 std::lock_guard<std::mutex> iLock(impl_->lock_);
234 return impl_->configureProjectionEvaluator(proj, context_);
239 std::lock_guard<std::mutex> iLock(impl_->lock_);
245 base::PlannerPtr planner;
246 base::SpaceInformationPtr si(goal->getSpaceInformation());
247 const base::StateSpacePtr &space(si->getStateSpace());
248 control::SpaceInformationPtr siC(std::dynamic_pointer_cast<control::SpaceInformation, base::SpaceInformation>(si));
252 if (space->hasDefaultProjection())
253 planner = std::make_shared<control::KPIECE1>(siC);
256 planner = std::make_shared<control::RRT>(siC);
262 OMPL_WARN(
"No goal specified; choosing RRT as the default planner");
263 planner = std::make_shared<geometric::RRT>(goal->getSpaceInformation());
268 if (space->hasDefaultProjection())
269 planner = std::make_shared<geometric::LBKPIECE1>(goal->getSpaceInformation());
271 planner = std::make_shared<geometric::RRTConnect>(goal->getSpaceInformation());
277 if (space->hasDefaultProjection())
278 planner = std::make_shared<geometric::KPIECE1>(goal->getSpaceInformation());
280 planner = std::make_shared<geometric::RRT>(goal->getSpaceInformation());
284 throw Exception(
"Unable to allocate default planner");
The exception type for ompl.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
@ GOAL_SAMPLEABLE_REGION
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible.
Main namespace. Contains everything in this library.