Class Vertex

Inheritance Relationships

Base Type

  • public std::enable_shared_from_this< Vertex >

Class Documentation

class Vertex : public std::enable_shared_from_this<Vertex>

Public Functions

Vertex(const ompl::base::SpaceInformationPtr &spaceInformation, const ompl::base::ProblemDefinitionPtr &problemDefinition, const std::size_t &batchId)

Constructs a vertex by sampling a state.

explicit Vertex(const std::shared_ptr<Vertex> &other)

Constructs a copy of another vertex.

virtual ~Vertex()

Destructs the vertex.

std::size_t getId() const

Get the unique id of this vertex.

ompl::base::State *getState()

Provides write access to the underlying state.

ompl::base::State const *getState() const

Provides read access to the underlying state.

ompl::base::ScopedState getScopedState() const

Returns a scoped copy of the underlying state.

ompl::base::Cost getCostToComeFromStart() const

Returns the cost to come to this vertex from the start.

ompl::base::Cost getCostToComeFromGoal() const

Returns the cost to come to this vertex from the goal.

ompl::base::Cost getCostToGoToGoal() const

Returns the cost to go heuristic from this vertex.

ompl::base::Cost getEdgeCostFromForwardParent() const

Returns the edge cost from the forward parent.

ompl::base::Cost getEdgeCostFromReverseParent() const

Returns the edge cost from the reverse parent.

ompl::base::Cost getValidForwardEdgeCost() const

Returns the valid edge cost from the forward and backward tree on a valid path.

ompl::base::Cost getValidReverseEdgeCost() const
void resetForwardParent()

Resets associated parents of this vertex.

void resetReverseParent()
void resetForwardEdgeParent()
void resetReverseEdgeParent()
bool hasForwardParent() const

Returns whether this vertex has a parent in either search.

bool hasReverseParent() const
bool hasReverseEdgeParent() const
bool hasForwardEdgeParent() const
std::shared_ptr<Vertex> getForwardParent() const

Returns the parent of the vertex (in the forward-search tree).

std::shared_ptr<Vertex> getForwardEdgeParent() const
std::shared_ptr<Vertex> getReverseParent() const

Returns the parent of the vertex (in the reverse-search tree).

std::shared_ptr<Vertex> getReverseEdgeParent() const
void setForwardEdgeCost(const ompl::base::Cost &cost)

Sets the cost to come to this vertex.

void setCostToComeFromStart(const ompl::base::Cost &cost)

Sets the cost to come to this vertex.

void setCostToComeFromGoal(const ompl::base::Cost &cost)

Sets the cost to come to this vertex from the goal.

void setCostToGoToGoal(const ompl::base::Cost &cost)

Sets the cost to go To goal heuristic of this vertex.

void setCostToGoToStart(const ompl::base::Cost &cost)

Sets the cost to go To start heuristic of this vertex.

void addToForwardChildren(const std::shared_ptr<Vertex> &vertex)

Adds a vertex to this vertex’s forward children.

void removeFromForwardChildren(std::size_t vertexId)

Removes a vertex from this vertex’s forward children.

std::vector<std::shared_ptr<Vertex>> getForwardChildren() const

Returns this vertex’s children in the forward search tree.

void addToReverseChildren(const std::shared_ptr<Vertex> &vertex)

Adds a vertex this vertex’s children.

void removeFromReverseChildren(std::size_t vertexId)

Removes a vertex from this vertex’s forward children.

std::vector<std::shared_ptr<Vertex>> getReverseChildren() const

Returns this vertex’s children in the reverse search tree.

void whitelistAsChild(const std::shared_ptr<Vertex> &vertex) const

Whitelists a child.

bool isWhitelistedAsChild(const std::shared_ptr<Vertex> &vertex) const

Returns whether a child is whitelisted.

void blacklistAsChild(const std::shared_ptr<Vertex> &vertex) const

Blacklists a child.

std::size_t getIncomingCollisionCheckResolution(const std::size_t vertexId) const
void setIncomingCollisionCheckResolution(const std::size_t vertexId, std::size_t numChecks) const
bool isBlacklistedAsChild(const std::shared_ptr<Vertex> &vertex) const

Returns whether a child is blacklisted.

bool hasCachedNeighbors() const

Returns whether the vertex knows its nearest neighbors on the current approximation.

void cacheNeighbors(const std::vector<std::shared_ptr<Vertex>> &neighbors) const

Caches the neighbors for the current approximation.

const std::vector<std::shared_ptr<Vertex>> getNeighbors() const

Returns the nearest neighbors, throws if not up to date.

bool isGoal()

set and evalue whether this vertex is a start, goal or meeting state.

void setMeet()
bool isStart()
bool meetVertex()
bool nearObstacle()

whether a state is near obstalce

void setGoalVertex()
bool forwardInvalid()
bool reverseInvalid()
void setStartVertex()

remark the state to be near obstalce.

void setNearObstacle()
void setForwardInvalid()
void setReverseInvalid()
void setReverseExpanded()

Sets wether this vertex is expanded.

void setForwardExpanded()
bool isForwardExpanded()

Check whether a state is expanded.

bool isReverseExpanded()
std::size_t getForwardId()

get the current search counter of a state

std::size_t getReverseId()
ompl::base::Cost getLowerCostBoundToGoal()

Set the the lower bound of the estimated heuristic value.

ompl::base::Cost getLowerCostBoundToStart()
void setForwardId(const std::size_t counter)

Set the current search counter.

void setReverseId(const std::size_t counter)
void setLowerCostBoundToGoal(const ompl::base::Cost &ToGoal)

Set the lower-cost-bound-to-go of this vertex.

void setLowerCostBoundToStart(const ompl::base::Cost &ToStart)
void resetMeet()

Resets the value of aforementioned values.

void resetNearObstacle()
void resetBackwardParent()
void resetForwardId()
void resetReverseId()
void resetForwardInvalid()
void resetReverseInvalid()
void resetForwardExpanded()
void resetReverseExpanded()
void resetCostToComeFromGoal()
void resetCostToComeFromStart()
void resetForwardVertexQueuePointer()

Resets the reverse queue pointer.

void resetReverseVertexQueuePointer()
VertexQueue::Element *getForwardVertexQueuePointer() const

Returns the reverse queue pointer of this vertex.

VertexQueue::Element *getReverseVertexQueuePointer() const
void setForwardVertexQueuePointer(typename VertexQueue::Element *pointer)

Sets the reverse queue pointer of this vertex.

void setReverseVertexQueuePointer(typename VertexQueue::Element *pointer)
void setForwardValidParent(const std::shared_ptr<Vertex> &vertex, const ompl::base::Cost &edgeCost)

Sets the valid parent vertex (in a valid path).

void setReverseValidParent(const std::shared_ptr<Vertex> &vertex, const ompl::base::Cost &edgeCost)
void setReverseVertexParent(const std::shared_ptr<Vertex> &vertex, const ompl::base::Cost &edgeCost)

Sets the parent vertex (in the reverse or forward -search tree).

void setForwardVertexParent(const std::shared_ptr<Vertex> &vertex, const ompl::base::Cost &edgeCost)