00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include "ompl/base/PlannerData.h"
00038 #include "ompl/base/PlannerDataGraph.h"
00039
00040 #include <boost/graph/graphviz.hpp>
00041 #include <boost/graph/graphml.hpp>
00042 #include <boost/graph/prim_minimum_spanning_tree.hpp>
00043
00044
00045
00046 #define graph_ reinterpret_cast<ompl::base::PlannerData::Graph*>(graphRaw_)
00047
00048 const ompl::base::PlannerDataEdge ompl::base::PlannerData::NO_EDGE = ompl::base::PlannerDataEdge();
00049 const ompl::base::PlannerDataVertex ompl::base::PlannerData::NO_VERTEX = ompl::base::PlannerDataVertex(0);
00050 const double ompl::base::PlannerData::INVALID_WEIGHT = std::numeric_limits<double>::infinity();
00051 const unsigned int ompl::base::PlannerData::INVALID_INDEX = std::numeric_limits<unsigned int>::max();
00052
00053 ompl::base::PlannerData::PlannerData (const SpaceInformationPtr &si) : si_(si)
00054 {
00055 graphRaw_ = new Graph();
00056 }
00057
00058 ompl::base::PlannerData::~PlannerData (void)
00059 {
00060 freeMemory();
00061
00062 if (graph_)
00063 {
00064 delete graph_;
00065 graphRaw_ = NULL;
00066 }
00067 }
00068
00069 void ompl::base::PlannerData::clear (void)
00070 {
00071 freeMemory();
00072 decoupledStates_.clear();
00073 }
00074
00075 void ompl::base::PlannerData::decoupleFromPlanner (void)
00076 {
00077 unsigned int count = 0;
00078 for (unsigned int i = 0; i < numVertices(); ++i)
00079 {
00080 PlannerDataVertex& vtx = getVertex(i);
00081
00082 if (decoupledStates_.find(const_cast<State*>(vtx.getState())) == decoupledStates_.end())
00083 {
00084 const State* oldState = vtx.getState();
00085 State* clone = si_->cloneState(oldState);
00086 decoupledStates_.insert(clone);
00087
00088 vtx.state_ = clone;
00089
00090
00091 stateIndexMap_.erase(oldState);
00092
00093 stateIndexMap_[clone] = i;
00094 count++;
00095 }
00096 }
00097 }
00098
00099 unsigned int ompl::base::PlannerData::getEdges (unsigned int v, std::vector<unsigned int>& edgeList) const
00100 {
00101 std::pair<Graph::AdjIterator, Graph::AdjIterator> iterators = boost::adjacent_vertices(boost::vertex(v, *graph_), *graph_);
00102
00103 edgeList.clear();
00104 boost::property_map<Graph::Type, boost::vertex_index_t>::type vertices = get(boost::vertex_index, *graph_);
00105 for (Graph::AdjIterator iter = iterators.first; iter != iterators.second; ++iter)
00106 edgeList.push_back(vertices[*iter]);
00107
00108 return edgeList.size();
00109 }
00110
00111 unsigned int ompl::base::PlannerData::getEdges (unsigned int v, std::map<unsigned int, const PlannerDataEdge*>& edgeMap) const
00112 {
00113 std::pair<Graph::OEIterator, Graph::OEIterator> iterators = boost::out_edges(boost::vertex(v, *graph_), *graph_);
00114
00115 edgeMap.clear();
00116 boost::property_map<Graph::Type, edge_type_t>::type edges = get(edge_type_t(), *graph_);
00117 boost::property_map<Graph::Type, boost::vertex_index_t>::type vertices = get(boost::vertex_index, *graph_);
00118 for (Graph::OEIterator iter = iterators.first; iter != iterators.second; ++iter)
00119 edgeMap[vertices[boost::target(*iter, *graph_)]] = boost::get(edges, *iter);
00120
00121 return edgeMap.size();
00122 }
00123
00124 unsigned int ompl::base::PlannerData::getIncomingEdges (unsigned int v, std::vector<unsigned int>& edgeList) const
00125 {
00126 std::pair<Graph::IEIterator, Graph::IEIterator> iterators = boost::in_edges(boost::vertex(v, *graph_), *graph_);
00127
00128 edgeList.clear();
00129 boost::property_map<Graph::Type, boost::vertex_index_t>::type vertices = get(boost::vertex_index, *graph_);
00130 for (Graph::IEIterator iter = iterators.first; iter != iterators.second; ++iter)
00131 edgeList.push_back(vertices[boost::source(*iter, *graph_)]);
00132
00133 return edgeList.size();
00134 }
00135
00136 unsigned int ompl::base::PlannerData::getIncomingEdges (unsigned int v, std::map<unsigned int, const PlannerDataEdge*> &edgeMap) const
00137 {
00138 std::pair<Graph::IEIterator, Graph::IEIterator> iterators = boost::in_edges(boost::vertex(v, *graph_), *graph_);
00139
00140 edgeMap.clear();
00141 boost::property_map<Graph::Type, edge_type_t>::type edges = get(edge_type_t(), *graph_);
00142 boost::property_map<Graph::Type, boost::vertex_index_t>::type vertices = get(boost::vertex_index, *graph_);
00143 for (Graph::IEIterator iter = iterators.first; iter != iterators.second; ++iter)
00144 edgeMap[vertices[boost::source(*iter, *graph_)]] = boost::get(edges, *iter);
00145
00146 return edgeMap.size();
00147 }
00148
00149 double ompl::base::PlannerData::getEdgeWeight(unsigned int v1, unsigned int v2) const
00150 {
00151 Graph::Edge e;
00152 bool exists;
00153 boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
00154
00155 if (exists)
00156 {
00157 boost::property_map<Graph::Type, boost::edge_weight_t>::type edges = get(boost::edge_weight, *graph_);
00158 return edges[e];
00159 }
00160
00161 return INVALID_WEIGHT;
00162 }
00163
00164 bool ompl::base::PlannerData::setEdgeWeight(unsigned int v1, unsigned int v2, double weight)
00165 {
00166 Graph::Edge e;
00167 bool exists;
00168 boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
00169
00170 if (exists)
00171 {
00172 boost::property_map<Graph::Type, boost::edge_weight_t>::type edges = get(boost::edge_weight, *graph_);
00173 edges[e] = weight;
00174 }
00175
00176 return exists;
00177 }
00178
00179 bool ompl::base::PlannerData::edgeExists (unsigned int v1, unsigned int v2) const
00180 {
00181 Graph::Edge e;
00182 bool exists;
00183
00184 boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
00185 return exists;
00186 }
00187
00188 bool ompl::base::PlannerData::vertexExists (const PlannerDataVertex &v) const
00189 {
00190 return vertexIndex(v) != INVALID_INDEX;
00191 }
00192
00193 unsigned int ompl::base::PlannerData::numVertices (void) const
00194 {
00195 return boost::num_vertices(*graph_);
00196 }
00197
00198 unsigned int ompl::base::PlannerData::numEdges (void) const
00199 {
00200 return boost::num_edges(*graph_);
00201 }
00202
00203 const ompl::base::PlannerDataVertex& ompl::base::PlannerData::getVertex (unsigned int index) const
00204 {
00205 if (index >= boost::num_vertices(*graph_))
00206 return NO_VERTEX;
00207
00208 boost::property_map<Graph::Type, vertex_type_t>::type vertices = get(vertex_type_t(), *graph_);
00209 return *(vertices[boost::vertex(index, *graph_)]);
00210 }
00211
00212 ompl::base::PlannerDataVertex& ompl::base::PlannerData::getVertex (unsigned int index)
00213 {
00214 if (index >= boost::num_vertices(*graph_))
00215 return const_cast<ompl::base::PlannerDataVertex&>(NO_VERTEX);
00216
00217 boost::property_map<Graph::Type, vertex_type_t>::type vertices = get(vertex_type_t(), *graph_);
00218 return *(vertices[boost::vertex(index, *graph_)]);
00219 }
00220
00221 const ompl::base::PlannerDataEdge& ompl::base::PlannerData::getEdge (unsigned int v1, unsigned int v2) const
00222 {
00223 Graph::Edge e;
00224 bool exists;
00225 boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
00226
00227 if (exists)
00228 {
00229 boost::property_map<Graph::Type, edge_type_t>::type edges = get(edge_type_t(), *graph_);
00230 return *(boost::get(edges, e));
00231 }
00232
00233 return NO_EDGE;
00234 }
00235
00236 ompl::base::PlannerDataEdge& ompl::base::PlannerData::getEdge (unsigned int v1, unsigned int v2)
00237 {
00238 Graph::Edge e;
00239 bool exists;
00240 boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
00241
00242 if (exists)
00243 {
00244 boost::property_map<Graph::Type, edge_type_t>::type edges = get(edge_type_t(), *graph_);
00245 return *(boost::get(edges, e));
00246 }
00247
00248 return const_cast<ompl::base::PlannerDataEdge&>(NO_EDGE);
00249 }
00250
00251 void ompl::base::PlannerData::printGraphviz (std::ostream& out) const
00252 {
00253 boost::write_graphviz(out, *graph_);
00254 }
00255
00256 void ompl::base::PlannerData::printGraphML (std::ostream& out) const
00257 {
00258
00259 boost::dynamic_properties dp;
00260 dp.property("weight", get(boost::edge_weight_t(), *graph_));
00261
00262 boost::write_graphml(out, *graph_, dp);
00263 }
00264
00265 unsigned int ompl::base::PlannerData::vertexIndex (const PlannerDataVertex &v) const
00266 {
00267 std::map<const State*, unsigned int>::const_iterator it = stateIndexMap_.find(v.getState());
00268 if (it != stateIndexMap_.end())
00269 return it->second;
00270 return INVALID_INDEX;
00271 }
00272
00273 unsigned int ompl::base::PlannerData::numStartVertices (void) const
00274 {
00275 return startVertexIndices_.size();
00276 }
00277
00278 unsigned int ompl::base::PlannerData::numGoalVertices (void) const
00279 {
00280 return goalVertexIndices_.size();
00281 }
00282
00283 unsigned int ompl::base::PlannerData::getStartIndex (unsigned int i) const
00284 {
00285 if (i >= startVertexIndices_.size())
00286 return INVALID_INDEX;
00287
00288 return startVertexIndices_[i];
00289 }
00290
00291 unsigned int ompl::base::PlannerData::getGoalIndex (unsigned int i) const
00292 {
00293 if (i >= goalVertexIndices_.size())
00294 return INVALID_INDEX;
00295
00296 return goalVertexIndices_[i];
00297 }
00298
00299 bool ompl::base::PlannerData::isStartVertex (unsigned int index) const
00300 {
00301 return std::binary_search(startVertexIndices_.begin(), startVertexIndices_.end(), index);
00302 }
00303
00304 bool ompl::base::PlannerData::isGoalVertex (unsigned int index) const
00305 {
00306 return std::binary_search(goalVertexIndices_.begin(), goalVertexIndices_.end(), index);
00307 }
00308
00309 const ompl::base::PlannerDataVertex& ompl::base::PlannerData::getStartVertex (unsigned int i) const
00310 {
00311 if (i >= startVertexIndices_.size())
00312 return NO_VERTEX;
00313
00314 return getVertex(startVertexIndices_[i]);
00315 }
00316
00317 ompl::base::PlannerDataVertex& ompl::base::PlannerData::getStartVertex (unsigned int i)
00318 {
00319 if (i >= startVertexIndices_.size())
00320 return const_cast<ompl::base::PlannerDataVertex&>(NO_VERTEX);
00321
00322 return getVertex(startVertexIndices_[i]);
00323 }
00324
00325 const ompl::base::PlannerDataVertex& ompl::base::PlannerData::getGoalVertex (unsigned int i) const
00326 {
00327 if (i >= goalVertexIndices_.size())
00328 return NO_VERTEX;
00329
00330 return getVertex(goalVertexIndices_[i]);
00331 }
00332
00333 ompl::base::PlannerDataVertex& ompl::base::PlannerData::getGoalVertex (unsigned int i)
00334 {
00335 if (i >= goalVertexIndices_.size())
00336 return const_cast<ompl::base::PlannerDataVertex&>(NO_VERTEX);
00337
00338 return getVertex(goalVertexIndices_[i]);
00339 }
00340
00341 unsigned int ompl::base::PlannerData::addVertex (const PlannerDataVertex &st)
00342 {
00343
00344 if (st.getState() == NULL)
00345 return INVALID_INDEX;
00346
00347 unsigned int index = vertexIndex(st);
00348 if (index == INVALID_INDEX)
00349 {
00350
00351 ompl::base::PlannerDataVertex *clone = st.clone();
00352 Graph::Vertex v = boost::add_vertex(clone, *graph_);
00353 boost::property_map<Graph::Type, boost::vertex_index_t>::type vertexIndexMap = get(boost::vertex_index, *graph_);
00354
00355
00356 stateIndexMap_[clone->getState()] = numVertices()-1;
00357 return vertexIndexMap[v];
00358 }
00359 return index;
00360 }
00361
00362 unsigned int ompl::base::PlannerData::addStartVertex (const PlannerDataVertex &v)
00363 {
00364 unsigned int index = addVertex(v);
00365 if (index != INVALID_INDEX)
00366 markStartState(v.getState());
00367
00368 return index;
00369 }
00370
00371 unsigned int ompl::base::PlannerData::addGoalVertex (const PlannerDataVertex &v)
00372 {
00373 unsigned int index = addVertex(v);
00374
00375 if (index != INVALID_INDEX)
00376 markGoalState(v.getState());
00377
00378 return index;
00379 }
00380
00381 bool ompl::base::PlannerData::addEdge(unsigned int v1, unsigned int v2, const PlannerDataEdge &edge, double weight)
00382 {
00383
00384 if (v1 >= numVertices() || v2 >= numVertices())
00385 return false;
00386
00387
00388 if (edgeExists (v1, v2))
00389 return false;
00390
00391
00392 ompl::base::PlannerDataEdge *clone = edge.clone();
00393 const Graph::edge_property_type properties(clone, weight);
00394
00395 Graph::Edge e;
00396 bool added = false;
00397 tie(e, added) = boost::add_edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), properties, *graph_);
00398
00399 if (!added)
00400 delete clone;
00401
00402 return added;
00403 }
00404
00405 bool ompl::base::PlannerData::addEdge (const PlannerDataVertex & v1, const PlannerDataVertex & v2, const PlannerDataEdge &edge, double weight)
00406 {
00407 unsigned int index1 = addVertex(v1);
00408 unsigned int index2 = addVertex(v2);
00409
00410
00411 if (index1 == INVALID_INDEX && index2 == INVALID_INDEX)
00412 return false;
00413
00414
00415 if (index1 != INVALID_INDEX && index2 != INVALID_INDEX)
00416 return addEdge (index1, index2, edge, weight);
00417
00418 return true;
00419 }
00420
00421 bool ompl::base::PlannerData::removeVertex (const PlannerDataVertex &st)
00422 {
00423 unsigned int index = vertexIndex (st);
00424 if (index != INVALID_INDEX)
00425 return removeVertex (index);
00426 return false;
00427 }
00428
00429 bool ompl::base::PlannerData::removeVertex (unsigned int vIndex)
00430 {
00431 if (vIndex >= boost::num_vertices(*graph_))
00432 return false;
00433
00434
00435 boost::property_map<Graph::Type, edge_type_t>::type edgePropertyMap = get(edge_type_t(), *graph_);
00436
00437
00438 std::pair<Graph::OEIterator, Graph::OEIterator> oiterators = boost::out_edges(boost::vertex(vIndex, *graph_), *graph_);
00439 for (Graph::OEIterator iter = oiterators.first; iter != oiterators.second; ++iter)
00440 delete edgePropertyMap[*iter];
00441
00442
00443 std::pair<Graph::IEIterator, Graph::IEIterator> initerators = boost::in_edges(boost::vertex(vIndex, *graph_), *graph_);
00444 for (Graph::IEIterator iter = initerators.first; iter != initerators.second; ++iter)
00445 delete edgePropertyMap[*iter];
00446
00447
00448 stateIndexMap_.erase(getVertex(vIndex).getState());
00449 boost::property_map<Graph::Type, vertex_type_t>::type vertices = get(vertex_type_t(), *graph_);
00450 for (unsigned int i = vIndex+1; i < boost::num_vertices(*graph_); ++i)
00451 stateIndexMap_[vertices[boost::vertex(i, *graph_)]->getState()]--;
00452
00453
00454 std::vector<unsigned int>::iterator it = std::find(startVertexIndices_.begin(), startVertexIndices_.end(), vIndex);
00455 if (it != startVertexIndices_.end())
00456 startVertexIndices_.erase(it);
00457 for (size_t i = 0; i < startVertexIndices_.size(); ++i)
00458 if (startVertexIndices_[i] > vIndex)
00459 startVertexIndices_[i]--;
00460
00461 it = std::find(goalVertexIndices_.begin(), goalVertexIndices_.end(), vIndex);
00462 if (it != goalVertexIndices_.end())
00463 goalVertexIndices_.erase(it);
00464 for (size_t i = 0; i < goalVertexIndices_.size(); ++i)
00465 if (goalVertexIndices_[i] > vIndex)
00466 goalVertexIndices_[i]--;
00467
00468
00469 State* vtxState = const_cast<State*>(getVertex(vIndex).getState());
00470 if (decoupledStates_.find(vtxState) != decoupledStates_.end())
00471 {
00472 decoupledStates_.erase(vtxState);
00473 si_->freeState(vtxState);
00474 vtxState = NULL;
00475 }
00476
00477
00478 boost::clear_vertex(boost::vertex(vIndex, *graph_), *graph_);
00479 boost::property_map<Graph::Type, vertex_type_t>::type vertexTypeMap = get(vertex_type_t(), *graph_);
00480 delete vertexTypeMap[boost::vertex(vIndex, *graph_)];
00481 boost::remove_vertex(boost::vertex(vIndex, *graph_), *graph_);
00482
00483 return true;
00484 }
00485
00486 bool ompl::base::PlannerData::removeEdge (unsigned int v1, unsigned int v2)
00487 {
00488 Graph::Edge e;
00489 bool exists;
00490 boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
00491
00492 if (!exists)
00493 return false;
00494
00495
00496 boost::property_map<Graph::Type, edge_type_t>::type edges = get(edge_type_t(), *graph_);
00497 delete edges[e];
00498
00499 boost::remove_edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
00500 return true;
00501 }
00502
00503 bool ompl::base::PlannerData::removeEdge (const PlannerDataVertex &v1, const PlannerDataVertex &v2)
00504 {
00505 unsigned int index1, index2;
00506 index1 = vertexIndex(v1);
00507 index2 = vertexIndex(v2);
00508
00509 if (index1 == INVALID_INDEX || index2 == INVALID_INDEX)
00510 return false;
00511
00512 return removeEdge (index1, index2);
00513 }
00514
00515 bool ompl::base::PlannerData::tagState (const base::State* st, int tag)
00516 {
00517 std::map<const State*, unsigned int>::const_iterator it = stateIndexMap_.find(st);
00518 if (it != stateIndexMap_.end())
00519 {
00520 getVertex(it->second).setTag(tag);
00521 return true;
00522 }
00523 return false;
00524 }
00525
00526 bool ompl::base::PlannerData::markStartState (const base::State* st)
00527 {
00528
00529 std::map<const State*, unsigned int>::const_iterator it = stateIndexMap_.find(st);
00530 if (it != stateIndexMap_.end())
00531 {
00532 if (!isStartVertex(it->second))
00533 {
00534 startVertexIndices_.push_back(it->second);
00535
00536 std::sort(startVertexIndices_.begin(), startVertexIndices_.end());
00537 }
00538 return true;
00539 }
00540 return false;
00541 }
00542
00543 bool ompl::base::PlannerData::markGoalState (const base::State* st)
00544 {
00545
00546 std::map<const State*, unsigned int>::const_iterator it = stateIndexMap_.find(st);
00547 if (it != stateIndexMap_.end())
00548 {
00549 if (!isGoalVertex(it->second))
00550 {
00551 goalVertexIndices_.push_back(it->second);
00552
00553 std::sort(startVertexIndices_.begin(), startVertexIndices_.end());
00554 }
00555 return true;
00556 }
00557 return false;
00558 }
00559
00560 void ompl::base::PlannerData::computeEdgeWeights(const ompl::base::PlannerData::EdgeWeightFn& f)
00561 {
00562
00563 ompl::base::PlannerData::EdgeWeightFn func = f;
00564 if (!func)
00565 func = boost::bind(&ompl::base::PlannerData::defaultEdgeWeight, this, _1, _2, _3);
00566
00567 unsigned int nv = numVertices();
00568 for (unsigned int i = 0; i < nv; ++i)
00569 {
00570 std::map<unsigned int, const PlannerDataEdge*> nbrs;
00571 getEdges(i, nbrs);
00572
00573 std::map<unsigned int, const PlannerDataEdge*>::const_iterator it;
00574 for (it = nbrs.begin(); it != nbrs.end(); ++it)
00575 setEdgeWeight(i, it->first, func(getVertex(i), getVertex(it->first), *it->second));
00576 }
00577 }
00578
00579 void ompl::base::PlannerData::extractMinimumSpanningTree (unsigned int v, base::PlannerData &mst) const
00580 {
00581 std::vector<ompl::base::PlannerData::Graph::Vertex> pred(numVertices());
00582
00583
00584 boost::prim_minimum_spanning_tree(*graph_, &pred[0], boost::weight_map(get(boost::edge_weight, *graph_)).
00585 vertex_index_map(get(boost::vertex_index, *graph_)).
00586 root_vertex(boost::vertex(v, *graph_)));
00587
00588
00589 for (std::size_t i = 0; i < pred.size(); ++i)
00590 {
00591 if (isStartVertex(i))
00592 mst.addStartVertex(getVertex(i));
00593 else if (isGoalVertex(i))
00594 mst.addGoalVertex(getVertex(i));
00595 else
00596 mst.addVertex(getVertex(i));
00597 }
00598
00599
00600 for (std::size_t i = 0; i < pred.size(); ++i)
00601 {
00602 if (pred[i] != i)
00603 mst.addEdge(pred[i], i, getEdge(pred[i], i), getEdgeWeight(pred[i], i));
00604 }
00605 }
00606
00607 void ompl::base::PlannerData::extractReachable(unsigned int v, base::PlannerData& data) const
00608 {
00609
00610 if (data.vertexExists(getVertex(v)))
00611 return;
00612
00613
00614 unsigned int idx;
00615 if (isStartVertex(v))
00616 idx = data.addStartVertex(getVertex(v));
00617 else if (isGoalVertex(v))
00618 idx = data.addGoalVertex(getVertex(v));
00619 else
00620 idx = data.addVertex(getVertex(v));
00621
00622 assert (idx != INVALID_INDEX);
00623
00624 std::map<unsigned int, const PlannerDataEdge*> neighbors;
00625 getEdges(v, neighbors);
00626
00627
00628 std::map<unsigned int, const PlannerDataEdge*>::iterator it;
00629 for (it = neighbors.begin(); it != neighbors.end(); ++it)
00630 {
00631 extractReachable(it->first, data);
00632 data.addEdge(idx, data.vertexIndex(getVertex(it->first)), *(it->second), getEdgeWeight(v, it->first));
00633 }
00634 }
00635
00636 ompl::base::PlannerData::Graph& ompl::base::PlannerData::toBoostGraph(void)
00637 {
00638 ompl::base::PlannerData::Graph* boostgraph = reinterpret_cast<ompl::base::PlannerData::Graph*>(graphRaw_);
00639 return *boostgraph;
00640 }
00641
00642 const ompl::base::PlannerData::Graph& ompl::base::PlannerData::toBoostGraph(void) const
00643 {
00644 const ompl::base::PlannerData::Graph* boostgraph = reinterpret_cast<const ompl::base::PlannerData::Graph*>(graphRaw_);
00645 return *boostgraph;
00646 }
00647
00648 double ompl::base::PlannerData::defaultEdgeWeight(const base::PlannerDataVertex &v1, const base::PlannerDataVertex &v2, const base::PlannerDataEdge& ) const
00649 {
00650 return si_->distance(v1.getState(), v2.getState());
00651 }
00652
00653 const ompl::base::SpaceInformationPtr& ompl::base::PlannerData::getSpaceInformation(void) const
00654 {
00655 return si_;
00656 }
00657
00658 void ompl::base::PlannerData::freeMemory(void)
00659 {
00660
00661 for (std::set<State*>::iterator it = decoupledStates_.begin(); it != decoupledStates_.end(); ++it)
00662 si_->freeState(*it);
00663
00664 if (graph_)
00665 {
00666 std::pair<Graph::EIterator, Graph::EIterator> eiterators = boost::edges(*graph_);
00667 boost::property_map<Graph::Type, edge_type_t>::type edges = get(edge_type_t(), *graph_);
00668 for (Graph::EIterator iter = eiterators.first; iter != eiterators.second; ++iter)
00669 delete boost::get(edges, *iter);
00670
00671 std::pair<Graph::VIterator, Graph::VIterator> viterators = boost::vertices(*graph_);
00672 boost::property_map<Graph::Type, vertex_type_t>::type vertices = get(vertex_type_t(), *graph_);
00673 for (Graph::VIterator iter = viterators.first; iter != viterators.second; ++iter)
00674 delete vertices[*iter];
00675
00676 graph_->clear();
00677 }
00678 }
00679
00680 bool ompl::base::PlannerData::hasControls(void) const
00681 {
00682 return false;
00683 }