38 #include "ompl/geometric/planners/bitstar/BITstar.h" 47 #include <boost/range/adaptor/reversed.hpp> 52 #include "ompl/util/Exception.h" 54 #include "ompl/geometric/PathGeometric.h" 56 #include "ompl/base/objectives/PathLengthOptimizationObjective.h" 60 #include "ompl/geometric/planners/bitstar/datastructures/IdGenerator.h" 62 #include "ompl/geometric/planners/bitstar/datastructures/Vertex.h" 64 #include "ompl/geometric/planners/bitstar/datastructures/CostHelper.h" 66 #include "ompl/geometric/planners/bitstar/datastructures/ImplicitGraph.h" 68 #include "ompl/geometric/planners/bitstar/datastructures/SearchQueue.h" 77 :
ompl::base::Planner(si, name)
80 OMPL_WARN(
"%s: Compiled with debug-level asserts.", Planner::getName().c_str());
81 #endif // BITSTAR_DEBUG 85 costHelpPtr_ = std::make_shared<CostHelper>();
86 graphPtr_ = std::make_shared<ImplicitGraph>([
this]()
90 queuePtr_ = std::make_shared<SearchQueue>([
this]()
94 queuePtr_->setPruneDuringResort(usePruning_);
97 if (graphPtr_->getUseKNearest() && Planner::getName() ==
"BITstar")
100 Planner::setName(
"kBITstar");
102 else if (!graphPtr_->getUseKNearest() && Planner::getName() ==
"kBITstar")
105 Planner::setName(
"BITstar");
111 Planner::specs_.multithreaded =
false;
113 Planner::specs_.approximateSolutions = graphPtr_->getTrackApproximateSolutions();
114 Planner::specs_.optimizingPaths =
true;
115 Planner::specs_.directed =
true;
116 Planner::specs_.provingSolutionNonExistence =
false;
117 Planner::specs_.canReportIntermediateSolutions =
true;
128 Planner::declareParam<double>(
"prune_threshold_as_fractional_cost_change",
this,
131 Planner::declareParam<bool>(
"delay_rewiring_to_first_solution",
this,
148 return bestCostProgressProperty();
152 return bestLengthProgressProperty();
156 return currentFreeProgressProperty();
160 return currentVertexProgressProperty();
164 return stateCollisionCheckProgressProperty();
168 return edgeCollisionCheckProgressProperty();
172 return nearestNeighbourProgressProperty();
226 if (static_cast<bool>(Planner::pdef_))
230 if (!Planner::pdef_->hasOptimizationObjective())
232 OMPL_INFORM(
"%s: No optimization objective specified. Defaulting to optimizing path length.",
233 Planner::getName().c_str());
234 Planner::pdef_->setOptimizationObjective(
235 std::make_shared<base::PathLengthOptimizationObjective>(Planner::si_));
240 if (static_cast<bool>(Planner::pdef_->getGoal()))
244 OMPL_ERROR(
"%s::setup() BIT* currently only supports goals that can be cast to a sampleable goal " 246 Planner::getName().c_str());
248 Planner::setup_ =
false;
256 costHelpPtr_->setup(Planner::pdef_->getOptimizationObjective(), graphPtr_);
259 queuePtr_->setup(costHelpPtr_, graphPtr_);
263 graphPtr_->setup(Planner::si_, Planner::pdef_, costHelpPtr_, queuePtr_,
this, Planner::pis_);
266 bestCost_ = costHelpPtr_->infiniteCost();
267 prunedCost_ = costHelpPtr_->infiniteCost();
270 prunedMeasure_ = Planner::si_->getSpaceMeasure();
277 Planner::setup_ =
false;
287 costHelpPtr_->clear();
292 curGoalVertex_.reset();
296 prunedMeasure_ = 0.0;
297 hasExactSolution_ =
false;
302 numEdgeCollisionChecks_ = 0u;
312 Planner::setup_ =
false;
321 Planner::checkValidity();
324 if (!Planner::setup_)
326 throw ompl::Exception(
"%s::solve() failed to set up the planner. Has a problem definition been set?", Planner::getName().c_str());
330 OMPL_INFORM(
"%s: Searching for a solution to the given planning problem.", Planner::getName().c_str());
337 if (!graphPtr_->hasAGoal())
339 graphPtr_->updateStartAndGoalStates(Planner::pis_, ptc);
351 while (!ptc && !stopLoop_ && !costHelpPtr_->isSatisfied(bestCost_) &&
352 (costHelpPtr_->isCostBetterThan(graphPtr_->minCost(), bestCost_) ||
353 Planner::pis_.haveMoreStartStates() || Planner::pis_.haveMoreGoalStates()))
359 if (hasExactSolution_)
361 this->endSuccessMessage();
365 this->endFailureMessage();
369 if (hasExactSolution_ || graphPtr_->getTrackApproximateSolutions())
372 this->publishSolution();
382 !hasExactSolution_ && graphPtr_->getTrackApproximateSolutions());
388 Planner::getPlannerData(data);
391 graphPtr_->getGraphAsPlannerData(data);
394 if (hasExactSolution_)
399 else if (!hasExactSolution_ && graphPtr_->getTrackApproximateSolutions())
402 data.
markGoalState(graphPtr_->closestVertexToGoal()->stateConst());
411 std::pair<ompl::base::State const *, ompl::base::State const *> nextEdge;
413 if (!queuePtr_->isEmpty())
420 nextEdge = std::make_pair(frontEdge.first->stateConst(), frontEdge.second->stateConst());
425 nextEdge = std::make_pair<ompl::base::State *, ompl::base::State *>(
nullptr,
nullptr);
437 if (!queuePtr_->isEmpty())
440 nextCost = queuePtr_->frontEdgeValue().at(0u);
445 nextCost = costHelpPtr_->infiniteCost();
453 queuePtr_->getEdges(edgesInQueue);
458 queuePtr_->getVertices(verticesInQueue);
463 return numIterations_;
479 void BITstar::iterate()
485 if (queuePtr_->isEmpty())
499 queuePtr_->popFrontEdge(&bestEdge);
503 if (costHelpPtr_->isCostBetterThan(costHelpPtr_->currentHeuristicEdge(bestEdge), bestCost_))
507 if (costHelpPtr_->isCostBetterThan(costHelpPtr_->currentHeuristicTarget(bestEdge),
508 bestEdge.second->getCost()))
517 trueEdgeCost = costHelpPtr_->trueEdgeCost(bestEdge);
521 if (costHelpPtr_->isCostBetterThan(
522 costHelpPtr_->combineCosts(costHelpPtr_->costToComeHeuristic(bestEdge.first),
524 costHelpPtr_->costToGoHeuristic(bestEdge.second)),
528 if (this->checkEdge(bestEdge))
532 if (costHelpPtr_->isCostBetterThan(
533 costHelpPtr_->combineCosts(bestEdge.first->getCost(), trueEdgeCost),
534 bestEdge.second->getCost()))
540 this->addEdge(bestEdge, trueEdgeCost);
544 this->updateGoalVertex();
570 void BITstar::newBatch()
579 if (Planner::pis_.haveMoreStartStates() || Planner::pis_.haveMoreGoalStates())
590 graphPtr_->addNewSamples(samplesPerBatch_);
593 void BITstar::prune()
600 if ((usePruning_) && (hasExactSolution_) &&
601 (std::abs(costHelpPtr_->fractionalChange(bestCost_, prunedCost_)) > pruneFraction_))
605 std::pair<unsigned int, unsigned int> numPruned(0u, 0u);
607 double informedMeasure = graphPtr_->getInformedMeasure(bestCost_);
609 double relativeMeasure = std::abs((informedMeasure - prunedMeasure_) / prunedMeasure_);
616 if ((graphPtr_->hasInformedMeasure() && informedMeasure < Planner::si_->getSpaceMeasure() &&
617 relativeMeasure > pruneFraction_) ||
618 (!graphPtr_->hasInformedMeasure()))
620 OMPL_INFORM(
"%s: Pruning the planning problem from a solution of %.4f to %.4f, changing the " 621 "problem size from %.4f to %.4f.",
622 Planner::getName().c_str(), prunedCost_.
value(), bestCost_.
value(), prunedMeasure_,
629 numPruned = numPruned + graphPtr_->prune(informedMeasure);
632 numPruned = numPruned + queuePtr_->prune(curGoalVertex_);
635 prunedCost_ = bestCost_;
638 prunedMeasure_ = informedMeasure;
640 OMPL_INFORM(
"%s: Pruning disconnected %d vertices from the tree and completely removed %d samples.",
641 Planner::getName().c_str(), numPruned.first, numPruned.second);
648 void BITstar::publishSolution()
652 std::vector<const ompl::base::State *> reversePath;
654 auto pathGeoPtr = std::make_shared<ompl::geometric::PathGeometric>(Planner::si_);
657 reversePath = this->bestPathFromGoalToStart();
660 for (
const auto &solnState : boost::adaptors::reverse(reversePath))
662 pathGeoPtr->append(solnState);
669 soln.setPlannerName(Planner::getName());
672 if (!hasExactSolution_ && graphPtr_->getTrackApproximateSolutions())
674 soln.setApproximate(graphPtr_->smallestDistanceToGoal());
678 soln.optimized_ = costHelpPtr_->isSatisfied(bestCost_);
681 Planner::pdef_->addSolutionPath(soln);
684 std::vector<const ompl::base::State *> BITstar::bestPathFromGoalToStart()
const 688 std::vector<const ompl::base::State *> reversePath;
693 if (hasExactSolution_)
696 curVertex = curGoalVertex_;
698 else if (!hasExactSolution_ && graphPtr_->getTrackApproximateSolutions())
701 curVertex = graphPtr_->closestVertexToGoal();
705 throw ompl::Exception(
"bestPathFromGoalToStart called without an exact or approximate solution.");
709 reversePath.push_back(curVertex->stateConst());
715 for (; !curVertex->isRoot();
716 curVertex = curVertex->getParentConst())
720 if (curVertex->hasParent() ==
false)
722 throw ompl::Exception(
"The path to the goal does not originate at a start state. Something went " 725 #endif // BITSTAR_DEBUG 728 reversePath.push_back(curVertex->getParentConst()->stateConst());
733 bool BITstar::checkEdge(
const VertexConstPtrPair &edge)
735 ++numEdgeCollisionChecks_;
736 return Planner::si_->checkMotion(edge.first->stateConst(), edge.second->stateConst());
739 void BITstar::addEdge(
const VertexPtrPair &newEdge,
const ompl::base::Cost &edgeCost)
742 if (newEdge.first->isInTree() ==
false)
744 throw ompl::Exception(
"Adding an edge from a vertex not connected to the graph");
746 if (costHelpPtr_->isCostEquivalentTo(costHelpPtr_->trueEdgeCost(newEdge), edgeCost) ==
false)
748 throw ompl::Exception(
"You have passed the wrong edge cost to addEdge.");
750 #endif // BITSTAR_DEBUG 754 bool isRewiring = newEdge.second->hasParent();
760 this->replaceParent(newEdge, edgeCost);
765 newEdge.second->markUnexpandedToSamples();
766 newEdge.second->markUnexpandedToVertices();
769 newEdge.first->addChild(newEdge.second,
false);
772 newEdge.second->addParent(newEdge.first, edgeCost,
true);
775 queuePtr_->enqueueVertex(newEdge.second,
true);
779 void BITstar::replaceParent(
const VertexPtrPair &newEdge,
const ompl::base::Cost &edgeCost)
782 if (newEdge.second->getParent()->getId() == newEdge.first->getId())
784 throw ompl::Exception(
"The new and old parents of the given rewiring are the same.");
786 if (costHelpPtr_->isCostBetterThan(newEdge.second->getCost(),
787 costHelpPtr_->combineCosts(newEdge.first->getCost(), edgeCost)) ==
true)
789 throw ompl::Exception(
"The new edge will increase the cost-to-come of the vertex!");
791 #endif // BITSTAR_DEBUG 797 newEdge.second->getParent()->removeChild(newEdge.second,
false);
800 newEdge.second->removeParent(
false);
803 newEdge.first->addChild(newEdge.second,
false);
807 newEdge.second->addParent(newEdge.first, edgeCost);
810 queuePtr_->markVertexUnsorted(newEdge.second);
813 void BITstar::updateGoalVertex()
817 bool goalUpdated =
false;
824 for (
auto goalIter = graphPtr_->goalVerticesBeginConst(); goalIter != graphPtr_->goalVerticesEndConst();
828 if ((*goalIter)->isInTree())
831 if (static_cast<bool>(newBestGoal))
834 if ((*goalIter)->getId() == newBestGoal->getId())
838 if (!costHelpPtr_->isCostEquivalentTo((*goalIter)->getCost(), newCost) ||
839 ((*goalIter)->getDepth() + 1u) != bestLength_)
843 newBestGoal = *goalIter;
844 newCost = newBestGoal->getCost();
852 if (costHelpPtr_->isCostBetterThan((*goalIter)->getCost(), newCost))
856 newBestGoal = *goalIter;
857 newCost = newBestGoal->getCost();
866 newBestGoal = *goalIter;
867 newCost = newBestGoal->getCost();
877 hasExactSolution_ =
true;
880 curGoalVertex_ = newBestGoal;
886 bestLength_ = curGoalVertex_->getDepth() + 1u;
889 queuePtr_->hasSolution(bestCost_);
890 graphPtr_->hasSolution(bestCost_);
893 stopLoop_ = stopOnSolnChange_;
899 if (static_cast<bool>(Planner::pdef_->getIntermediateSolutionCallback()))
907 Planner::pdef_->getIntermediateSolutionCallback()(
this, this->bestPathFromGoalToStart(), bestCost_);
913 void BITstar::goalMessage()
const 915 OMPL_INFORM(
"%s (%u iters): Found a solution of cost %.4f (%u vertices) from %u samples by processing %u " 916 "edges (%u collision checked) to create %u vertices and perform %u rewirings. The graph " 917 "currently has %u vertices.",
918 Planner::getName().c_str(), numIterations_, bestCost_.
value(), bestLength_,
919 graphPtr_->numStatesGenerated(), queuePtr_->numEdgesPopped(), numEdgeCollisionChecks_,
920 graphPtr_->numVerticesConnected(), numRewirings_, graphPtr_->numConnectedVertices());
923 void BITstar::endSuccessMessage()
const 925 OMPL_INFORM(
"%s: Finished with a solution of cost %.4f (%u vertices) found from %u samples by processing " 926 "%u edges (%u collision checked) to create %u vertices and perform %u rewirings. The final " 927 "graph has %u vertices.",
928 Planner::getName().c_str(), bestCost_.
value(), bestLength_, graphPtr_->numStatesGenerated(),
929 queuePtr_->numEdgesPopped(), numEdgeCollisionChecks_, graphPtr_->numVerticesConnected(),
930 numRewirings_, graphPtr_->numConnectedVertices());
933 void BITstar::endFailureMessage()
const 935 if (graphPtr_->getTrackApproximateSolutions())
937 OMPL_INFORM(
"%s (%u iters): Did not find an exact solution from %u samples after processing %u edges " 938 "(%u collision checked) to create %u vertices and perform %u rewirings. The final graph " 939 "has %u vertices. The best approximate solution was %.4f from the goal and has a cost of " 941 Planner::getName().c_str(), numIterations_, graphPtr_->numStatesGenerated(),
942 queuePtr_->numEdgesPopped(), numEdgeCollisionChecks_, graphPtr_->numVerticesConnected(),
943 numRewirings_, graphPtr_->numConnectedVertices(), graphPtr_->smallestDistanceToGoal(),
944 graphPtr_->closestVertexToGoal()->getCost().value());
948 OMPL_INFORM(
"%s (%u iters): Did not find an exact solution from %u samples after processing %u edges " 949 "(%u collision checked) to create %u vertices and perform %u rewirings. The final graph " 951 Planner::getName().c_str(), numIterations_, graphPtr_->numStatesGenerated(),
952 queuePtr_->numEdgesPopped(), numEdgeCollisionChecks_, graphPtr_->numVerticesConnected(),
953 numRewirings_, graphPtr_->numConnectedVertices());
957 void BITstar::statusMessage(
const ompl::msg::LogLevel &msgLevel,
const std::string &status)
const 964 std::stringstream outputStream;
968 outputStream << Planner::getName();
969 outputStream <<
" (";
971 outputStream <<
"l: " << std::setw(6) << std::setfill(
' ') << std::setprecision(5) << bestCost_.
value();
973 outputStream <<
", b: " << std::setw(5) << std::setfill(
' ') << numBatches_;
975 outputStream <<
", i: " << std::setw(5) << std::setfill(
' ') << numIterations_;
977 outputStream <<
", g: " << std::setw(5) << std::setfill(
' ') << graphPtr_->numConnectedVertices();
979 outputStream <<
", f: " << std::setw(5) << std::setfill(
' ') << graphPtr_->numFreeSamples();
981 outputStream <<
", q: " << std::setw(5) << std::setfill(
' ') << queuePtr_->numEdges();
983 outputStream <<
", t: " << std::setw(5) << std::setfill(
' ') << queuePtr_->numEdgesPopped();
985 outputStream <<
", s: " << std::setw(5) << std::setfill(
' ') << graphPtr_->numStatesGenerated();
987 outputStream <<
", v: " << std::setw(5) << std::setfill(
' ') << graphPtr_->numVerticesConnected();
989 outputStream <<
", p: " << std::setw(5) << std::setfill(
' ') << numPrunings_;
991 outputStream <<
", r: " << std::setw(5) << std::setfill(
' ') << numRewirings_;
993 outputStream <<
", n: " << std::setw(5) << std::setfill(
' ') << graphPtr_->numNearestLookups();
995 outputStream <<
", c(s): " << std::setw(5) << std::setfill(
' ') << graphPtr_->numStateCollisionChecks();
997 outputStream <<
", c(e): " << std::setw(5) << std::setfill(
' ') << numEdgeCollisionChecks_;
998 outputStream <<
"): ";
1000 outputStream << status;
1002 if (msgLevel == ompl::msg::LOG_DEBUG)
1004 OMPL_DEBUG(
"%s: ", outputStream.str().c_str());
1006 else if (msgLevel == ompl::msg::LOG_INFO)
1010 else if (msgLevel == ompl::msg::LOG_WARN)
1012 OMPL_WARN(
"%s: ", outputStream.str().c_str());
1014 else if (msgLevel == ompl::msg::LOG_ERROR)
1016 OMPL_ERROR(
"%s: ", outputStream.str().c_str());
1031 graphPtr_->setRewireFactor(rewireFactor);
1036 return graphPtr_->getRewireFactor();
1041 samplesPerBatch_ = n;
1046 return samplesPerBatch_;
1052 graphPtr_->setUseKNearest(useKNearest);
1055 if (graphPtr_->getUseKNearest() && Planner::getName() ==
"kBITstar")
1058 Planner::setName(
"BITstar");
1060 else if (!graphPtr_->getUseKNearest() && Planner::getName() ==
"BITstar")
1063 Planner::setName(
"kBITstar");
1070 return graphPtr_->getUseKNearest();
1075 queuePtr_->setStrictQueueOrdering(beStrict);
1080 return queuePtr_->getStrictQueueOrdering();
1087 OMPL_WARN(
"%s: Turning pruning off has never really been tested.", Planner::getName().c_str());
1091 usePruning_ = prune;
1094 queuePtr_->setPruneDuringResort(usePruning_);
1104 if (fractionalChange < 0.0 || fractionalChange > 1.0)
1106 throw ompl::Exception(
"Prune threshold must be specified as a fraction between [0, 1].");
1109 pruneFraction_ = fractionalChange;
1114 return pruneFraction_;
1119 queuePtr_->setDelayedRewiring(delayRewiring);
1124 return queuePtr_->getDelayedRewiring();
1129 graphPtr_->setJustInTimeSampling(useJit);
1134 return graphPtr_->getJustInTimeSampling();
1139 graphPtr_->setDropSamplesOnPrune(dropSamples);
1144 return graphPtr_->getDropSamplesOnPrune();
1149 stopOnSolnChange_ = stopOnChange;
1154 return stopOnSolnChange_;
1160 graphPtr_->setTrackApproximateSolutions(findApproximate);
1163 Planner::specs_.approximateSolutions = graphPtr_->getTrackApproximateSolutions();
1168 return graphPtr_->getTrackApproximateSolutions();
1171 template <
template <
typename T>
class NN>
1176 if (Planner::setup_)
1178 OMPL_WARN(
"%s: The nearest neighbour datastructures cannot be changed once the planner is setup. " 1179 "Continuing to use the existing containers.",
1180 Planner::getName().c_str());
1184 graphPtr_->setNearestNeighbors<NN>();
1188 std::string BITstar::bestCostProgressProperty()
const 1190 return std::to_string(this->
bestCost().value());
1193 std::string BITstar::bestLengthProgressProperty()
const 1195 return std::to_string(bestLength_);
1198 std::string BITstar::currentFreeProgressProperty()
const 1200 return std::to_string(graphPtr_->numFreeSamples());
1203 std::string BITstar::currentVertexProgressProperty()
const 1205 return std::to_string(graphPtr_->numConnectedVertices());
1208 std::string BITstar::vertexQueueSizeProgressProperty()
const 1210 return std::to_string(queuePtr_->numVertices());
1213 std::string BITstar::edgeQueueSizeProgressProperty()
const 1215 return std::to_string(queuePtr_->numEdges());
1218 std::string BITstar::iterationProgressProperty()
const 1223 std::string BITstar::batchesProgressProperty()
const 1228 std::string BITstar::pruningProgressProperty()
const 1230 return std::to_string(numPrunings_);
1233 std::string BITstar::totalStatesCreatedProgressProperty()
const 1235 return std::to_string(graphPtr_->numStatesGenerated());
1238 std::string BITstar::verticesConstructedProgressProperty()
const 1240 return std::to_string(graphPtr_->numVerticesConnected());
1243 std::string BITstar::statesPrunedProgressProperty()
const 1245 return std::to_string(graphPtr_->numFreeStatesPruned());
1248 std::string BITstar::verticesDisconnectedProgressProperty()
const 1250 return std::to_string(graphPtr_->numVerticesDisconnected());
1253 std::string BITstar::rewiringProgressProperty()
const 1255 return std::to_string(numRewirings_);
1258 std::string BITstar::stateCollisionCheckProgressProperty()
const 1260 return std::to_string(graphPtr_->numStateCollisionChecks());
1263 std::string BITstar::edgeCollisionCheckProgressProperty()
const 1265 return std::to_string(numEdgeCollisionChecks_);
1268 std::string BITstar::nearestNeighbourProgressProperty()
const 1270 return std::to_string(graphPtr_->numNearestLookups());
1273 std::string BITstar::edgesProcessedProgressProperty()
const 1275 return std::to_string(queuePtr_->numEdgesPopped());
void addPlannerProgressProperty(const std::string &progressPropertyName, const PlannerProgressProperty &prop)
Add a planner progress property called progressPropertyName with a property querying function prop to...
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
bool getJustInTimeSampling() const
Get whether we're using just-in-time sampling.
std::vector< VertexConstPtrPair > VertexConstPtrPairVector
A vector of pairs of const vertices, i.e., a vector of edges.
void setNearestNeighbors()
Set a different nearest neighbours datastructure.
Representation of a solution to a planning problem.
void getPlannerData(base::PlannerData &data) const override
Get results.
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
unsigned int numBatches() const
Retrieve the number of batches processed as the raw data. (numBatches_)
void setConsiderApproximateSolutions(bool findApproximate)
Set BIT* to consider approximate solutions during its initial search.
bool getDelayRewiringUntilInitialSolution() const
Get whether BIT* is delaying rewiring until a solution is found.
void setDelayRewiringUntilInitialSolution(bool delayRewiring)
Delay the consideration of rewiring edges until an initial solution is found. When multiple batches a...
ompl::base::Cost bestCost() const
Retrieve the best exact-solution cost found.
PlannerTerminationCondition plannerAlwaysTerminatingCondition()
Simple termination condition that always returns true. The termination condition will always be met...
std::shared_ptr< const Vertex > VertexConstPtr
A constant vertex shared pointer.
BITstar(const base::SpaceInformationPtr &si, const std::string &name="BITstar")
Construct!
void setSamplesPerBatch(unsigned int n)
Set the number of samplers per batch.
void setStrictQueueOrdering(bool beStrict)
Enable "strict sorting" of the edge queue. Rewirings can change the position in the queue of an edge...
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Solve.
Main namespace. Contains everything in this library.
void setUseKNearest(bool useKNearest)
Enable a k-nearest search for instead of an r-disc search.
ompl::base::Cost getNextEdgeValueInQueue()
Get the value of the next edge to be processed. Causes vertices in the queue to be expanded (if neces...
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
void getEdgeQueue(VertexConstPtrPairVector *edgesInQueue)
Get the whole messy set of edges in the queue. Expensive but helpful for some videos.
std::pair< VertexConstPtr, VertexConstPtr > VertexConstPtrPair
A pair of const vertices, i.e., an edge.
bool getDropSamplesOnPrune() const
Get whether unconnected samples are dropped on pruning.
LogLevel getLogLevel()
Retrieve the current level of logging data. Messages with lower logging levels will not be recorded...
bool getPruning() const
Get whether graph and sample pruning is in use.
std::pair< const ompl::base::State *, const ompl::base::State * > getNextEdgeInQueue()
Get the next edge to be processed. Causes vertices in the queue to be expanded (if necessary) and the...
A class to store the exit status of Planner::solve()
void setRewireFactor(double rewireFactor)
Set the rewiring scale factor, s, such that r_rrg = s r_rrg*.
void setPruneThresholdFraction(double fractionalChange)
Set the fractional change in the solution cost AND problem measure necessary for pruning to occur...
unsigned int numIterations() const
Get the number of iterations completed.
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
std::pair< VertexPtr, VertexPtr > VertexPtrPair
A pair of vertices, i.e., an edge.
void setJustInTimeSampling(bool useJit)
Delay the generation of samples until they are necessary. This only works when using an r-disc connec...
The exception type for ompl.
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
void clear() override
Clear.
void setup() override
Setup.
std::vector< VertexConstPtr > VertexConstPtrVector
A vector of shared const pointers.
const std::string & getName() const
Get the name of the planner.
bool markGoalState(const State *st)
Mark the given state as a goal vertex. If the given state does not exist in a vertex, false is returned.
bool getConsiderApproximateSolutions() const
Get whether BIT* is considering approximate solutions.
bool getUseKNearest() const
Get whether a k-nearest search is being used.
double value() const
The value of the cost.
double getRewireFactor() const
Get the rewiring scale factor.
double getPruneThresholdFraction() const
Get the fractional change in the solution cost AND problem measure necessary for pruning to occur...
void getVertexQueue(VertexConstPtrVector *verticesInQueue)
Get the whole set of vertices to be expanded. Expensive but helpful for some videos.
void setDropSamplesOnPrune(bool dropSamples)
Drop all unconnected samples when pruning, regardless of their heuristic value. This provides a metho...
void setPruning(bool prune)
Enable pruning of vertices/samples that CANNOT improve the current solution. When a vertex in the gra...
bool getStopOnSolnImprovement() const
Get whether BIT* stops each time a solution is found.
unsigned int getSamplesPerBatch() const
Get the number of samplers per batch.
LogLevel
The set of priorities for message logging.
bool getStrictQueueOrdering() const
Get whether strict queue ordering is in use.
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
void setStopOnSolnImprovement(bool stopOnChange)
Stop the planner each time a solution improvement is found. Useful for examining the intermediate sol...
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible...
#define OMPL_INFORM(fmt,...)
Log a formatted information string.