37 #include "ompl/base/PlannerData.h"
38 #include "ompl/base/PlannerDataGraph.h"
40 #include <boost/graph/graphviz.hpp>
41 #include <boost/graph/graphml.hpp>
42 #include <boost/graph/prim_minimum_spanning_tree.hpp>
46 #define graph_ reinterpret_cast<ompl::base::PlannerData::Graph*>(graphRaw_)
55 graphRaw_ =
new Graph();
72 decoupledStates_.clear();
77 unsigned int count = 0;
78 for (
unsigned int i = 0; i < numVertices(); ++i)
82 if (decoupledStates_.find(const_cast<State*>(vtx.
getState())) == decoupledStates_.end())
85 State* clone = si_->cloneState(oldState);
86 decoupledStates_.insert(clone);
91 stateIndexMap_.erase(oldState);
93 stateIndexMap_[clone] = i;
101 std::pair<Graph::AdjIterator, Graph::AdjIterator> iterators = boost::adjacent_vertices(boost::vertex(v, *graph_), *graph_);
104 boost::property_map<Graph::Type, boost::vertex_index_t>::type vertices =
get(boost::vertex_index, *graph_);
106 edgeList.push_back(vertices[*iter]);
108 return edgeList.size();
113 std::pair<Graph::OEIterator, Graph::OEIterator> iterators = boost::out_edges(boost::vertex(v, *graph_), *graph_);
116 boost::property_map<Graph::Type, edge_type_t>::type edges =
get(edge_type_t(), *graph_);
117 boost::property_map<Graph::Type, boost::vertex_index_t>::type vertices =
get(boost::vertex_index, *graph_);
119 edgeMap[vertices[boost::target(*iter, *graph_)]] = boost::get(edges, *iter);
121 return edgeMap.size();
126 std::pair<Graph::IEIterator, Graph::IEIterator> iterators = boost::in_edges(boost::vertex(v, *graph_), *graph_);
129 boost::property_map<Graph::Type, boost::vertex_index_t>::type vertices =
get(boost::vertex_index, *graph_);
131 edgeList.push_back(vertices[boost::source(*iter, *graph_)]);
133 return edgeList.size();
138 std::pair<Graph::IEIterator, Graph::IEIterator> iterators = boost::in_edges(boost::vertex(v, *graph_), *graph_);
141 boost::property_map<Graph::Type, edge_type_t>::type edges =
get(edge_type_t(), *graph_);
142 boost::property_map<Graph::Type, boost::vertex_index_t>::type vertices =
get(boost::vertex_index, *graph_);
144 edgeMap[vertices[boost::source(*iter, *graph_)]] = boost::get(edges, *iter);
146 return edgeMap.size();
153 boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
157 boost::property_map<Graph::Type, boost::edge_weight_t>::type edges =
get(boost::edge_weight, *graph_);
161 return INVALID_WEIGHT;
168 boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
172 boost::property_map<Graph::Type, boost::edge_weight_t>::type edges =
get(boost::edge_weight, *graph_);
184 boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
190 return vertexIndex(v) != INVALID_INDEX;
195 return boost::num_vertices(*graph_);
200 return boost::num_edges(*graph_);
205 if (index >= boost::num_vertices(*graph_))
208 boost::property_map<Graph::Type, vertex_type_t>::type vertices =
get(vertex_type_t(), *graph_);
209 return *(vertices[boost::vertex(index, *graph_)]);
214 if (index >= boost::num_vertices(*graph_))
217 boost::property_map<Graph::Type, vertex_type_t>::type vertices =
get(vertex_type_t(), *graph_);
218 return *(vertices[boost::vertex(index, *graph_)]);
225 boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
229 boost::property_map<Graph::Type, edge_type_t>::type edges =
get(edge_type_t(), *graph_);
230 return *(boost::get(edges, e));
240 boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
244 boost::property_map<Graph::Type, edge_type_t>::type edges =
get(edge_type_t(), *graph_);
245 return *(boost::get(edges, e));
253 boost::write_graphviz(out, *graph_);
259 boost::dynamic_properties dp;
260 dp.property(
"weight",
get(boost::edge_weight_t(), *graph_));
262 boost::write_graphml(out, *graph_, dp);
267 std::map<const State*, unsigned int>::const_iterator it = stateIndexMap_.find(v.
getState());
268 if (it != stateIndexMap_.end())
270 return INVALID_INDEX;
275 return startVertexIndices_.size();
280 return goalVertexIndices_.size();
285 if (i >= startVertexIndices_.size())
286 return INVALID_INDEX;
288 return startVertexIndices_[i];
293 if (i >= goalVertexIndices_.size())
294 return INVALID_INDEX;
296 return goalVertexIndices_[i];
301 return std::binary_search(startVertexIndices_.begin(), startVertexIndices_.end(), index);
306 return std::binary_search(goalVertexIndices_.begin(), goalVertexIndices_.end(), index);
311 if (i >= startVertexIndices_.size())
314 return getVertex(startVertexIndices_[i]);
319 if (i >= startVertexIndices_.size())
320 return const_cast<ompl::base::PlannerDataVertex&>(NO_VERTEX);
322 return getVertex(startVertexIndices_[i]);
327 if (i >= goalVertexIndices_.size())
330 return getVertex(goalVertexIndices_[i]);
335 if (i >= goalVertexIndices_.size())
336 return const_cast<ompl::base::PlannerDataVertex&>(NO_VERTEX);
338 return getVertex(goalVertexIndices_[i]);
345 return INVALID_INDEX;
347 unsigned int index = vertexIndex(st);
348 if (index == INVALID_INDEX)
353 boost::property_map<Graph::Type, boost::vertex_index_t>::type vertexIndexMap =
get(boost::vertex_index, *graph_);
356 stateIndexMap_[clone->
getState()] = numVertices()-1;
357 return vertexIndexMap[v];
364 unsigned int index = addVertex(v);
365 if (index != INVALID_INDEX)
373 unsigned int index = addVertex(v);
375 if (index != INVALID_INDEX)
384 if (v1 >= numVertices() || v2 >= numVertices())
388 if (edgeExists (v1, v2))
393 const Graph::edge_property_type properties(clone, weight);
397 tie(e, added) = boost::add_edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), properties, *graph_);
407 unsigned int index1 = addVertex(v1);
408 unsigned int index2 = addVertex(v2);
411 if (index1 == INVALID_INDEX && index2 == INVALID_INDEX)
415 if (index1 != INVALID_INDEX && index2 != INVALID_INDEX)
416 return addEdge (index1, index2, edge, weight);
423 unsigned int index = vertexIndex (st);
424 if (index != INVALID_INDEX)
425 return removeVertex (index);
431 if (vIndex >= boost::num_vertices(*graph_))
435 boost::property_map<Graph::Type, edge_type_t>::type edgePropertyMap =
get(edge_type_t(), *graph_);
438 std::pair<Graph::OEIterator, Graph::OEIterator> oiterators = boost::out_edges(boost::vertex(vIndex, *graph_), *graph_);
439 for (
Graph::OEIterator iter = oiterators.first; iter != oiterators.second; ++iter)
440 delete edgePropertyMap[*iter];
443 std::pair<Graph::IEIterator, Graph::IEIterator> initerators = boost::in_edges(boost::vertex(vIndex, *graph_), *graph_);
444 for (
Graph::IEIterator iter = initerators.first; iter != initerators.second; ++iter)
445 delete edgePropertyMap[*iter];
448 stateIndexMap_.erase(getVertex(vIndex).getState());
449 boost::property_map<Graph::Type, vertex_type_t>::type vertices =
get(vertex_type_t(), *graph_);
450 for (
unsigned int i = vIndex+1; i < boost::num_vertices(*graph_); ++i)
451 stateIndexMap_[vertices[boost::vertex(i, *graph_)]->getState()]--;
454 std::vector<unsigned int>::iterator it = std::find(startVertexIndices_.begin(), startVertexIndices_.end(), vIndex);
455 if (it != startVertexIndices_.end())
456 startVertexIndices_.erase(it);
457 for (
size_t i = 0; i < startVertexIndices_.size(); ++i)
458 if (startVertexIndices_[i] > vIndex)
459 startVertexIndices_[i]--;
461 it = std::find(goalVertexIndices_.begin(), goalVertexIndices_.end(), vIndex);
462 if (it != goalVertexIndices_.end())
463 goalVertexIndices_.erase(it);
464 for (
size_t i = 0; i < goalVertexIndices_.size(); ++i)
465 if (goalVertexIndices_[i] > vIndex)
466 goalVertexIndices_[i]--;
469 State* vtxState =
const_cast<State*
>(getVertex(vIndex).getState());
470 if (decoupledStates_.find(vtxState) != decoupledStates_.end())
472 decoupledStates_.erase(vtxState);
473 si_->freeState(vtxState);
478 boost::clear_vertex(boost::vertex(vIndex, *graph_), *graph_);
479 boost::property_map<Graph::Type, vertex_type_t>::type vertexTypeMap =
get(vertex_type_t(), *graph_);
480 delete vertexTypeMap[boost::vertex(vIndex, *graph_)];
481 boost::remove_vertex(boost::vertex(vIndex, *graph_), *graph_);
490 boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
496 boost::property_map<Graph::Type, edge_type_t>::type edges =
get(edge_type_t(), *graph_);
499 boost::remove_edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
505 unsigned int index1, index2;
506 index1 = vertexIndex(v1);
507 index2 = vertexIndex(v2);
509 if (index1 == INVALID_INDEX || index2 == INVALID_INDEX)
512 return removeEdge (index1, index2);
517 std::map<const State*, unsigned int>::const_iterator it = stateIndexMap_.find(st);
518 if (it != stateIndexMap_.end())
520 getVertex(it->second).setTag(tag);
529 std::map<const State*, unsigned int>::const_iterator it = stateIndexMap_.find(st);
530 if (it != stateIndexMap_.end())
532 if (!isStartVertex(it->second))
534 startVertexIndices_.push_back(it->second);
536 std::sort(startVertexIndices_.begin(), startVertexIndices_.end());
546 std::map<const State*, unsigned int>::const_iterator it = stateIndexMap_.find(st);
547 if (it != stateIndexMap_.end())
549 if (!isGoalVertex(it->second))
551 goalVertexIndices_.push_back(it->second);
553 std::sort(startVertexIndices_.begin(), startVertexIndices_.end());
565 func = boost::bind(&ompl::base::PlannerData::defaultEdgeWeight,
this, _1, _2, _3);
567 unsigned int nv = numVertices();
568 for (
unsigned int i = 0; i < nv; ++i)
570 std::map<unsigned int, const PlannerDataEdge*> nbrs;
573 std::map<unsigned int, const PlannerDataEdge*>::const_iterator it;
574 for (it = nbrs.begin(); it != nbrs.end(); ++it)
575 setEdgeWeight(i, it->first, func(getVertex(i), getVertex(it->first), *it->second));
581 std::vector<ompl::base::PlannerData::Graph::Vertex> pred(numVertices());
584 boost::prim_minimum_spanning_tree(*graph_, &pred[0], boost::weight_map(
get(boost::edge_weight, *graph_)).
585 vertex_index_map(
get(boost::vertex_index, *graph_)).
586 root_vertex(boost::vertex(v, *graph_)));
589 for (std::size_t i = 0; i < pred.size(); ++i)
591 if (isStartVertex(i))
593 else if (isGoalVertex(i))
600 for (std::size_t i = 0; i < pred.size(); ++i)
603 mst.
addEdge(pred[i], i, getEdge(pred[i], i), getEdgeWeight(pred[i], i));
615 if (isStartVertex(v))
617 else if (isGoalVertex(v))
622 assert (idx != INVALID_INDEX);
624 std::map<unsigned int, const PlannerDataEdge*> neighbors;
625 getEdges(v, neighbors);
628 std::map<unsigned int, const PlannerDataEdge*>::iterator it;
629 for (it = neighbors.begin(); it != neighbors.end(); ++it)
631 extractReachable(it->first, data);
632 data.
addEdge(idx, data.
vertexIndex(getVertex(it->first)), *(it->second), getEdgeWeight(v, it->first));
658 void ompl::base::PlannerData::freeMemory(
void)
661 for (std::set<State*>::iterator it = decoupledStates_.begin(); it != decoupledStates_.end(); ++it)
666 std::pair<Graph::EIterator, Graph::EIterator> eiterators = boost::edges(*graph_);
667 boost::property_map<Graph::Type, edge_type_t>::type edges =
get(edge_type_t(), *graph_);
668 for (Graph::EIterator iter = eiterators.first; iter != eiterators.second; ++iter)
669 delete boost::get(edges, *iter);
671 std::pair<Graph::VIterator, Graph::VIterator> viterators = boost::vertices(*graph_);
672 boost::property_map<Graph::Type, vertex_type_t>::type vertices =
get(vertex_type_t(), *graph_);
673 for (Graph::VIterator iter = viterators.first; iter != viterators.second; ++iter)
674 delete vertices[*iter];