PlannerData.cpp
58 const ompl::base::PlannerDataEdge ompl::base::PlannerData::NO_EDGE = ompl::base::PlannerDataEdge();
59 const ompl::base::PlannerDataVertex ompl::base::PlannerData::NO_VERTEX = ompl::base::PlannerDataVertex(0);
60 const unsigned int ompl::base::PlannerData::INVALID_INDEX = std::numeric_limits<unsigned int>::max();
108 unsigned int ompl::base::PlannerData::getEdges (unsigned int v, std::vector<unsigned int>& edgeList) const
110 std::pair<Graph::AdjIterator, Graph::AdjIterator> iterators = boost::adjacent_vertices(boost::vertex(v, *graph_), *graph_);
113 boost::property_map<Graph::Type, boost::vertex_index_t>::type vertices = get(boost::vertex_index, *graph_);
120 unsigned int ompl::base::PlannerData::getEdges (unsigned int v, std::map<unsigned int, const PlannerDataEdge*>& edgeMap) const
122 std::pair<Graph::OEIterator, Graph::OEIterator> iterators = boost::out_edges(boost::vertex(v, *graph_), *graph_);
126 boost::property_map<Graph::Type, boost::vertex_index_t>::type vertices = get(boost::vertex_index, *graph_);
133 unsigned int ompl::base::PlannerData::getIncomingEdges (unsigned int v, std::vector<unsigned int>& edgeList) const
135 std::pair<Graph::IEIterator, Graph::IEIterator> iterators = boost::in_edges(boost::vertex(v, *graph_), *graph_);
138 boost::property_map<Graph::Type, boost::vertex_index_t>::type vertices = get(boost::vertex_index, *graph_);
145 unsigned int ompl::base::PlannerData::getIncomingEdges (unsigned int v, std::map<unsigned int, const PlannerDataEdge*> &edgeMap) const
147 std::pair<Graph::IEIterator, Graph::IEIterator> iterators = boost::in_edges(boost::vertex(v, *graph_), *graph_);
151 boost::property_map<Graph::Type, boost::vertex_index_t>::type vertices = get(boost::vertex_index, *graph_);
158 bool ompl::base::PlannerData::getEdgeWeight(unsigned int v1, unsigned int v2, Cost* weight) const
162 boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
166 boost::property_map<Graph::Type, boost::edge_weight_t>::type edges = get(boost::edge_weight, *graph_);
178 boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
182 boost::property_map<Graph::Type, boost::edge_weight_t>::type edges = get(boost::edge_weight, *graph_);
194 boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
213 const ompl::base::PlannerDataVertex& ompl::base::PlannerData::getVertex (unsigned int index) const
231 const ompl::base::PlannerDataEdge& ompl::base::PlannerData::getEdge (unsigned int v1, unsigned int v2) const
235 boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
246 ompl::base::PlannerDataEdge& ompl::base::PlannerData::getEdge (unsigned int v1, unsigned int v2)
250 boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
372 const ompl::base::PlannerDataVertex& ompl::base::PlannerData::getStartVertex (unsigned int i) const
388 const ompl::base::PlannerDataVertex& ompl::base::PlannerData::getGoalVertex (unsigned int i) const
416 boost::property_map<Graph::Type, boost::vertex_index_t>::type vertexIndexMap = get(boost::vertex_index, *graph_);
444 bool ompl::base::PlannerData::addEdge(unsigned int v1, unsigned int v2, const PlannerDataEdge &edge, Cost weight)
460 tie(e, added) = boost::add_edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), properties, *graph_);
468 bool ompl::base::PlannerData::addEdge (const PlannerDataVertex & v1, const PlannerDataVertex & v2, const PlannerDataEdge &edge, Cost weight)
498 boost::property_map<Graph::Type, edge_type_t>::type edgePropertyMap = get(edge_type_t(), *graph_);
501 std::pair<Graph::OEIterator, Graph::OEIterator> oiterators = boost::out_edges(boost::vertex(vIndex, *graph_), *graph_);
506 std::pair<Graph::IEIterator, Graph::IEIterator> initerators = boost::in_edges(boost::vertex(vIndex, *graph_), *graph_);
517 std::vector<unsigned int>::iterator it = std::find(startVertexIndices_.begin(), startVertexIndices_.end(), vIndex);
542 boost::property_map<Graph::Type, vertex_type_t>::type vertexTypeMap = get(vertex_type_t(), *graph_);
553 boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
566 bool ompl::base::PlannerData::removeEdge (const PlannerDataVertex &v1, const PlannerDataVertex &v2)
740 for (std::map<const State*, unsigned int>::const_iterator it = stateIndexMap_.begin() ; it != stateIndexMap_.end() ; ++it)
747 for (std::map<unsigned int, unsigned int>::const_iterator it = indexMap.begin() ; it != indexMap.end() ; ++it)
763 ompl::base::PlannerData::Graph *boostgraph = reinterpret_cast<ompl::base::PlannerData::Graph*>(graphRaw_);
769 const ompl::base::PlannerData::Graph *boostgraph = reinterpret_cast<const ompl::base::PlannerData::Graph*>(graphRaw_);
781 for (std::set<State*>::iterator it = decoupledStates_.begin(); it != decoupledStates_.end(); ++it)
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:164
void computeEdgeWeights()
Computes all edge weights using state space distance (i.e. getSpaceInformation()->distance()) ...
Definition: PlannerData.cpp:640
Wrapper class for the Boost.Graph representation of the PlannerData. This class inherits from a boost...
Definition: PlannerDataGraph.h:65
StateStoragePtr extractStateStorage() const
Extract a ompl::base::GraphStateStorage object from this PlannerData. Memory for states is copied (th...
Definition: PlannerData.cpp:733
std::vector< unsigned int > startVertexIndices_
A mutable listing of the vertices marked as start states. Stored in sorted order. ...
Definition: PlannerData.h:400
boost::graph_traits< Type >::edge_descriptor Edge
Boost.Graph edge descriptor.
Definition: PlannerDataGraph.h:74
bool vertexExists(const PlannerDataVertex &v) const
Check whether a vertex exists with the given vertex data.
Definition: PlannerData.cpp:198
virtual void decoupleFromPlanner()
Creates a deep copy of the states contained in the vertices of this PlannerData structure so that whe...
Definition: PlannerData.cpp:84
boost::graph_traits< Type >::adjacency_iterator AdjIterator
Boost.Graph adjacency iterator.
Definition: PlannerDataGraph.h:84
PlannerDataGraph Type
Data type for the Boost.Graph representation.
Definition: PlannerDataGraph.h:69
virtual bool hasControls() const
Indicate whether any information about controls (ompl::control::Control) is stored in this instance...
Definition: PlannerData.cpp:800
unsigned int addGoalVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
Definition: PlannerData.cpp:434
boost::graph_traits< Type >::in_edge_iterator IEIterator
Boost.Graph input edge iterator.
Definition: PlannerDataGraph.h:80
void extractReachable(unsigned int v, PlannerData &data) const
Extracts the subset of PlannerData reachable from the vertex with index v. For tree structures...
Definition: PlannerData.cpp:702
bool markStartState(const State *st)
Mark the given state as a start vertex. If the given state does not exist in a vertex, false is returned.
Definition: PlannerData.cpp:589
Graph & toBoostGraph()
Extract a Boost.Graph object from this PlannerData.
Definition: PlannerData.cpp:761
unsigned int addVertex(const PlannerDataVertex &st)
Adds the given vertex to the graph data. The vertex index is returned. Duplicates are not added...
Definition: PlannerData.cpp:404
unsigned int numGoalVertices() const
Returns the number of goal vertices.
Definition: PlannerData.cpp:341
const PlannerDataVertex & getStartVertex(unsigned int i) const
Retrieve a reference to the ith start vertex object. If i is greater than the number of start vertice...
Definition: PlannerData.cpp:372
std::vector< unsigned int > goalVertexIndices_
A mutable listing of the vertices marked as goal states. Stored in sorted order.
Definition: PlannerData.h:402
unsigned int getStartIndex(unsigned int i) const
Returns the index of the ith start state. INVALID_INDEX is returned if i is out of range...
Definition: PlannerData.cpp:346
boost::graph_traits< Type >::edge_iterator EIterator
Boost.Graph edge iterator.
Definition: PlannerDataGraph.h:78
bool getEdgeWeight(unsigned int v1, unsigned int v2, Cost *weight) const
Returns the weight of the edge between the given vertex indices. If there exists an edge between v1 a...
Definition: PlannerData.cpp:158
unsigned int getIncomingEdges(unsigned int v, std::vector< unsigned int > &edgeList) const
Returns a list of vertices with outgoing edges to the vertex with index v. The number of edges connec...
Definition: PlannerData.cpp:133
PlannerData(const SpaceInformationPtr &si)
Constructor. Accepts a SpaceInformationPtr for the space planned in.
Definition: PlannerData.cpp:62
virtual Cost infiniteCost() const
Get a cost which is greater than all other costs in this OptimizationObjective; required for use in D...
Definition: OptimizationObjective.cpp:83
virtual void addState(const State *state)
Add a state to the set of states maintained by this storage structure. The state is copied to interna...
Definition: StateStorage.h:226
virtual Cost motionCost(const State *s1, const State *s2) const =0
Get the cost that corresponds to the motion segment between s1 and s2.
boost::graph_traits< Type >::vertex_iterator VIterator
Boost.Graph vertex iterator.
Definition: PlannerDataGraph.h:76
bool setEdgeWeight(unsigned int v1, unsigned int v2, Cost weight)
Sets the weight of the edge between the given vertex indices. If an edge between v1 and v2 does not e...
Definition: PlannerData.cpp:174
unsigned int getEdges(unsigned int v, std::vector< unsigned int > &edgeList) const
Returns a list of the vertex indexes directly connected to vertex with index v (outgoing edges)...
Definition: PlannerData.cpp:108
std::set< State * > decoupledStates_
A list of states that are allocated during the decoupleFromPlanner method. These states are freed by ...
Definition: PlannerData.h:408
boost::graph_traits< Type >::out_edge_iterator OEIterator
Boost.Graph output edge iterator.
Definition: PlannerDataGraph.h:82
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:60
virtual bool removeVertex(const PlannerDataVertex &st)
Removes the vertex associated with the given data. If the vertex does not exist, false is returned...
Definition: PlannerData.cpp:484
bool isGoalVertex(unsigned int index) const
Returns true if the given vertex index is marked as a goal vertex.
Definition: PlannerData.cpp:367
const PlannerDataVertex & getGoalVertex(unsigned int i) const
Retrieve a reference to the ith goal vertex object. If i is greater than the number of goal vertices...
Definition: PlannerData.cpp:388
bool tagState(const State *st, int tag)
Set the integer tag associated with the given state. If the given state does not exist in a vertex...
Definition: PlannerData.cpp:578
unsigned int numEdges() const
Retrieve the number of edges in this structure.
Definition: PlannerData.cpp:208
void printGraphML(std::ostream &out=std::cout) const
Writes a GraphML file of this structure to the given stream.
Definition: PlannerData.cpp:294
unsigned int numVertices() const
Retrieve the number of vertices in this structure.
Definition: PlannerData.cpp:203
boost::graph_traits< Type >::vertex_descriptor Vertex
Boost.Graph vertex descriptor.
Definition: PlannerDataGraph.h:72
StateStorageWithMetadata< std::vector< std::size_t > > GraphStateStorage
Storage of states where the metadata is a vector of indices. This is is typically used to store a gra...
Definition: StateStorage.h:277
unsigned int vertexIndex(const PlannerDataVertex &v) const
Return the index for the vertex associated with the given data. INVALID_INDEX is returned if this ver...
Definition: PlannerData.cpp:328
const M & getMetadata(unsigned int index) const
Get const access to the metadata of a state at a particular index.
Definition: StateStorage.h:246
virtual bool addEdge(unsigned int v1, unsigned int v2, const PlannerDataEdge &edge=PlannerDataEdge(), Cost weight=Cost(1.0))
Adds a directed edge between the given vertex indexes. An optional edge structure and weight can be s...
Definition: PlannerData.cpp:444
A boost shared pointer wrapper for ompl::base::SpaceInformation.
const PlannerDataVertex & getVertex(unsigned int index) const
Retrieve a reference to the vertex object with the given index. If this vertex does not exist...
Definition: PlannerData.cpp:213
void printGraphviz(std::ostream &out=std::cout) const
Writes a Graphviz dot file of this structure to the given stream.
Definition: PlannerData.cpp:261
An optimization objective which corresponds to optimizing path length.
Definition: PathLengthOptimizationObjective.h:47
unsigned int addStartVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
Definition: PlannerData.cpp:425
std::vector< double > reals() const
Return the real values corresponding to this state. If a conversion is not possible, an exception is thrown.
Definition: ScopedState.h:356
virtual PlannerDataEdge * clone() const
Return a clone of this object, allocated from the heap.
Definition: PlannerData.h:123
static const PlannerDataVertex NO_VERTEX
Representation for a non-existant vertex.
Definition: PlannerData.h:172
State storage that allows storing state metadata as well.
Definition: StateStorage.h:210
Abstract definition of optimization objectives.
Definition: OptimizationObjective.h:66
bool edgeExists(unsigned int v1, unsigned int v2) const
Check whether an edge between vertex index v1 and index v2 exists.
Definition: PlannerData.cpp:189
static const unsigned int INVALID_INDEX
Representation of an invalid vertex index.
Definition: PlannerData.h:174
const PlannerDataEdge & getEdge(unsigned int v1, unsigned int v2) const
Retrieve a reference to the edge object connecting vertices with indexes v1 and v2. If this edge does not exist, NO_EDGE is returned.
Definition: PlannerData.cpp:231
bool markGoalState(const State *st)
Mark the given state as a goal vertex. If the given state does not exist in a vertex, false is returned.
Definition: PlannerData.cpp:606
bool isStartVertex(unsigned int index) const
Returns true if the given vertex index is marked as a start vertex.
Definition: PlannerData.cpp:362
virtual void setTag(int tag)
Set the integer tag associated with this vertex.
Definition: PlannerData.h:72
unsigned int numStartVertices() const
Returns the number of start vertices.
Definition: PlannerData.cpp:336
void extractMinimumSpanningTree(unsigned int v, const OptimizationObjective &opt, PlannerData &mst) const
Extracts the minimum spanning tree of the data rooted at the vertex with index v. The minimum spannin...
Definition: PlannerData.cpp:657
const SpaceInformationPtr & getSpaceInformation() const
Return the instance of SpaceInformation used in this PlannerData.
Definition: PlannerData.cpp:773
std::map< const State *, unsigned int > stateIndexMap_
A mapping of states to vertex indexes. For fast lookup of vertex index.
Definition: PlannerData.h:398
virtual PlannerDataVertex * clone() const
Return a clone of this object, allocated from the heap.
Definition: PlannerData.h:77
virtual const State * getState() const
Retrieve the state associated with this vertex.
Definition: PlannerData.h:74
unsigned int getGoalIndex(unsigned int i) const
Returns the index of the ith goal state. INVALID_INDEX is returned if i is out of range Indexes are v...
Definition: PlannerData.cpp:354
virtual bool removeEdge(unsigned int v1, unsigned int v2)
Removes the edge between vertex indexes v1 and v2. Success is returned.
Definition: PlannerData.cpp:549
virtual Cost identityCost() const
Get the identity cost value. The identity cost value is the cost c_i such that, for all costs c...
Definition: OptimizationObjective.cpp:78
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
static const PlannerDataEdge NO_EDGE
Representation for a non-existant edge.
Definition: PlannerData.h:167
std::map< std::string, std::string > properties
Any extra properties (key-value pairs) the planner can set.
Definition: PlannerData.h:394