ImplicitGraph.cpp
149 ImplicitGraph::updateStartAndGoalStates(const ompl::base::PlannerTerminationCondition &terminationCondition,
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.
187 // If we added a new start and have previously pruned goals, we might want to add the goals back.
221 // If we added a new goal and have previously pruned starts, we might want to add the starts back.
332 newSamples_.emplace_back(std::make_shared<Vertex>(spaceInformation_, problemDefinition_, batchId_));
341 } while (!spaceInformation_->getStateValidityChecker()->isValid(newSamples_.back()->getState()));
473 // Check if the combination of the admissible costToCome and costToGo estimates results in a path
528 // Each sampled state is checked for collision. Only sampled states are checked for collision (number of
PlannerTerminationCondition plannerAlwaysTerminatingCondition()
Simple termination condition that always returns true. The termination condition will always be met.
Definition: PlannerTerminationCondition.cpp:189
bool hasAStartState() const
Returns whether the graph has a goal state.
Definition: ImplicitGraph.cpp:138
bool haveMoreStartStates() const
Check if there are more potential start states.
Definition: Planner.cpp:348
std::vector< std::shared_ptr< Vertex > > getVertices() const
Get all vertices.
Definition: ImplicitGraph.cpp:450
A shared pointer wrapper for ompl::base::SpaceInformation.
Helper class to extract valid start & goal states. Usually used internally by planners.
Definition: Planner.h:78
void setup(const ompl::base::SpaceInformationPtr &spaceInformation, const ompl::base::ProblemDefinitionPtr &problemDefinition, ompl::base::PlannerInputStates *inputStates)
The setup method for the graph. Needed to have it on the stack.
Definition: ImplicitGraph.cpp:55
std::size_t getNumberOfNearestNeighborCalls() const
Get the number of nearest neighbor calls.
Definition: ImplicitGraph.cpp:533
void updateStartAndGoalStates(const ompl::base::PlannerTerminationCondition &terminationCondition, ompl::base::PlannerInputStates *inputStates)
Adds new start and goals to the graph if avavilable and creates a new informed sampler if necessary.
Definition: ImplicitGraph.cpp:149
bool getUseKNearest() const
Whether the graph uses a k-nearest connection model. If false, it uses an r-disc model.
Definition: ImplicitGraph.cpp:100
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
bool hasAGoalState() const
Returns whether the graph has a goal state.
Definition: ImplicitGraph.cpp:143
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:48
void registerStartState(const ompl::base::State *const startState)
Registers a state as a start state.
Definition: ImplicitGraph.cpp:105
const std::vector< std::shared_ptr< Vertex > > & getStartVertices() const
Get the start vertices.
Definition: ImplicitGraph.cpp:440
double unitNBallMeasure(unsigned int N)
The Lebesgue measure (i.e., "volume") of an n-dimensional ball with a unit radius.
Definition: GeometricEquations.cpp:55
void registerGoalState(const ompl::base::State *const goalState)
Registers a state as a goal state.
Definition: ImplicitGraph.cpp:123
std::size_t getNumberOfValidSamples() const
Returns the total number of valid samples found.
Definition: ImplicitGraph.cpp:521
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Definition: PlannerTerminationCondition.h:64
const State * nextStart()
Return the next valid start state or nullptr if no more valid start states are available.
Definition: Planner.cpp:237
void clear()
Resets the graph to its construction state, without resetting options.
Definition: ImplicitGraph.cpp:71
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
std::size_t getNumberOfSampledStates() const
Returns the total number of sampled states.
Definition: ImplicitGraph.cpp:516
A shared pointer wrapper for ompl::base::ProblemDefinition.
std::size_t getNumVertices() const
Gets the number of samples in the graph.
Definition: ImplicitGraph.cpp:381
bool isStart(const std::shared_ptr< Vertex > &vertex) const
Checks whether the vertex is a start vertex.
Definition: ImplicitGraph.cpp:416
void setUseKNearest(bool useKNearest)
Whether to use a k-nearest connection model. If false, it uses an r-disc model.
Definition: ImplicitGraph.cpp:95
void prune()
Prune all samples that can not contribute to a solution better than the current one.
Definition: ImplicitGraph.cpp:457
const std::vector< std::shared_ptr< Vertex > > & getGoalVertices() const
Get the goal vertices.
Definition: ImplicitGraph.cpp:445
std::vector< std::shared_ptr< Vertex > > getNeighbors(const std::shared_ptr< Vertex > &vertex) const
Get neighbors of a vertex.
Definition: ImplicitGraph.cpp:392
bool haveMoreGoalStates() const
Check if there are more potential goal states.
Definition: Planner.cpp:355
bool addSamples(std::size_t numNewSamples, const ompl::base::PlannerTerminationCondition &terminationCondition)
Adds a batch of samples and returns the samples it has added.
Definition: ImplicitGraph.cpp:317
void setRewireFactor(double rewireFactor)
Set the rewire factor of the RGG.
Definition: ImplicitGraph.cpp:85
bool isGoal(const std::shared_ptr< Vertex > &vertex) const
Checks whether the vertex is a goal vertex.
Definition: ImplicitGraph.cpp:428
ImplicitGraph(const ompl::base::Cost &solutionCost)
Constructs an implicit graph.
Definition: ImplicitGraph.cpp:50
std::size_t getNumberOfStateCollisionChecks() const
Get the number of state collision checks.
Definition: ImplicitGraph.cpp:526