Class RandomGeometricGraph

Class Documentation

class RandomGeometricGraph

Public Functions

RandomGeometricGraph(const std::shared_ptr<ompl::base::SpaceInformation> &spaceInfo, const ompl::base::Cost &solutionCost)

Constructs a random geometric graph with the given space information and reference to the current solution cost.

~RandomGeometricGraph() = default

Destricts this random geometric graph.

void setup(const std::shared_ptr<ompl::base::ProblemDefinition> &problem, ompl::base::PlannerInputStates *inputStates)

Setup the graph with the given problem definition and planner input states.

void clear()

Clears all internal planner structures but retains settings. Subsequent calls to solve() will start from scratch.

void clearQuery()

Clears all query-specific structures, such as start and goal states.

void updateStartAndGoalStates(const ompl::base::PlannerTerminationCondition &terminationCondition, ompl::base::PlannerInputStates *inputStates)

Adds new starts and goals to the graph if available and creates a new informed sampler if necessary.

ompl::base::Cost minPossibleCost() const

Returns the minimum possible cost for the current problem, using admissible cost estimates to calculate it.

void setRadiusFactor(double factor)

Sets the radius factor (eta in the paper).

double getRadiusFactor() const

Returns the radius factor (eta in the paper).

void setEffortThreshold(const unsigned int threshold)

Sets the effort threshold.

unsigned int getEffortThreshold() const

Gets the effort threshold.

void enablePruning(bool prune)

Enable pruning of the graph.

bool isPruningEnabled() const

Returns Whether pruning is enabled.

void enableMultiquery(bool multiquery)

Enable multiquery usage of the graph.

bool isMultiqueryEnabled() const

Returns Whether multiquery usage of the graph is enabled.

void setUseKNearest(bool useKNearest)

Set whether to use a k-nearest connection model. If false, it uses an r-disc model.

bool getUseKNearest() const

Returns whether the graph uses a k-nearest connection model. If false, it uses an r-disc model.

void setMaxNumberOfGoals(unsigned int maxNumberOfGoals)

Sets the maximum number of goals EIT* will sample from sampleable goal regions.

unsigned int getMaxNumberOfGoals() const

Returns the maximum number of goals EIT* will sample from sampleable goal regions.

bool addStates(std::size_t numStates, const ompl::base::PlannerTerminationCondition &terminationCondition)

Samples random states and adds them to the graph.

void prune()

Prunes the graph of states that can not improve the current solution.

std::vector<std::weak_ptr<State>> getNeighbors(const std::shared_ptr<State> &state) const

Returns the neighbors of a state.

const std::vector<std::shared_ptr<State>> &getStartStates() const

Returns the start states.

const std::vector<std::shared_ptr<State>> &getGoalStates() const

Returns the goal states.

unsigned int getNumberOfSampledStates() const

Returns the number of sampled states.

unsigned int getNumberOfValidSamples() const

Returns the number of valid samples.

unsigned int getNumberOfNearestNeighborCalls() const

Returns the number of nearest neighbor calls.

std::shared_ptr<State> registerStartState(const ompl::base::State *start)

Sets the start state.

std::shared_ptr<State> registerGoalState(const ompl::base::State *goal)

Sets the goal state.

void registerWhitelistedState(const std::shared_ptr<State> &state) const

Registers a whitelisted state.

bool hasStartState() const

Returns whether a start state is available.

bool hasGoalState() const

Returns whether a goal state is available.

bool isStart(const std::shared_ptr<State> &state) const

Returns whether the given state is a start state.

bool isGoal(const std::shared_ptr<State> &state) const

Returns whether the given state is a goal state.

std::vector<std::shared_ptr<State>> getStates() const

Returns all sampled states (that have not been pruned).

void registerInvalidEdge(const Edge &edge) const

Registers an invalid edge.

std::size_t getTag() const

Returns the tag of the current RGG.

unsigned int inadmissibleEffortToCome(const std::shared_ptr<State> &state) const

Returns the inadmissible effort to come.