PlannerData.cpp
54 const ompl::base::PlannerDataEdge ompl::base::PlannerData::NO_EDGE = ompl::base::PlannerDataEdge();
55 const ompl::base::PlannerDataVertex ompl::base::PlannerData::NO_VERTEX = ompl::base::PlannerDataVertex(nullptr);
56 const unsigned int ompl::base::PlannerData::INVALID_INDEX = std::numeric_limits<unsigned int>::max();
104 unsigned int ompl::base::PlannerData::getEdges(unsigned int v, std::vector<unsigned int> &edgeList) const
110 boost::property_map<Graph::Type, boost::vertex_index_t>::type vertices = get(boost::vertex_index, *graph_);
120 std::pair<Graph::OEIterator, Graph::OEIterator> iterators = boost::out_edges(boost::vertex(v, *graph_), *graph_);
124 boost::property_map<Graph::Type, boost::vertex_index_t>::type vertices = get(boost::vertex_index, *graph_);
131 unsigned int ompl::base::PlannerData::getIncomingEdges(unsigned int v, std::vector<unsigned int> &edgeList) const
133 std::pair<Graph::IEIterator, Graph::IEIterator> iterators = boost::in_edges(boost::vertex(v, *graph_), *graph_);
136 boost::property_map<Graph::Type, boost::vertex_index_t>::type vertices = get(boost::vertex_index, *graph_);
146 std::pair<Graph::IEIterator, Graph::IEIterator> iterators = boost::in_edges(boost::vertex(v, *graph_), *graph_);
150 boost::property_map<Graph::Type, boost::vertex_index_t>::type vertices = get(boost::vertex_index, *graph_);
157 bool ompl::base::PlannerData::getEdgeWeight(unsigned int v1, unsigned int v2, Cost *weight) const
161 boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
165 boost::property_map<Graph::Type, boost::edge_weight_t>::type edges = get(boost::edge_weight, *graph_);
177 boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
181 boost::property_map<Graph::Type, boost::edge_weight_t>::type edges = get(boost::edge_weight, *graph_);
193 boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
212 const ompl::base::PlannerDataVertex &ompl::base::PlannerData::getVertex(unsigned int index) const
230 const ompl::base::PlannerDataEdge &ompl::base::PlannerData::getEdge(unsigned int v1, unsigned int v2) const
234 boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
249 boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
301 boost::function_property_map<std::function<std::string(Vertex)>, Vertex> coordsmap([this, &s](Vertex v)
359 const ompl::base::PlannerDataVertex &ompl::base::PlannerData::getStartVertex(unsigned int i) const
375 const ompl::base::PlannerDataVertex &ompl::base::PlannerData::getGoalVertex(unsigned int i) const
432 bool ompl::base::PlannerData::addEdge(unsigned int v1, unsigned int v2, const PlannerDataEdge &edge, Cost weight)
448 tie(e, added) = boost::add_edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), properties, *graph_);
487 boost::property_map<Graph::Type, edge_type_t>::type edgePropertyMap = get(edge_type_t(), *graph_);
533 boost::property_map<Graph::Type, vertex_type_t>::type vertexTypeMap = get(vertex_type_t(), *graph_);
544 boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
557 bool ompl::base::PlannerData::removeEdge(const PlannerDataVertex &v1, const PlannerDataVertex &v2)
625 setEdgeWeight(i, it->first, opt.motionCost(getVertex(i).getState(), getVertex(it->first).getState()));
638 void ompl::base::PlannerData::extractMinimumSpanningTree(unsigned int v, const base::OptimizationObjective &opt,
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:48
Abstract definition of optimization objectives.
Definition: OptimizationObjective.h:71
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:96
virtual bool isCostBetterThan(Cost c1, Cost c2) const
Check whether the the cost c1 is considered better than the cost c2. By default, this returns true if...
Definition: OptimizationObjective.cpp:70
virtual Cost motionCost(const State *s1, const State *s2) const =0
Get the cost that corresponds to the motion segment between s1 and s2.
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:101
An optimization objective which corresponds to optimizing path length.
Definition: PathLengthOptimizationObjective.h:48
virtual PlannerDataEdge * clone() const
Return a clone of this object, allocated from the heap.
Definition: PlannerData.h:132
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:59
virtual const State * getState() const
Retrieve the state associated with this vertex.
Definition: PlannerData.h:80
virtual PlannerDataVertex * clone() const
Return a clone of this object, allocated from the heap.
Definition: PlannerData.h:86
Wrapper class for the Boost.Graph representation of the PlannerData. This class inherits from a boost...
Definition: PlannerDataGraph.h:73
boost::graph_traits< Type >::vertex_descriptor Vertex
Boost.Graph vertex descriptor.
Definition: PlannerDataGraph.h:79
boost::graph_traits< Type >::edge_descriptor Edge
Boost.Graph edge descriptor.
Definition: PlannerDataGraph.h:81
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:175
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:157
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:315
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:173
bool edgeExists(unsigned int v1, unsigned int v2) const
Check whether an edge between vertex index v1 and index v2 exists.
Definition: PlannerData.cpp:188
bool isGoalVertex(unsigned int index) const
Returns true if the given vertex index is marked as a goal vertex.
Definition: PlannerData.cpp:354
void printGraphviz(std::ostream &out=std::cout) const
Writes a Graphviz dot file of this structure to the given stream.
Definition: PlannerData.cpp:260
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:333
void computeEdgeWeights()
Computes all edge weights using state space distance (i.e. getSpaceInformation()->distance())
Definition: PlannerData.cpp:630
const SpaceInformationPtr & getSpaceInformation() const
Return the instance of SpaceInformation used in this PlannerData.
Definition: PlannerData.cpp:757
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:569
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:413
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:422
unsigned int numEdges() const
Retrieve the number of edges in this structure.
Definition: PlannerData.cpp:207
StateStoragePtr extractStateStorage() const
Extract a ompl::base::GraphStateStorage object from this PlannerData. Memory for states is copied (th...
Definition: PlannerData.cpp:716
bool markStartState(const State *st)
Mark the given state as a start vertex. If the given state does not exist in a vertex,...
Definition: PlannerData.cpp:580
static const PlannerDataVertex NO_VERTEX
Representation for a non-existant vertex.
Definition: PlannerData.h:182
static const PlannerDataEdge NO_EDGE
Representation for a non-existant edge.
Definition: PlannerData.h:177
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:473
bool markGoalState(const State *st)
Mark the given state as a goal vertex. If the given state does not exist in a vertex,...
Definition: PlannerData.cpp:597
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:131
unsigned int numVertices() const
Retrieve the number of vertices in this structure.
Definition: PlannerData.cpp:202
Graph & toBoostGraph()
Extract a Boost.Graph object from this PlannerData.
Definition: PlannerData.cpp:744
virtual bool removeEdge(unsigned int v1, unsigned int v2)
Removes the edge between vertex indexes v1 and v2. Success is returned.
Definition: PlannerData.cpp:540
unsigned int numStartVertices() const
Returns the number of start vertices.
Definition: PlannerData.cpp:323
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:104
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:638
const PlannerDataEdge & getEdge(unsigned int v1, unsigned int v2) const
Retrieve a reference to the edge object connecting vertices with indexes v1 and v2....
Definition: PlannerData.cpp:230
void printPLY(std::ostream &out, bool asIs=false) const
Write a mesh of the planner graph to a stream. Insert additional vertices if asIs == true.
Definition: PlannerData.cpp:789
static const unsigned int INVALID_INDEX
Representation of an invalid vertex index.
Definition: PlannerData.h:184
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:359
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:341
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:212
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:432
virtual bool hasControls() const
Indicate whether any information about controls (ompl::control::Control) is stored in this instance.
Definition: PlannerData.cpp:784
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:685
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:391
bool vertexExists(const PlannerDataVertex &v) const
Check whether a vertex exists with the given vertex data.
Definition: PlannerData.cpp:197
unsigned int numGoalVertices() const
Returns the number of goal vertices.
Definition: PlannerData.cpp:328
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:375
void printGraphML(std::ostream &out=std::cout) const
Writes a GraphML file of this structure to the given stream.
Definition: PlannerData.cpp:284
bool isStartVertex(unsigned int index) const
Returns true if the given vertex index is marked as a start vertex.
Definition: PlannerData.cpp:349
virtual void decoupleFromPlanner()
Creates a deep copy of the states contained in the vertices of this PlannerData structure so that whe...
Definition: PlannerData.cpp:80
A shared pointer wrapper for ompl::base::SpaceInformation.
Representation of a space in which planning can be performed. Topology specific sampling,...
Definition: StateSpace.h:71
virtual unsigned int getDimension() const =0
Get the dimension of the space (not the dimension of the surrounding ambient space)
virtual void copyToReals(std::vector< double > &reals, const State *source) const
Copy all the real values from a state source to the array reals using getValueAddressAtLocation()
Definition: StateSpace.cpp:329