37 #include "ompl/base/PlannerData.h"
38 #include "ompl/base/PlannerDataGraph.h"
39 #include "ompl/base/StateStorage.h"
40 #include "ompl/base/OptimizationObjective.h"
41 #include "ompl/base/objectives/PathLengthOptimizationObjective.h"
42 #include "ompl/base/ScopedState.h"
44 #include <boost/graph/graphviz.hpp>
45 #include <boost/graph/graphml.hpp>
46 #include <boost/graph/dijkstra_shortest_paths.hpp>
47 #include <boost/version.hpp>
48 #if BOOST_VERSION < 105100
49 #warning Boost version >=1.51 is needed for ompl::base::PlannerData::printGraphML
51 #include <boost/property_map/function_property_map.hpp>
56 #define graph_ reinterpret_cast<ompl::base::PlannerData::Graph*>(graphRaw_)
64 graphRaw_ =
new Graph();
81 decoupledStates_.clear();
86 unsigned int count = 0;
87 for (
unsigned int i = 0; i < numVertices(); ++i)
91 if (decoupledStates_.find(const_cast<State*>(vtx.
getState())) == decoupledStates_.end())
94 State* clone = si_->cloneState(oldState);
95 decoupledStates_.insert(clone);
100 stateIndexMap_.erase(oldState);
102 stateIndexMap_[clone] = i;
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_);
115 edgeList.push_back(vertices[*iter]);
117 return edgeList.size();
122 std::pair<Graph::OEIterator, Graph::OEIterator> iterators = boost::out_edges(boost::vertex(v, *graph_), *graph_);
125 boost::property_map<Graph::Type, edge_type_t>::type edges =
get(edge_type_t(), *graph_);
126 boost::property_map<Graph::Type, boost::vertex_index_t>::type vertices =
get(boost::vertex_index, *graph_);
128 edgeMap[vertices[boost::target(*iter, *graph_)]] = boost::get(edges, *iter);
130 return edgeMap.size();
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_);
140 edgeList.push_back(vertices[boost::source(*iter, *graph_)]);
142 return edgeList.size();
147 std::pair<Graph::IEIterator, Graph::IEIterator> iterators = boost::in_edges(boost::vertex(v, *graph_), *graph_);
150 boost::property_map<Graph::Type, edge_type_t>::type edges =
get(edge_type_t(), *graph_);
151 boost::property_map<Graph::Type, boost::vertex_index_t>::type vertices =
get(boost::vertex_index, *graph_);
153 edgeMap[vertices[boost::source(*iter, *graph_)]] = boost::get(edges, *iter);
155 return edgeMap.size();
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_);
200 return vertexIndex(v) != INVALID_INDEX;
205 return boost::num_vertices(*graph_);
210 return boost::num_edges(*graph_);
215 if (index >= boost::num_vertices(*graph_))
218 boost::property_map<Graph::Type, vertex_type_t>::type vertices =
get(vertex_type_t(), *graph_);
219 return *(vertices[boost::vertex(index, *graph_)]);
224 if (index >= boost::num_vertices(*graph_))
227 boost::property_map<Graph::Type, vertex_type_t>::type vertices =
get(vertex_type_t(), *graph_);
228 return *(vertices[boost::vertex(index, *graph_)]);
235 boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
239 boost::property_map<Graph::Type, edge_type_t>::type edges =
get(edge_type_t(), *graph_);
240 return *(boost::get(edges, e));
250 boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
254 boost::property_map<Graph::Type, edge_type_t>::type edges =
get(edge_type_t(), *graph_);
255 return *(boost::get(edges, e));
263 boost::write_graphviz(out, *graph_);
273 return get(boost::edge_weight_t(), g)[e].v;
281 s = *
get(vertex_type_t(), g)[v]->getState();
282 std::vector<double> coords(s.
reals());
283 std::ostringstream sstream;
286 sstream << coords[0];
287 for (std::size_t i = 1; i < coords.size(); ++i)
288 sstream <<
',' << coords[i];
290 return sstream.str();
296 #if BOOST_VERSION < 105100
297 OMPL_WARN(
"Boost version >=1.51 is needed for ompl::base::PlannerData::printGraphML");
306 boost::function_property_map<
307 boost::function<double (ompl::base::PlannerData::Graph::Edge)>,
310 weightmap(boost::bind(&edgeWeightAsDouble, *graph_, _1));
312 boost::function_property_map<
313 boost::function<std::string (ompl::base::PlannerData::Graph::Vertex)>,
316 coordsmap(boost::bind(&vertexCoords, *graph_, s, _1));
320 boost::dynamic_properties dp;
321 dp.property(
"weight", weightmap);
322 dp.property(
"coords", coordsmap);
324 boost::write_graphml(out, *graph_, dp);
330 std::map<const State*, unsigned int>::const_iterator it = stateIndexMap_.find(v.
getState());
331 if (it != stateIndexMap_.end())
333 return INVALID_INDEX;
338 return startVertexIndices_.size();
343 return goalVertexIndices_.size();
348 if (i >= startVertexIndices_.size())
349 return INVALID_INDEX;
351 return startVertexIndices_[i];
356 if (i >= goalVertexIndices_.size())
357 return INVALID_INDEX;
359 return goalVertexIndices_[i];
364 return std::binary_search(startVertexIndices_.begin(), startVertexIndices_.end(), index);
369 return std::binary_search(goalVertexIndices_.begin(), goalVertexIndices_.end(), index);
374 if (i >= startVertexIndices_.size())
377 return getVertex(startVertexIndices_[i]);
382 if (i >= startVertexIndices_.size())
383 return const_cast<ompl::base::PlannerDataVertex&>(NO_VERTEX);
385 return getVertex(startVertexIndices_[i]);
390 if (i >= goalVertexIndices_.size())
393 return getVertex(goalVertexIndices_[i]);
398 if (i >= goalVertexIndices_.size())
399 return const_cast<ompl::base::PlannerDataVertex&>(NO_VERTEX);
401 return getVertex(goalVertexIndices_[i]);
408 return INVALID_INDEX;
410 unsigned int index = vertexIndex(st);
411 if (index == INVALID_INDEX)
416 boost::property_map<Graph::Type, boost::vertex_index_t>::type vertexIndexMap =
get(boost::vertex_index, *graph_);
419 stateIndexMap_[clone->
getState()] = numVertices()-1;
420 return vertexIndexMap[v];
427 unsigned int index = addVertex(v);
428 if (index != INVALID_INDEX)
436 unsigned int index = addVertex(v);
438 if (index != INVALID_INDEX)
447 if (v1 >= numVertices() || v2 >= numVertices())
451 if (edgeExists (v1, v2))
456 const Graph::edge_property_type properties(clone, weight);
460 tie(e, added) = boost::add_edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), properties, *graph_);
470 unsigned int index1 = addVertex(v1);
471 unsigned int index2 = addVertex(v2);
474 if (index1 == INVALID_INDEX && index2 == INVALID_INDEX)
478 if (index1 != INVALID_INDEX && index2 != INVALID_INDEX)
479 return addEdge (index1, index2, edge, weight);
486 unsigned int index = vertexIndex (st);
487 if (index != INVALID_INDEX)
488 return removeVertex (index);
494 if (vIndex >= boost::num_vertices(*graph_))
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_);
502 for (
Graph::OEIterator iter = oiterators.first; iter != oiterators.second; ++iter)
503 delete edgePropertyMap[*iter];
506 std::pair<Graph::IEIterator, Graph::IEIterator> initerators = boost::in_edges(boost::vertex(vIndex, *graph_), *graph_);
507 for (
Graph::IEIterator iter = initerators.first; iter != initerators.second; ++iter)
508 delete edgePropertyMap[*iter];
511 stateIndexMap_.erase(getVertex(vIndex).getState());
512 boost::property_map<Graph::Type, vertex_type_t>::type vertices =
get(vertex_type_t(), *graph_);
513 for (
unsigned int i = vIndex+1; i < boost::num_vertices(*graph_); ++i)
514 stateIndexMap_[vertices[boost::vertex(i, *graph_)]->getState()]--;
517 std::vector<unsigned int>::iterator it = std::find(startVertexIndices_.begin(), startVertexIndices_.end(), vIndex);
518 if (it != startVertexIndices_.end())
519 startVertexIndices_.erase(it);
520 for (
size_t i = 0; i < startVertexIndices_.size(); ++i)
521 if (startVertexIndices_[i] > vIndex)
522 startVertexIndices_[i]--;
524 it = std::find(goalVertexIndices_.begin(), goalVertexIndices_.end(), vIndex);
525 if (it != goalVertexIndices_.end())
526 goalVertexIndices_.erase(it);
527 for (
size_t i = 0; i < goalVertexIndices_.size(); ++i)
528 if (goalVertexIndices_[i] > vIndex)
529 goalVertexIndices_[i]--;
532 State* vtxState =
const_cast<State*
>(getVertex(vIndex).getState());
533 if (decoupledStates_.find(vtxState) != decoupledStates_.end())
535 decoupledStates_.erase(vtxState);
536 si_->freeState(vtxState);
541 boost::clear_vertex(boost::vertex(vIndex, *graph_), *graph_);
542 boost::property_map<Graph::Type, vertex_type_t>::type vertexTypeMap =
get(vertex_type_t(), *graph_);
543 delete vertexTypeMap[boost::vertex(vIndex, *graph_)];
544 boost::remove_vertex(boost::vertex(vIndex, *graph_), *graph_);
553 boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
559 boost::property_map<Graph::Type, edge_type_t>::type edges =
get(edge_type_t(), *graph_);
562 boost::remove_edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
568 unsigned int index1, index2;
569 index1 = vertexIndex(v1);
570 index2 = vertexIndex(v2);
572 if (index1 == INVALID_INDEX || index2 == INVALID_INDEX)
575 return removeEdge (index1, index2);
580 std::map<const State*, unsigned int>::const_iterator it = stateIndexMap_.find(st);
581 if (it != stateIndexMap_.end())
583 getVertex(it->second).setTag(tag);
592 std::map<const State*, unsigned int>::const_iterator it = stateIndexMap_.find(st);
593 if (it != stateIndexMap_.end())
595 if (!isStartVertex(it->second))
597 startVertexIndices_.push_back(it->second);
599 std::sort(startVertexIndices_.begin(), startVertexIndices_.end());
609 std::map<const State*, unsigned int>::const_iterator it = stateIndexMap_.find(st);
610 if (it != stateIndexMap_.end())
612 if (!isGoalVertex(it->second))
614 goalVertexIndices_.push_back(it->second);
616 std::sort(startVertexIndices_.begin(), startVertexIndices_.end());
625 unsigned int nv = numVertices();
626 for (
unsigned int i = 0; i < nv; ++i)
628 std::map<unsigned int, const PlannerDataEdge*> nbrs;
631 std::map<unsigned int, const PlannerDataEdge*>::const_iterator it;
632 for (it = nbrs.begin(); it != nbrs.end(); ++it)
634 setEdgeWeight(i, it->first, opt.
motionCost(getVertex(i).getState(),
635 getVertex(it->first).getState()));
645 computeEdgeWeights(opt);
661 std::vector<ompl::base::PlannerData::Graph::Vertex> pred(numVertices());
670 boost::dijkstra_shortest_paths
672 boost::predecessor_map(&pred[0]).
674 isCostBetterThan, &opt, _1, _2)).
675 distance_combine(&project2nd).
680 for (std::size_t i = 0; i < pred.size(); ++i)
682 if (isStartVertex(i))
684 else if (isGoalVertex(i))
691 for (std::size_t i = 0; i < pred.size(); ++i)
696 getEdgeWeight(pred[i], i, &c);
697 mst.
addEdge(pred[i], i, getEdge(pred[i], i), c);
710 if (isStartVertex(v))
712 else if (isGoalVertex(v))
717 assert (idx != INVALID_INDEX);
719 std::map<unsigned int, const PlannerDataEdge*> neighbors;
720 getEdges(v, neighbors);
723 std::map<unsigned int, const PlannerDataEdge*>::iterator it;
724 for (it = neighbors.begin(); it != neighbors.end(); ++it)
726 extractReachable(it->first, data);
728 getEdgeWeight(v, it->first, &weight);
739 std::map<unsigned int, unsigned int> indexMap;
740 for (std::map<const State*, unsigned int>::const_iterator it = stateIndexMap_.begin() ; it != stateIndexMap_.end() ; ++it)
742 indexMap[it->second] = store->
size();
747 for (std::map<unsigned int, unsigned int>::const_iterator it = indexMap.begin() ; it != indexMap.end() ; ++it)
749 std::vector<unsigned int> edgeList;
750 getEdges(it->first, edgeList);
752 md.resize(edgeList.size());
754 for (std::size_t k = 0 ; k < edgeList.size() ; ++k)
755 md[k] = indexMap[edgeList[k]];
758 return StateStoragePtr(store);
778 void ompl::base::PlannerData::freeMemory(
void)
781 for (std::set<State*>::iterator it = decoupledStates_.begin(); it != decoupledStates_.end(); ++it)
786 std::pair<Graph::EIterator, Graph::EIterator> eiterators = boost::edges(*graph_);
787 boost::property_map<Graph::Type, edge_type_t>::type edges =
get(edge_type_t(), *graph_);
788 for (Graph::EIterator iter = eiterators.first; iter != eiterators.second; ++iter)
789 delete boost::get(edges, *iter);
791 std::pair<Graph::VIterator, Graph::VIterator> viterators = boost::vertices(*graph_);
792 boost::property_map<Graph::Type, vertex_type_t>::type vertices =
get(vertex_type_t(), *graph_);
793 for (Graph::VIterator iter = viterators.first; iter != viterators.second; ++iter)
794 delete vertices[*iter];
const State * state_
The state represented by this vertex.
unsigned int numVertices(void) const
Retrieve the number of vertices in this structure.
virtual PlannerDataVertex * clone(void) const
Return a clone of this object, allocated from the heap.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
void computeEdgeWeights()
Computes all edge weights using state space distance (i.e. getSpaceInformation()->distance()) ...
virtual ~PlannerData(void)
Destructor.
Wrapper class for the Boost.Graph representation of the PlannerData. This class inherits from a boost...
unsigned int numStartVertices(void) const
Returns the number of start vertices.
boost::graph_traits< Type >::edge_descriptor Edge
Boost.Graph edge descriptor.
bool vertexExists(const PlannerDataVertex &v) const
Check whether a vertex exists with the given vertex data.
boost::graph_traits< Type >::adjacency_iterator AdjIterator
Boost.Graph adjacency iterator.
PlannerDataGraph Type
Data type for the Boost.Graph representation.
Definition of a scoped state.
std::size_t size(void) const
Return the number of stored states.
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...
boost::graph_traits< Type >::in_edge_iterator IEIterator
Boost.Graph input edge iterator.
unsigned int numGoalVertices(void) const
Returns the number of goal vertices.
void extractReachable(unsigned int v, PlannerData &data) const
Extracts the subset of PlannerData reachable from the vertex with index v. For tree structures...
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.
unsigned int addVertex(const PlannerDataVertex &st)
Adds the given vertex to the graph data. The vertex index is returned. Duplicates are not added...
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...
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...
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...
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...
PlannerData(const SpaceInformationPtr &si)
Constructor. Accepts a SpaceInformationPtr for the space planned in.
virtual Cost infiniteCost() const
Get a cost which is greater than all other costs in this OptimizationObjective; required for use in D...
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...
virtual Cost motionCost(const State *s1, const State *s2) const =0
Get the cost that corresponds to the motion segment between s1 and s2.
StateStoragePtr extractStateStorage(void) const
Extract a ompl::base::GraphStateStorage object from this PlannerData. Memory for states is copied (th...
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)...
boost::graph_traits< Type >::out_edge_iterator OEIterator
Boost.Graph output edge iterator.
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
virtual const State * getState(void) const
Retrieve the state associated with this vertex.
virtual bool removeVertex(const PlannerDataVertex &st)
Removes the vertex associated with the given data. If the vertex does not exist, false is returned...
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
bool isGoalVertex(unsigned int index) const
Returns true if the given vertex index is marked as a goal vertex.
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...
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...
void printGraphML(std::ostream &out=std::cout) const
Writes a GraphML file of this structure to the given stream.
boost::graph_traits< Type >::vertex_descriptor Vertex
Boost.Graph vertex descriptor.
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...
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...
Graph & toBoostGraph(void)
Extract a Boost.Graph object from this PlannerData.
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...
const PlannerDataVertex & getVertex(unsigned int index) const
Retrieve a reference to the vertex object with the given index. If this vertex does not exist...
void printGraphviz(std::ostream &out=std::cout) const
Writes a Graphviz dot file of this structure to the given stream.
An optimization objective which corresponds to optimizing path length.
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 of an abstract state.
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
virtual PlannerDataEdge * clone() const
Return a clone of this object, allocated from the heap.
static const PlannerDataVertex NO_VERTEX
Representation for a non-existant vertex.
Abstract definition of optimization objectives.
bool edgeExists(unsigned int v1, unsigned int v2) const
Check whether an edge between vertex index v1 and index v2 exists.
const SpaceInformationPtr & getSpaceInformation(void) const
Return the instance of SpaceInformation used in this PlannerData.
static const unsigned int INVALID_INDEX
Representation of an invalid vertex index.
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.
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.
virtual bool hasControls(void) const
Indicate whether any information about controls (ompl::control::Control) is stored in this instance...
bool isStartVertex(unsigned int index) const
Returns true if the given vertex index is marked as a start vertex.
virtual void clear(void)
Clears the entire data structure.
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...
Base class for a PlannerData edge.
virtual void decoupleFromPlanner(void)
Creates a deep copy of the states contained in the vertices of this PlannerData structure so that whe...
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...
virtual bool removeEdge(unsigned int v1, unsigned int v2)
Removes the edge between vertex indexes v1 and v2. Success is returned.
std::vector< double > reals(void) const
Return the real values corresponding to this state. If a conversion is not possible, an exception is thrown.
virtual Cost identityCost() const
Get the identity cost value. The identity cost value is the cost c_i such that, for all costs c...
unsigned int numEdges(void) const
Retrieve the number of edges in this structure.
static const PlannerDataEdge NO_EDGE
Representation for a non-existant edge.