ImplicitGraph.cpp
146 }
149 ImplicitGraph::updateStartAndGoalStates(const ompl::base::PlannerTerminationCondition &terminationCondition,
151 {
156 // First update the goals. We have to call inputStates->nextGoal(terminationCondition) at least once
157 // (regardless of the return value of inputStates->moreGoalStates()) in case the termination condition
161 // Get a new goal. If there are none, or the underlying state is invalid this will be a nullptr.
167 registerGoalState(newGoalState);
181 {
187 // If we added a new start and have previously pruned goals, we might want to add the goals back.
191 std::vector<std::vector<std::shared_ptr<Vertex>>::iterator> revivedGoals;
201 heuristicCost, objective_->motionCostHeuristic(start->getState(), (*it)->getState()));
219 }
221 // If we added a new goal and have previously pruned starts, we might want to add the starts back.
234 heuristicCost = objective_->betterCost(
245 }
328 newVertices.emplace_back(std::make_shared<Vertex>(spaceInformation_, problemDefinition_, batchId_));
337 } while (!spaceInformation_->getStateValidityChecker()->isValid(newVertices.back()->getState()));
454 // Check if the combination of the admissible costToCome and costToGo estimates results in a path
463 prunedGoalVertices_.emplace_back(vertex);
469 verticesToBePruned.emplace_back(vertex);
505 }
509 // Each sampled state is checked for collision. Only sampled states are checked for collision (number of
517 }
527 (sampler_->getInformedMeasure(solutionCost_) /
PlannerTerminationCondition plannerAlwaysTerminatingCondition()
Simple termination condition that always returns true. The termination condition will always be met.
Definition: PlannerTerminationCondition.cpp:189
bool haveMoreStartStates() const
Check if there are more potential start states.
Definition: Planner.cpp:348
void setRewireFactor(double rewireFactor)
Set the rewiring scale factor, s, such that r_rrg = s \times r_rrg*.
Definition: ImplicitGraph.cpp:1543
A shared pointer wrapper for ompl::base::SpaceInformation.
Helper class to extract valid start & goal states. Usually used internally by planners.
Definition: Planner.h:142
void setUseKNearest(bool useKNearest)
Enable a k-nearest search for instead of an r-disc search.
Definition: ImplicitGraph.cpp:1561
bool getUseKNearest() const
Get whether a k-nearest search is being used.
Definition: ImplicitGraph.cpp:1584
void log(const char *file, int line, LogLevel level, const char *m,...)
Root level logging function. This should not be invoked directly, but rather used via a logging macro...
Definition: Console.cpp:120
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:112
void setup(const ompl::base::SpaceInformationPtr &spaceInformation, const ompl::base::ProblemDefinitionPtr &problemDefinition, CostHelper *costHelper, SearchQueue *searchQueue, const ompl::base::Planner *plannerPtr, ompl::base::PlannerInputStates &inputStates)
Setup the ImplicitGraph, must be called before use. Does not take a copy of the PlannerInputStates,...
Definition: ImplicitGraph.cpp:91
double unitNBallMeasure(unsigned int N)
The Lebesgue measure (i.e., "volume") of an n-dimensional ball with a unit radius.
Definition: GeometricEquations.cpp:55
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Definition: PlannerTerminationCondition.h:128
const State * nextStart()
Return the next valid start state or nullptr if no more valid start states are available.
Definition: Planner.cpp:237
const State * nextGoal(const PlannerTerminationCondition &ptc)
Return the next valid goal state or nullptr if no more valid goal states are available....
Definition: Planner.cpp:274
A shared pointer wrapper for ompl::base::ProblemDefinition.
void updateStartAndGoalStates(ompl::base::PlannerInputStates &inputStates, const base::PlannerTerminationCondition &terminationCondition)
Adds any new goals or starts that have appeared in the problem definition to the vector of vertices a...
Definition: ImplicitGraph.cpp:383
bool haveMoreGoalStates() const
Check if there are more potential goal states.
Definition: Planner.cpp:355
std::pair< unsigned int, unsigned int > prune(double prunedMeasure)
Prune the samples to the subproblem of the given measure. Returns the number of vertices disconnected...
Definition: ImplicitGraph.cpp:646
ImplicitGraph(const ompl::base::Cost &solutionCost)
Constructs an implicit graph.
Definition: ImplicitGraph.cpp:146
Main namespace. Contains everything in this library.
Definition: ConstrainedSpaceInformation.h:53