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.
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
509 // Each sampled state is checked for collision. Only sampled states are checked for collision (number of
559 // Check if the combination of the admissible costToCome and costToGo estimates results in a path
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:48
Helper class to extract valid start & goal states. Usually used internally by planners.
Definition: Planner.h:78
bool haveMoreStartStates() const
Check if there are more potential start states.
Definition: Planner.cpp:348
const State * nextStart()
Return the next valid start state or nullptr if no more valid start states are available.
Definition: Planner.cpp:237
bool haveMoreGoalStates() const
Check if there are more potential goal states.
Definition: Planner.cpp:355
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
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Definition: PlannerTerminationCondition.h:64
A shared pointer wrapper for ompl::base::ProblemDefinition.
A shared pointer wrapper for ompl::base::SpaceInformation.
bool isStart(const std::shared_ptr< Vertex > &vertex) const
Checks whether the vertex is a start vertex.
Definition: ImplicitGraph.cpp:397
bool hasAStartState() const
Returns whether the graph has a goal state.
Definition: ImplicitGraph.cpp:138
std::size_t getNumberOfNearestNeighborCalls() const
Get the number of nearest neighbor calls.
Definition: ImplicitGraph.cpp:514
const std::vector< std::shared_ptr< Vertex > > & getStartVertices() const
Get the start vertices.
Definition: ImplicitGraph.cpp:421
std::vector< std::shared_ptr< Vertex > > getVertices() const
Get all vertices.
Definition: ImplicitGraph.cpp:431
bool isGoal(const std::shared_ptr< Vertex > &vertex) const
Checks whether the vertex is a goal vertex.
Definition: ImplicitGraph.cpp:409
std::size_t getNumberOfStateCollisionChecks() const
Get the number of state collision checks.
Definition: ImplicitGraph.cpp:507
std::size_t getNumberOfValidSamples() const
Returns the total number of valid samples found.
Definition: ImplicitGraph.cpp:502
void prune()
Prune all samples that can not contribute to a solution better than the current one.
Definition: ImplicitGraph.cpp:438
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
std::vector< std::shared_ptr< Vertex > > addSamples(std::size_t numNewSamples)
Adds a batch of samples and returns the samples it has added.
Definition: ImplicitGraph.cpp:317
void registerStartState(const ompl::base::State *const startState)
Registers a state as a start state.
Definition: ImplicitGraph.cpp:105
void clear()
Resets the graph to its construction state, without resetting options.
Definition: ImplicitGraph.cpp:71
void registerGoalState(const ompl::base::State *const goalState)
Registers a state as a goal state.
Definition: ImplicitGraph.cpp:123
const std::vector< std::shared_ptr< Vertex > > & getGoalVertices() const
Get the goal vertices.
Definition: ImplicitGraph.cpp:426
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
ImplicitGraph(const ompl::base::Cost &solutionCost)
Constructs an implicit graph.
Definition: ImplicitGraph.cpp:50
std::vector< std::shared_ptr< Vertex > > getNeighbors(const std::shared_ptr< Vertex > &vertex) const
Get neighbors of a vertex.
Definition: ImplicitGraph.cpp:373
std::size_t getNumVertices() const
Gets the number of samples in the graph.
Definition: ImplicitGraph.cpp:362
bool hasAGoalState() const
Returns whether the graph has a goal state.
Definition: ImplicitGraph.cpp:143
bool getUseKNearest() const
Whether the graph uses a k-nearest connection model. If false, it uses an r-disc model.
Definition: ImplicitGraph.cpp:100
std::size_t getNumberOfSampledStates() const
Returns the total number of sampled states.
Definition: ImplicitGraph.cpp:497
void setRewireFactor(double rewireFactor)
Set the rewire factor of the RGG.
Definition: ImplicitGraph.cpp:85
void setUseKNearest(bool useKNearest)
Whether to use a k-nearest connection model. If false, it uses an r-disc model.
Definition: ImplicitGraph.cpp:95
PlannerTerminationCondition plannerAlwaysTerminatingCondition()
Simple termination condition that always returns true. The termination condition will always be met.
Definition: PlannerTerminationCondition.cpp:189
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
Main namespace. Contains everything in this library.
Definition: ConstrainedSpaceInformation.h:53
double unitNBallMeasure(unsigned int N)
The Lebesgue measure (i.e., "volume") of an n-dimensional ball with a unit radius.
Definition: GeometricEquations.cpp:55