All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
src/ompl/base/src/PlannerData.cpp
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2011, Rice University
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Rice University nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 /* Author: Ryan Luna */
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 // This is a convenient macro to cast the void* graph pointer as the
00045 // Boost.Graph structure from PlannerDataGraph.h
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         // If this vertex's state is not in the decoupled list, clone it and add it
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             // Replacing the shallow state pointer with our shiny new clone
00088             vtx.state_ = clone;
00089 
00090             // Remove oldState from stateIndexMap
00091             stateIndexMap_.erase(oldState);
00092             // Add the new, cloned state to stateIndexMap
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     // Not writing vertex or edge structures.
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     // Do not add vertices with null states
00344     if (st.getState() == NULL)
00345         return INVALID_INDEX;
00346 
00347     unsigned int index = vertexIndex(st);
00348     if (index == INVALID_INDEX) // Vertex does not already exist
00349     {
00350         // Clone the state to prevent object slicing when retrieving this object
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         // Insert this entry into the stateIndexMap_ for fast lookup
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     // If either of the vertices do not exist, don't add an edge
00384     if (v1 >= numVertices() || v2 >= numVertices())
00385         return false;
00386 
00387      // If an edge already exists, do not add one
00388      if (edgeExists (v1, v2))
00389         return false;
00390 
00391     // Clone the edge to prevent object slicing
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     // If neither vertex was added or already exists, return false
00411     if (index1 == INVALID_INDEX && index2 == INVALID_INDEX)
00412         return false;
00413 
00414     // Only add the edge if both vertices exist
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     // Retrieve a list of all edge structures
00435     boost::property_map<Graph::Type, edge_type_t>::type edgePropertyMap = get(edge_type_t(), *graph_);
00436 
00437     // Freeing memory associated with outgoing edges of this vertex
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     // Freeing memory associated with incoming edges of this vertex
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     // Remove this vertex from stateIndexMap_, and update the map
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     // Remove this vertex from the start and/or goal index list, if it exists.  Update the lists.
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     // If the state attached to this vertex was decoupled, free it here
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     // Slay the vertex
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     // Freeing memory associated with this edge
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     // Find the index in the stateIndexMap_
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             // Sort the indices for quick lookup
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     // Find the index in the stateIndexMap_
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             // Sort the indices for quick lookup
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     // If f wasn't specified, use defaultEdgeWeight
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     // Ask boost nicely for the minimum spanning tree
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     // Adding vertices to MST
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     // Adding edges to MST
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     // If this vertex already exists in data, return
00610     if (data.vertexExists(getVertex(v)))
00611         return;
00612 
00613     // Adding the vertex corresponding to v into data
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     // Depth-first traversal of reachable graph
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& /*e*/) 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     // Freeing decoupled states, if any
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 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines