void ompl::geometric::RRTstar::setup()
{
Planner::setup();
tools::SelfConfig sc(si_, getName());
sc.configurePlannerRange(maxDistance_);
if (!si_->getStateSpace()->hasSymmetricDistance() || !si_->getStateSpace()->hasSymmetricInterpolate())
{
OMPL_WARN("%s requires a state space with symmetric distance and symmetric interpolation.", getName().c_str());
}
if (!nn_)
nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(si_->getStateSpace()));
nn_->setDistanceFunction(boost::bind(&RRTstar::distanceFunction, this, _1, _2));
// Setup optimization objective
//
// If no optimization objective was specified, then default to
// optimizing path length as computed by the distance() function
// in the state space.
if (pdef_)
{
if (pdef_->hasOptimizationObjective())
opt_ = pdef_->getOptimizationObjective();
else
{
OMPL_INFORM("%s: No optimization objective specified. Defaulting to optimizing path length for the allowed planning time.", getName().c_str());
opt_.reset(new base::PathLengthOptimizationObjective(si_));
}
}
else
{
OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
setup_ = false;
}
}
void ompl::geometric::CForest::setup()
{
Planner::setup();
if (pdef_->hasOptimizationObjective())
opt_ = pdef_->getOptimizationObjective();
else
{
OMPL_INFORM("%s: No optimization objective specified. Defaulting to optimizing path length for the allowed "
"planning time.",
getName().c_str());
opt_ = std::make_shared<base::PathLengthOptimizationObjective>(si_);
}
bestCost_ = opt_->infiniteCost();
if (planners_.empty())
{
OMPL_INFORM("%s: Number and type of instances not specified. Defaulting to %d instances of RRTstar.",
getName().c_str(), numThreads_);
addPlannerInstances<RRTstar>(numThreads_);
}
for (auto &planner : planners_)
if (!planner->isSetup())
planner->setup();
// This call is needed to make sure the ParamSet is up to date after changes induced by the planner setup calls
// above, via the state space wrappers for CForest.
si_->setup();
}
bool ompl::tools::LightningDB::load(const std::string &fileName)
{
// Error checking
if (fileName.empty())
{
OMPL_ERROR("Empty filename passed to save function");
return false;
}
if ( !boost::filesystem::exists( fileName ) )
{
OMPL_WARN("Database file does not exist: %s", fileName.c_str());
return false;
}
// Load database from file, track loading time
time::point start = time::now();
OMPL_INFORM("Loading database from file: %s", fileName.c_str());
// Open a binary input stream
std::ifstream iStream(fileName.c_str(), std::ios::binary);
// Get the total number of paths saved
double numPaths = 0;
iStream >> numPaths;
// Check that the number of paths makes sense
if (numPaths < 0 || numPaths > std::numeric_limits<double>::max())
{
OMPL_WARN("Number of paths to load %d is a bad value", numPaths);
return false;
}
// Start loading all the PlannerDatas
for (std::size_t i = 0; i < numPaths; ++i)
{
// Create a new planner data instance
ompl::base::PlannerDataPtr plannerData(new ompl::base::PlannerData(si_));
// Note: the StateStorage class checks if the states match for us
plannerDataStorage_.load(iStream, *plannerData.get());
// Add to nearest neighbor tree
nn_->add(plannerData);
}
// Close file
iStream.close();
double loadTime = time::seconds(time::now() - start);
OMPL_INFORM("Loaded database from file in %f sec with %d paths", loadTime, nn_->size());
return true;
}
// we provide a duplicate implementation here to allow the planner to choose how the time is turned into a planner termination condition
ompl::base::PlannerStatus ompl::control::SimpleSetup::solve(double time)
{
setup();
last_status_ = base::PlannerStatus::UNKNOWN;
time::point start = time::now();
last_status_ = planner_->solve(time);
planTime_ = time::seconds(time::now() - start);
if (last_status_)
OMPL_INFORM("Solution found in %f seconds", planTime_);
else
OMPL_INFORM("No solution found after %f seconds", planTime_);
return last_status_;
}
ompl::base::PlannerStatus ompl::geometric::SimpleSetup::solve(const base::PlannerTerminationCondition &ptc)
{
setup();
lastStatus_ = base::PlannerStatus::UNKNOWN;
time::point start = time::now();
lastStatus_ = planner_->solve(ptc);
planTime_ = time::seconds(time::now() - start);
if (lastStatus_)
OMPL_INFORM("Solution found in %f seconds", planTime_);
else
OMPL_INFORM("No solution found after %f seconds", planTime_);
return lastStatus_;
}
void ompl::geometric::RRTXstatic::setup()
{
Planner::setup();
tools::SelfConfig sc(si_, getName());
sc.configurePlannerRange(maxDistance_);
if (!si_->getStateSpace()->hasSymmetricDistance() || !si_->getStateSpace()->hasSymmetricInterpolate())
{
OMPL_WARN("%s requires a state space with symmetric distance and symmetric interpolation.", getName().c_str());
}
if (!nn_)
nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion *>(this));
nn_->setDistanceFunction([this](const Motion *a, const Motion *b) { return distanceFunction(a, b); });
// Setup optimization objective
//
// If no optimization objective was specified, then default to
// optimizing path length as computed by the distance() function
// in the state space.
if (pdef_)
{
if (pdef_->hasOptimizationObjective())
opt_ = pdef_->getOptimizationObjective();
else
{
OMPL_INFORM("%s: No optimization objective specified. Defaulting to optimizing path length for the allowed "
"planning time.",
getName().c_str());
opt_ = std::make_shared<base::PathLengthOptimizationObjective>(si_);
// Store the new objective in the problem def'n
pdef_->setOptimizationObjective(opt_);
}
mc_ = MotionCompare(opt_, pdef_);
q_ = BinaryHeap<Motion *, MotionCompare>(mc_);
}
else
{
OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
setup_ = false;
}
// Calculate some constants:
calculateRewiringLowerBounds();
// Set the bestCost_ and prunedCost_ as infinite
bestCost_ = opt_->infiniteCost();
}
void ompl::geometric::RRTstar::setup()
{
Planner::setup();
tools::SelfConfig sc(si_, getName());
sc.configurePlannerRange(maxDistance_);
if (!si_->getStateSpace()->hasSymmetricDistance() || !si_->getStateSpace()->hasSymmetricInterpolate())
{
OMPL_WARN("%s requires a state space with symmetric distance and symmetric interpolation.", getName().c_str());
}
if (!nn_)
nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(this));
nn_->setDistanceFunction(std::bind(&RRTstar::distanceFunction, this, std::placeholders::_1, std::placeholders::_2));
// Setup optimization objective
//
// If no optimization objective was specified, then default to
// optimizing path length as computed by the distance() function
// in the state space.
if (pdef_)
{
if (pdef_->hasOptimizationObjective())
opt_ = pdef_->getOptimizationObjective();
else
{
OMPL_INFORM("%s: No optimization objective specified. Defaulting to optimizing path length for the allowed planning time.", getName().c_str());
opt_.reset(new base::PathLengthOptimizationObjective(si_));
// Store the new objective in the problem def'n
pdef_->setOptimizationObjective(opt_);
}
}
else
{
OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
setup_ = false;
}
// Get the measure of the entire space:
prunedMeasure_ = si_->getSpaceMeasure();
// Calculate some constants:
calculateRewiringLowerBounds();
// Set the bestCost_ and prunedCost_ as infinite
bestCost_ = opt_->infiniteCost();
prunedCost_ = opt_->infiniteCost();
}
bool ompl::tools::ThunderDB::addPath(ompl::geometric::PathGeometric &solutionPath, double &insertionTime)
{
// Error check
if (!spars_)
{
OMPL_ERROR("SPARSdb planner has not been passed into the ThunderDB yet");
insertionTime = 0;
return false;
}
// Prevent inserting into database
if (!saving_enabled_)
{
OMPL_WARN("ThunderDB: Saving is disabled so not adding path");
return false;
}
bool result;
double seconds = 120; // 10; // a large number, should never need to use this
ompl::base::PlannerTerminationCondition ptc = ompl::base::timedPlannerTerminationCondition(seconds, 0.1);
// Benchmark runtime
time::point startTime = time::now();
{
result = spars_->addPathToRoadmap(ptc, solutionPath);
}
insertionTime = time::seconds(time::now() - startTime);
OMPL_INFORM("SPARSdb now has %d states", spars_->getNumVertices());
// Record this new addition
numPathsInserted_++;
return result;
}
void ompl::geometric::SPARStwo::setup()
{
Planner::setup();
if (!nn_)
nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(si_->getStateSpace()));
nn_->setDistanceFunction(boost::bind(&SPARStwo::distanceFunction, this, _1, _2));
double maxExt = si_->getMaximumExtent();
sparseDelta_ = sparseDeltaFraction_ * maxExt;
denseDelta_ = denseDeltaFraction_ * maxExt;
// Setup optimization objective
//
// If no optimization objective was specified, then default to
// optimizing path length as computed by the distance() function
// in the state space.
if (pdef_)
{
if (pdef_->hasOptimizationObjective())
{
opt_ = pdef_->getOptimizationObjective();
if (!dynamic_cast<base::PathLengthOptimizationObjective*>(opt_.get()))
OMPL_WARN("%s: Asymptotic optimality has only been proven with path length optimizaton; convergence for other optimizaton objectives is not guaranteed.", getName().c_str());
}
else
opt_.reset(new base::PathLengthOptimizationObjective(si_));
}
else
{
OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
setup_ = false;
}
}
ompl::base::PlannerStatus ompl::geometric::CForest::solve(const base::PlannerTerminationCondition &ptc)
{
using solveFunctionType = void (ompl::geometric::CForest::*)(base::Planner *, const base::PlannerTerminationCondition &);
checkValidity();
time::point start = time::now();
std::vector<std::thread*> threads(planners_.size());
const base::ReportIntermediateSolutionFn prevSolutionCallback = getProblemDefinition()->getIntermediateSolutionCallback();
if (prevSolutionCallback)
OMPL_WARN("Cannot use previously set intermediate solution callback with %s", getName().c_str());
pdef_->setIntermediateSolutionCallback(std::bind(&CForest::newSolutionFound, this,
std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
bestCost_ = opt_->infiniteCost();
// run each planner in its own thread, with the same ptc.
for (std::size_t i = 0 ; i < threads.size() ; ++i)
threads[i] = new std::thread(std::bind((solveFunctionType)&CForest::solve, this, planners_[i].get(), ptc));
for (auto & thread : threads)
{
thread->join();
delete thread;
}
// restore callback
getProblemDefinition()->setIntermediateSolutionCallback(prevSolutionCallback);
OMPL_INFORM("Solution found in %f seconds", time::seconds(time::now() - start));
return base::PlannerStatus(pdef_->hasSolution(), pdef_->hasApproximateSolution());
}
void ompl::control::SimpleSetup::setup()
{
if (!configured_ || !si_->isSetup() || !planner_->isSetup())
{
if (!si_->isSetup())
si_->setup();
if (!planner_)
{
if (pa_)
planner_ = pa_(si_);
if (!planner_)
{
OMPL_INFORM("No planner specified. Using default.");
planner_ = getDefaultPlanner(getGoal());
}
}
planner_->setProblemDefinition(pdef_);
if (!planner_->isSetup())
planner_->setup();
params_.clear();
params_.include(si_->params());
params_.include(planner_->params());
configured_ = true;
}
}
void ompl::geometric::RRTstar::setSampleRejection(const bool reject)
{
if (static_cast<bool>(opt_) == true)
{
if (opt_->hasCostToGoHeuristic() == false)
{
OMPL_INFORM("%s: No cost-to-go heuristic set. Informed techniques will not work well.", getName().c_str());
}
}
// This option is mutually exclusive with setSampleRejection, assert that:
if (reject == true && useInformedSampling_ == true)
{
OMPL_ERROR("%s: InformedSampling and SampleRejection are mutually exclusive options.", getName().c_str());
}
// Check if we're changing the setting of rejection sampling. If we are, we will need to create a new sampler, which we only want to do if one is already allocated.
if (reject != useRejectionSampling_)
{
// Store the setting
useRejectionSampling_ = reject;
// If we currently have a sampler, we need to make a new one
if (sampler_ || infSampler_)
{
// Reset the samplers
sampler_.reset();
infSampler_.reset();
// Create the sampler
allocSampler();
}
}
}
bool plan(unsigned int start_row, unsigned int start_col, unsigned int goal_row, unsigned int goal_col)
{
if (!ss_)
return false;
ob::ScopedState<> start(ss_->getStateSpace());
start[0] = start_row;
start[1] = start_col;
ob::ScopedState<> goal(ss_->getStateSpace());
goal[0] = goal_row;
goal[1] = goal_col;
ss_->setStartAndGoalStates(start, goal);
// generate a few solutions; all will be added to the goal;
for (int i = 0 ; i < 10 ; ++i)
{
if (ss_->getPlanner())
ss_->getPlanner()->clear();
ss_->solve();
}
const std::size_t ns = ss_->getProblemDefinition()->getSolutionCount();
OMPL_INFORM("Found %d solutions", (int)ns);
if (ss_->haveSolutionPath())
{
ss_->simplifySolution();
og::PathGeometric &p = ss_->getSolutionPath();
ss_->getPathSimplifier()->simplifyMax(p);
ss_->getPathSimplifier()->smoothBSpline(p);
return true;
}
return false;
}
bool ompl::tools::ThunderDB::saveIfChanged(const std::string &fileName)
{
if (numPathsInserted_)
return save(fileName);
else
OMPL_INFORM("Not saving because database has not changed");
return true;
}
bool ompl::tools::ThunderDB::findNearestStartGoal(int nearestK, const base::State *start, const base::State *goal,
ompl::geometric::SPARSdb::CandidateSolution &candidateSolution,
const base::PlannerTerminationCondition &ptc)
{
bool result = spars_->getSimilarPaths(nearestK, start, goal, candidateSolution, ptc);
if (!result)
{
OMPL_INFORM("RETRIEVE COULD NOT FIND SOLUTION ");
OMPL_INFORM("spars::getSimilarPaths() returned false - retrieve could not find solution");
return false;
}
OMPL_INFORM("spars::getSimilarPaths() returned true - found a solution of size %d",
candidateSolution.getStateCount());
return true;
}
bool ompl::tools::LightningDB::saveIfChanged(const std::string &fileName)
{
if (numUnsavedPaths_)
return save(fileName);
else
OMPL_INFORM("Not saving because database has not changed");
return true;
}
bool plan()
{
if (!lightning_)
return false;
ob::ScopedState<> start(lightning_->getStateSpace());
vss_->sample(start.get());
ob::ScopedState<> goal(lightning_->getStateSpace());
vss_->sample(goal.get());
lightning_->setStartAndGoalStates(start, goal);
bool solved = lightning_->solve(10.);
if (solved)
OMPL_INFORM("Found solution in %g seconds",
lightning_->getLastPlanComputationTime());
else
OMPL_INFORM("No solution found");
return false;
}
ob::PlannerStatus KrisLibraryOMPLPlanner::solve (const ob::PlannerTerminationCondition &ptc)
{
if(!planner) {
//may have had a previous clear() call
//fprintf(stderr,"KrisLibraryOMPLPlanner::solve(): Warning, setup() not called yet\n");
setup();
if(!planner)
return ob::PlannerStatus(ob::PlannerStatus::UNKNOWN);
}
ob::ProblemDefinitionPtr pdef = this->getProblemDefinition();
//how much to plan?
int increment = (StartsWith(factory.type.c_str(),"fmm") ? 1 : 50);
Real oldBest = Inf;
bool optimizing = planner->IsOptimizing();
double desiredCost = 0;
if(optimizing) {
if(pdef->getOptimizationObjective() != NULL)
desiredCost = pdef->getOptimizationObjective()->getCostThreshold().value();
else
OMPL_INFORM("%s: No optimization objective specified. Defaulting to optimizing path length for the allowed planning time.", getName().c_str());
}
while(!ptc()) {
if(planner->IsSolved()) {
//convert solution to OMPL solution
MilestonePath path;
planner->GetSolution(path);
if(optimizing) {
//optimizing
Real cost = path.Length();
if(cost < oldBest) {
oldBest = cost;
if(cost < desiredCost) {
ob::PathPtr pptr(ToOMPL(si_,path));
this->getProblemDefinition()->addSolutionPath(pptr);
return ob::PlannerStatus(true,false);
}
}
}
else {
//non-optimizing
ob::PathPtr pptr(ToOMPL(si_,path));
this->getProblemDefinition()->addSolutionPath(pptr);
return ob::PlannerStatus(true,false);
}
}
planner->PlanMore(increment);
}
if(planner->IsSolved()) {
//convert solution to OMPL solution
MilestonePath path;
planner->GetSolution(path);
ob::PathPtr pptr(ToOMPL(si_,path));
this->getProblemDefinition()->addSolutionPath(pptr);
return ob::PlannerStatus(true,false);
}
return ob::PlannerStatus(false,false);
}
bool ompl::tools::LightningDB::save(const std::string &fileName)
{
// Error checking
if (fileName.empty())
{
OMPL_ERROR("Empty filename passed to save function");
return false;
}
// Save database from file, track saving time
time::point start = time::now();
OMPL_INFORM("Saving database to file: %s", fileName.c_str());
// Open a binary output stream
std::ofstream outStream(fileName.c_str(), std::ios::binary);
// Convert the NN tree to a vector
std::vector<ompl::base::PlannerDataPtr> plannerDatas;
nn_->list(plannerDatas);
// Write the number of paths we will be saving
double numPaths = plannerDatas.size();
outStream << numPaths;
// Start saving each planner data object
for (std::size_t i = 0; i < numPaths; ++i)
{
ompl::base::PlannerData &pd = *plannerDatas[i].get();
// Save a single planner data
plannerDataStorage_.store(pd, outStream);
}
// Close file
outStream.close();
// Benchmark
double loadTime = time::seconds(time::now() - start);
OMPL_INFORM("Saved database to file in %f sec with %d paths", loadTime, plannerDatas.size());
numUnsavedPaths_ = 0;
return true;
}
void ompl::geometric::RRTstar::allocSampler()
{
// Allocate the appropriate type of sampler.
if (useInformedSampling_)
{
// We are using informed sampling, this can end-up reverting to rejection sampling in some cases
OMPL_INFORM("%s: Using informed sampling.", getName().c_str());
infSampler_ = opt_->allocInformedStateSampler(pdef_, numSampleAttempts_);
}
else if (useRejectionSampling_)
{
// We are explicitly using rejection sampling.
OMPL_INFORM("%s: Using rejection sampling.", getName().c_str());
infSampler_ = std::make_shared<base::RejectionInfSampler>(pdef_, numSampleAttempts_);
}
else
{
// We are using a regular sampler
sampler_ = si_->allocStateSampler();
}
}
void plan(KoulesSetup& ks, double maxTime, const std::string& outputFile)
{
if (ks.solve(maxTime))
{
std::ofstream out(outputFile.c_str());
oc::PathControl path(ks.getSolutionPath());
path.interpolate();
if (!path.check())
OMPL_ERROR("Path is invalid");
writeParams(out);
path.printAsMatrix(out);
if (!ks.haveExactSolutionPath())
OMPL_INFORM("Solution is approximate. Distance to actual goal is %g",
ks.getProblemDefinition()->getSolutionDifference());
OMPL_INFORM("Output saved in %s", outputFile.c_str());
}
#if 0
// Get the planner data, save the ship's (x,y) coordinates to one file and
// the edge information to another file. This can be used for debugging
// purposes; plotting the tree of states might give you some idea of
// a planner's strategy.
ob::PlannerData pd(ks.getSpaceInformation());
ks.getPlannerData(pd);
std::ofstream vertexFile((outputFile + "-vertices").c_str()), edgeFile((outputFile + "-edges").c_str());
double* coords;
unsigned numVerts = pd.numVertices();
std::vector<unsigned int> edgeList;
for (unsigned int i = 0; i < numVerts; ++i)
{
coords = pd.getVertex(i).getState()->as<KoulesStateSpace::StateType>()->values;
vertexFile << coords[0] << ' ' << coords[1] << '\n';
pd.getEdges(i, edgeList);
for (unsigned int j = 0; j < edgeList.size(); ++j)
edgeFile << i << ' ' << edgeList[j] << '\n';
}
#endif
}
void configureProjectionEvaluator(base::ProjectionEvaluatorPtr &proj, const std::string &context)
{
base::SpaceInformationPtr si = wsi_.lock();
checkSetup(si);
if (!proj && si)
{
OMPL_INFORM("%sAttempting to use default projection.", context.c_str());
proj = si->getStateSpace()->getDefaultProjection();
}
if (!proj)
throw Exception("No projection evaluator specified");
proj->setup();
}
void ompl::base::Planner::setup()
{
if (!si_->isSetup())
{
OMPL_INFORM("%s: Space information setup was not yet called. Calling now.", getName().c_str());
si_->setup();
}
if (setup_)
OMPL_WARN("%s: Planner setup called multiple times", getName().c_str());
else
setup_ = true;
}
void benchmark(KoulesSetup& ks, ot::Benchmark::Request request,
const std::string& plannerName, const std::string& outputFile)
{
// Create a benchmark class
ompl::tools::Benchmark b(ks, "Koules experiment");
// Add the planner to evaluate
b.addPlanner(ks.getConfiguredPlannerInstance(plannerName));
// Start benchmark
b.benchmark(request);
// Save the results
b.saveResultsToFile(outputFile.c_str());
OMPL_INFORM("Output saved in %s", outputFile.c_str());
}
void ompl::geometric::SPARS::setup()
{
Planner::setup();
if (!nn_)
nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<DenseVertex>(this));
nn_->setDistanceFunction([this](const DenseVertex a, const DenseVertex b)
{
return distanceFunction(a, b);
});
if (!snn_)
snn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<SparseVertex>(this));
snn_->setDistanceFunction([this](const SparseVertex a, const SparseVertex b)
{
return sparseDistanceFunction(a, b);
});
if (!connectionStrategy_)
connectionStrategy_ = KStarStrategy<DenseVertex>(
[this]
{
return milestoneCount();
},
nn_, si_->getStateDimension());
double maxExt = si_->getMaximumExtent();
sparseDelta_ = sparseDeltaFraction_ * maxExt;
denseDelta_ = denseDeltaFraction_ * maxExt;
// Setup optimization objective
//
// If no optimization objective was specified, then default to
// optimizing path length as computed by the distance() function
// in the state space.
if (pdef_)
{
if (pdef_->hasOptimizationObjective())
{
opt_ = pdef_->getOptimizationObjective();
if (!dynamic_cast<base::PathLengthOptimizationObjective *>(opt_.get()))
OMPL_WARN("%s: Asymptotic optimality has only been proven with path length optimizaton; convergence "
"for other optimizaton objectives is not guaranteed.",
getName().c_str());
}
else
opt_ = std::make_shared<base::PathLengthOptimizationObjective>(si_);
}
else
{
OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
setup_ = false;
}
}
void ompl::geometric::FMT::setup()
{
if (pdef_)
{
/* Setup the optimization objective. If no optimization objective was
specified, then default to optimizing path length as computed by the
distance() function in the state space */
if (pdef_->hasOptimizationObjective())
opt_ = pdef_->getOptimizationObjective();
else
{
OMPL_INFORM("%s: No optimization objective specified. Defaulting to optimizing path length.", getName().c_str());
opt_.reset(new base::PathLengthOptimizationObjective(si_));
// Store the new objective in the problem def'n
pdef_->setOptimizationObjective(opt_);
}
Open_.getComparisonOperator().opt_ = opt_.get();
Open_.getComparisonOperator().heuristics_ = heuristics_;
if (!nn_)
nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(this));
nn_->setDistanceFunction(std::bind(&FMT::distanceFunction, this,
std::placeholders::_1, std::placeholders::_2));
if (nearestK_ && !nn_->reportsSortedResults())
{
OMPL_WARN("%s: NearestNeighbors datastructure does not return sorted solutions. Nearest K strategy disabled.", getName().c_str());
nearestK_ = false;
}
}
else
{
OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
setup_ = false;
}
}
void benchmark(KoulesSetup& ks, ot::Benchmark::Request request,
const std::string& plannerName, const std::string& outputFile)
{
// Create a benchmark class
ompl::tools::Benchmark b(ks, "Koules");
b.addExperimentParameter("num_koules", "INTEGER", boost::lexical_cast<std::string>(
(ks.getStateSpace()->getDimension() - 5) / 4));
// Add the planner to evaluate
b.addPlanner(ks.getConfiguredPlannerInstance(plannerName));
// Start benchmark
b.benchmark(request);
// Save the results
b.saveResultsToFile(outputFile.c_str());
OMPL_INFORM("Output saved in %s", outputFile.c_str());
}
void ompl::geometric::LightningRetrieveRepair::getPlannerData(base::PlannerData &data) const
{
OMPL_INFORM("LightningRetrieveRepair: including %d similar paths", nearestPaths_.size());
// Visualize the n candidate paths that we recalled from the database
for (std::size_t i = 0 ; i < nearestPaths_.size() ; ++i)
{
ompl::base::PlannerDataPtr pd = nearestPaths_[i];
for (std::size_t j = 1; j < pd->numVertices(); ++j)
{
data.addEdge(
base::PlannerDataVertex(pd->getVertex(j - 1).getState()),
base::PlannerDataVertex(pd->getVertex(j).getState()));
}
}
}
void ompl::tools::Lightning::initialize()
{
recallEnabled_ = true;
scratchEnabled_ = true;
// Load dynamic time warp
dtw_.reset(new ot::DynamicTimeWarp(si_));
// Load the experience database
experienceDB_.reset(new ompl::tools::LightningDB(si_->getStateSpace()));
// Load the Retrieve repair database. We do it here so that setRepairPlanner() works
rrPlanner_ = ob::PlannerPtr(new og::LightningRetrieveRepair(si_, experienceDB_));
OMPL_INFORM("Lightning Framework initialized.");
}
void ompl::geometric::RRTstar::setInformedSampling(bool informedSampling)
{
if (static_cast<bool>(opt_) == true)
{
if (opt_->hasCostToGoHeuristic() == false)
{
OMPL_INFORM("%s: No cost-to-go heuristic set. Informed techniques will not work well.", getName().c_str());
}
}
// This option is mutually exclusive with setSampleRejection, assert that:
if (informedSampling == true && useRejectionSampling_ == true)
{
OMPL_ERROR("%s: InformedSampling and SampleRejection are mutually exclusive options.", getName().c_str());
}
// If we just disabled tree pruning, but we are using prunedMeasure, we need to disable that as it required myself
if (informedSampling == false && getPrunedMeasure() == true)
{
setPrunedMeasure(false);
}
// Check if we're changing the setting of informed sampling. If we are, we will need to create a new sampler, which we only want to do if one is already allocated.
if (informedSampling != useInformedSampling_)
{
//If we're disabled informedSampling, and prunedMeasure is enabled, we need to disable that
if (informedSampling == false && usePrunedMeasure_ == true)
{
setPrunedMeasure(false);
}
// Store the value
useInformedSampling_ = informedSampling;
// If we currently have a sampler, we need to make a new one
if (sampler_ || infSampler_)
{
// Reset the samplers
sampler_.reset();
infSampler_.reset();
// Create the sampler
allocSampler();
}
}
}