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 #include "ompl/base/StateStorage.h"
00040 #include "ompl/base/OptimizationObjective.h"
00041 #include "ompl/base/objectives/PathLengthOptimizationObjective.h"
00042 #include "ompl/base/ScopedState.h"
00043 
00044 #include <boost/graph/graphviz.hpp>
00045 #include <boost/graph/graphml.hpp>
00046 #include <boost/graph/dijkstra_shortest_paths.hpp>
00047 #include <boost/version.hpp>
00048 #if BOOST_VERSION < 105100
00049 #warning Boost version >=1.51 is needed for ompl::base::PlannerData::printGraphML
00050 #else
00051 #include <boost/property_map/function_property_map.hpp>
00052 #endif
00053 
00054 // This is a convenient macro to cast the void* graph pointer as the
00055 // Boost.Graph structure from PlannerDataGraph.h
00056 #define graph_ reinterpret_cast<ompl::base::PlannerData::Graph*>(graphRaw_)
00057 
00058 const ompl::base::PlannerDataEdge   ompl::base::PlannerData::NO_EDGE = ompl::base::PlannerDataEdge();
00059 const ompl::base::PlannerDataVertex ompl::base::PlannerData::NO_VERTEX = ompl::base::PlannerDataVertex(0);
00060 const unsigned int ompl::base::PlannerData::INVALID_INDEX = std::numeric_limits<unsigned int>::max();
00061 
00062 ompl::base::PlannerData::PlannerData (const SpaceInformationPtr &si) : si_(si)
00063 {
00064     graphRaw_ = new Graph();
00065 }
00066 
00067 ompl::base::PlannerData::~PlannerData ()
00068 {
00069     freeMemory();
00070 
00071     if (graph_)
00072     {
00073         delete graph_;
00074         graphRaw_ = NULL;
00075     }
00076 }
00077 
00078 void ompl::base::PlannerData::clear ()
00079 {
00080     freeMemory();
00081     decoupledStates_.clear();
00082 }
00083 
00084 void ompl::base::PlannerData::decoupleFromPlanner ()
00085 {
00086     unsigned int count = 0;
00087     for (unsigned int i = 0; i < numVertices(); ++i)
00088     {
00089         PlannerDataVertex &vtx = getVertex(i);
00090         // If this vertex's state is not in the decoupled list, clone it and add it
00091         if (decoupledStates_.find(const_cast<State*>(vtx.getState())) == decoupledStates_.end())
00092         {
00093             const State *oldState = vtx.getState();
00094             State *clone = si_->cloneState(oldState);
00095             decoupledStates_.insert(clone);
00096             // Replacing the shallow state pointer with our shiny new clone
00097             vtx.state_ = clone;
00098 
00099             // Remove oldState from stateIndexMap
00100             stateIndexMap_.erase(oldState);
00101             // Add the new, cloned state to stateIndexMap
00102             stateIndexMap_[clone] = i;
00103             count++;
00104         }
00105     }
00106 }
00107 
00108 unsigned int ompl::base::PlannerData::getEdges (unsigned int v, std::vector<unsigned int>& edgeList) const
00109 {
00110     std::pair<Graph::AdjIterator, Graph::AdjIterator> iterators = boost::adjacent_vertices(boost::vertex(v, *graph_), *graph_);
00111 
00112     edgeList.clear();
00113     boost::property_map<Graph::Type, boost::vertex_index_t>::type vertices = get(boost::vertex_index, *graph_);
00114     for (Graph::AdjIterator iter = iterators.first; iter != iterators.second; ++iter)
00115         edgeList.push_back(vertices[*iter]);
00116 
00117     return edgeList.size();
00118 }
00119 
00120 unsigned int ompl::base::PlannerData::getEdges (unsigned int v, std::map<unsigned int, const PlannerDataEdge*>& edgeMap) const
00121 {
00122     std::pair<Graph::OEIterator, Graph::OEIterator> iterators = boost::out_edges(boost::vertex(v, *graph_), *graph_);
00123 
00124     edgeMap.clear();
00125     boost::property_map<Graph::Type, edge_type_t>::type edges = get(edge_type_t(), *graph_);
00126     boost::property_map<Graph::Type, boost::vertex_index_t>::type vertices = get(boost::vertex_index, *graph_);
00127     for (Graph::OEIterator iter = iterators.first; iter != iterators.second; ++iter)
00128         edgeMap[vertices[boost::target(*iter, *graph_)]] = boost::get(edges, *iter);
00129 
00130     return edgeMap.size();
00131 }
00132 
00133 unsigned int ompl::base::PlannerData::getIncomingEdges (unsigned int v, std::vector<unsigned int>& edgeList) const
00134 {
00135     std::pair<Graph::IEIterator, Graph::IEIterator> iterators = boost::in_edges(boost::vertex(v, *graph_), *graph_);
00136 
00137     edgeList.clear();
00138     boost::property_map<Graph::Type, boost::vertex_index_t>::type vertices = get(boost::vertex_index, *graph_);
00139     for (Graph::IEIterator iter = iterators.first; iter != iterators.second; ++iter)
00140         edgeList.push_back(vertices[boost::source(*iter, *graph_)]);
00141 
00142     return edgeList.size();
00143 }
00144 
00145 unsigned int ompl::base::PlannerData::getIncomingEdges (unsigned int v, std::map<unsigned int, const PlannerDataEdge*> &edgeMap) const
00146 {
00147     std::pair<Graph::IEIterator, Graph::IEIterator> iterators = boost::in_edges(boost::vertex(v, *graph_), *graph_);
00148 
00149     edgeMap.clear();
00150     boost::property_map<Graph::Type, edge_type_t>::type edges = get(edge_type_t(), *graph_);
00151     boost::property_map<Graph::Type, boost::vertex_index_t>::type vertices = get(boost::vertex_index, *graph_);
00152     for (Graph::IEIterator iter = iterators.first; iter != iterators.second; ++iter)
00153         edgeMap[vertices[boost::source(*iter, *graph_)]] = boost::get(edges, *iter);
00154 
00155     return edgeMap.size();
00156 }
00157 
00158 bool ompl::base::PlannerData::getEdgeWeight(unsigned int v1, unsigned int v2, Cost* weight) const
00159 {
00160     Graph::Edge e;
00161     bool exists;
00162     boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
00163 
00164     if (exists)
00165     {
00166         boost::property_map<Graph::Type, boost::edge_weight_t>::type edges = get(boost::edge_weight, *graph_);
00167         *weight = edges[e];
00168         return true;
00169     }
00170 
00171     return false;
00172 }
00173 
00174 bool ompl::base::PlannerData::setEdgeWeight(unsigned int v1, unsigned int v2, Cost weight)
00175 {
00176     Graph::Edge e;
00177     bool exists;
00178     boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
00179 
00180     if (exists)
00181     {
00182         boost::property_map<Graph::Type, boost::edge_weight_t>::type edges = get(boost::edge_weight, *graph_);
00183         edges[e] = weight;
00184     }
00185 
00186     return exists;
00187 }
00188 
00189 bool ompl::base::PlannerData::edgeExists (unsigned int v1, unsigned int v2) const
00190 {
00191     Graph::Edge e;
00192     bool exists;
00193 
00194     boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
00195     return exists;
00196 }
00197 
00198 bool ompl::base::PlannerData::vertexExists (const PlannerDataVertex &v) const
00199 {
00200     return vertexIndex(v) != INVALID_INDEX;
00201 }
00202 
00203 unsigned int ompl::base::PlannerData::numVertices () const
00204 {
00205     return boost::num_vertices(*graph_);
00206 }
00207 
00208 unsigned int ompl::base::PlannerData::numEdges () const
00209 {
00210     return boost::num_edges(*graph_);
00211 }
00212 
00213 const ompl::base::PlannerDataVertex& ompl::base::PlannerData::getVertex (unsigned int index) const
00214 {
00215     if (index >= boost::num_vertices(*graph_))
00216         return NO_VERTEX;
00217 
00218     boost::property_map<Graph::Type, vertex_type_t>::type vertices = get(vertex_type_t(), *graph_);
00219     return *(vertices[boost::vertex(index, *graph_)]);
00220 }
00221 
00222 ompl::base::PlannerDataVertex& ompl::base::PlannerData::getVertex (unsigned int index)
00223 {
00224     if (index >= boost::num_vertices(*graph_))
00225         return const_cast<ompl::base::PlannerDataVertex&>(NO_VERTEX);
00226 
00227     boost::property_map<Graph::Type, vertex_type_t>::type vertices = get(vertex_type_t(), *graph_);
00228     return *(vertices[boost::vertex(index, *graph_)]);
00229 }
00230 
00231 const ompl::base::PlannerDataEdge& ompl::base::PlannerData::getEdge (unsigned int v1, unsigned int v2) const
00232 {
00233     Graph::Edge e;
00234     bool exists;
00235     boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
00236 
00237     if (exists)
00238     {
00239         boost::property_map<Graph::Type, edge_type_t>::type edges = get(edge_type_t(), *graph_);
00240         return *(boost::get(edges, e));
00241     }
00242 
00243     return NO_EDGE;
00244 }
00245 
00246 ompl::base::PlannerDataEdge& ompl::base::PlannerData::getEdge (unsigned int v1, unsigned int v2)
00247 {
00248     Graph::Edge e;
00249     bool exists;
00250     boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
00251 
00252     if (exists)
00253     {
00254         boost::property_map<Graph::Type, edge_type_t>::type edges = get(edge_type_t(), *graph_);
00255         return *(boost::get(edges, e));
00256     }
00257 
00258     return const_cast<ompl::base::PlannerDataEdge&>(NO_EDGE);
00259 }
00260 
00261 void ompl::base::PlannerData::printGraphviz (std::ostream& out) const
00262 {
00263     boost::write_graphviz(out, *graph_);
00264 }
00265 
00266 namespace
00267 {
00268     // Property map for extracting the edge weight of a graph edge as
00269     // a double for printGraphML.
00270     double edgeWeightAsDouble(ompl::base::PlannerData::Graph::Type &g,
00271                               ompl::base::PlannerData::Graph::Edge e)
00272     {
00273         return get(boost::edge_weight_t(), g)[e].value();
00274     }
00275 
00276     // Property map for extracting states as arrays of doubles
00277     std::string vertexCoords (ompl::base::PlannerData::Graph::Type &g,
00278                               ompl::base::ScopedState<>& s,
00279                               ompl::base::PlannerData::Graph::Vertex v)
00280     {
00281         s = *get(vertex_type_t(), g)[v]->getState();
00282         std::vector<double> coords(s.reals());
00283         std::ostringstream sstream;
00284         if (coords.size()>0)
00285         {
00286             sstream << coords[0];
00287             for (std::size_t i = 1; i < coords.size(); ++i)
00288                 sstream << ',' << coords[i];
00289         }
00290         return sstream.str();
00291     }
00292 }
00293 
00294 void ompl::base::PlannerData::printGraphML (std::ostream& out) const
00295 {
00296 #if BOOST_VERSION < 105100
00297     OMPL_WARN("Boost version >=1.51 is needed for ompl::base::PlannerData::printGraphML");
00298 #else
00299     // For some reason, make_function_property_map can't infer its
00300     // template arguments corresponding to edgeWeightAsDouble's type
00301     // signature. So, we have to use this horribly verbose
00302     // instantiation of the property map.
00303     //
00304     // \TODO Can we use make_function_property_map() here and have it
00305     // infer the property template arguments?
00306     boost::function_property_map<
00307         boost::function<double (ompl::base::PlannerData::Graph::Edge)>,
00308         ompl::base::PlannerData::Graph::Edge,
00309         double>
00310         weightmap(boost::bind(&edgeWeightAsDouble, *graph_, _1));
00311     ompl::base::ScopedState<> s(si_);
00312     boost::function_property_map<
00313         boost::function<std::string (ompl::base::PlannerData::Graph::Vertex)>,
00314         ompl::base::PlannerData::Graph::Vertex,
00315         std::string >
00316         coordsmap(boost::bind(&vertexCoords, *graph_, s, _1));
00317 
00318 
00319     // Not writing vertex or edge structures.
00320     boost::dynamic_properties dp;
00321     dp.property("weight", weightmap);
00322     dp.property("coords", coordsmap);
00323 
00324     boost::write_graphml(out, *graph_, dp);
00325 #endif
00326 }
00327 
00328 unsigned int ompl::base::PlannerData::vertexIndex (const PlannerDataVertex &v) const
00329 {
00330     std::map<const State*, unsigned int>::const_iterator it = stateIndexMap_.find(v.getState());
00331     if (it != stateIndexMap_.end())
00332         return it->second;
00333     return INVALID_INDEX;
00334 }
00335 
00336 unsigned int ompl::base::PlannerData::numStartVertices () const
00337 {
00338     return startVertexIndices_.size();
00339 }
00340 
00341 unsigned int ompl::base::PlannerData::numGoalVertices () const
00342 {
00343     return goalVertexIndices_.size();
00344 }
00345 
00346 unsigned int ompl::base::PlannerData::getStartIndex (unsigned int i) const
00347 {
00348     if (i >= startVertexIndices_.size())
00349         return INVALID_INDEX;
00350 
00351     return startVertexIndices_[i];
00352 }
00353 
00354 unsigned int ompl::base::PlannerData::getGoalIndex (unsigned int i) const
00355 {
00356     if (i >= goalVertexIndices_.size())
00357         return INVALID_INDEX;
00358 
00359     return goalVertexIndices_[i];
00360 }
00361 
00362 bool ompl::base::PlannerData::isStartVertex (unsigned int index) const
00363 {
00364     return std::binary_search(startVertexIndices_.begin(), startVertexIndices_.end(), index);
00365 }
00366 
00367 bool ompl::base::PlannerData::isGoalVertex (unsigned int index) const
00368 {
00369     return std::binary_search(goalVertexIndices_.begin(), goalVertexIndices_.end(), index);
00370 }
00371 
00372 const ompl::base::PlannerDataVertex& ompl::base::PlannerData::getStartVertex (unsigned int i) const
00373 {
00374     if (i >= startVertexIndices_.size())
00375         return NO_VERTEX;
00376 
00377     return getVertex(startVertexIndices_[i]);
00378 }
00379 
00380 ompl::base::PlannerDataVertex& ompl::base::PlannerData::getStartVertex (unsigned int i)
00381 {
00382     if (i >= startVertexIndices_.size())
00383         return const_cast<ompl::base::PlannerDataVertex&>(NO_VERTEX);
00384 
00385     return getVertex(startVertexIndices_[i]);
00386 }
00387 
00388 const ompl::base::PlannerDataVertex& ompl::base::PlannerData::getGoalVertex (unsigned int i) const
00389 {
00390     if (i >= goalVertexIndices_.size())
00391         return NO_VERTEX;
00392 
00393     return getVertex(goalVertexIndices_[i]);
00394 }
00395 
00396 ompl::base::PlannerDataVertex& ompl::base::PlannerData::getGoalVertex (unsigned int i)
00397 {
00398     if (i >= goalVertexIndices_.size())
00399         return const_cast<ompl::base::PlannerDataVertex&>(NO_VERTEX);
00400 
00401     return getVertex(goalVertexIndices_[i]);
00402 }
00403 
00404 unsigned int ompl::base::PlannerData::addVertex (const PlannerDataVertex &st)
00405 {
00406     // Do not add vertices with null states
00407     if (st.getState() == NULL)
00408         return INVALID_INDEX;
00409 
00410     unsigned int index = vertexIndex(st);
00411     if (index == INVALID_INDEX) // Vertex does not already exist
00412     {
00413         // Clone the state to prevent object slicing when retrieving this object
00414         ompl::base::PlannerDataVertex *clone = st.clone();
00415         Graph::Vertex v = boost::add_vertex(clone, *graph_);
00416         boost::property_map<Graph::Type, boost::vertex_index_t>::type vertexIndexMap = get(boost::vertex_index, *graph_);
00417 
00418         // Insert this entry into the stateIndexMap_ for fast lookup
00419         stateIndexMap_[clone->getState()] = numVertices()-1;
00420         return vertexIndexMap[v];
00421     }
00422     return index;
00423 }
00424 
00425 unsigned int ompl::base::PlannerData::addStartVertex (const PlannerDataVertex &v)
00426 {
00427     unsigned int index = addVertex(v);
00428     if (index != INVALID_INDEX)
00429         markStartState(v.getState());
00430 
00431     return index;
00432 }
00433 
00434 unsigned int ompl::base::PlannerData::addGoalVertex  (const PlannerDataVertex &v)
00435 {
00436     unsigned int index = addVertex(v);
00437 
00438     if (index != INVALID_INDEX)
00439         markGoalState(v.getState());
00440 
00441     return index;
00442 }
00443 
00444 bool ompl::base::PlannerData::addEdge(unsigned int v1, unsigned int v2, const PlannerDataEdge &edge, Cost weight)
00445 {
00446     // If either of the vertices do not exist, don't add an edge
00447     if (v1 >= numVertices() || v2 >= numVertices())
00448         return false;
00449 
00450      // If an edge already exists, do not add one
00451      if (edgeExists (v1, v2))
00452         return false;
00453 
00454     // Clone the edge to prevent object slicing
00455     ompl::base::PlannerDataEdge *clone = edge.clone();
00456     const Graph::edge_property_type properties(clone, weight);
00457 
00458     Graph::Edge e;
00459     bool added = false;
00460     tie(e, added) = boost::add_edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), properties, *graph_);
00461 
00462     if (!added)
00463         delete clone;
00464 
00465     return added;
00466 }
00467 
00468 bool ompl::base::PlannerData::addEdge (const PlannerDataVertex & v1, const PlannerDataVertex & v2, const PlannerDataEdge &edge, Cost weight)
00469 {
00470     unsigned int index1 = addVertex(v1);
00471     unsigned int index2 = addVertex(v2);
00472 
00473     // If neither vertex was added or already exists, return false
00474     if (index1 == INVALID_INDEX && index2 == INVALID_INDEX)
00475         return false;
00476 
00477     // Only add the edge if both vertices exist
00478     if (index1 != INVALID_INDEX && index2 != INVALID_INDEX)
00479         return addEdge (index1, index2, edge, weight);
00480 
00481     return true;
00482 }
00483 
00484 bool ompl::base::PlannerData::removeVertex (const PlannerDataVertex &st)
00485 {
00486     unsigned int index = vertexIndex (st);
00487     if (index != INVALID_INDEX)
00488         return removeVertex (index);
00489     return false;
00490 }
00491 
00492 bool ompl::base::PlannerData::removeVertex (unsigned int vIndex)
00493 {
00494     if (vIndex >= boost::num_vertices(*graph_))
00495         return false;
00496 
00497     // Retrieve a list of all edge structures
00498     boost::property_map<Graph::Type, edge_type_t>::type edgePropertyMap = get(edge_type_t(), *graph_);
00499 
00500     // Freeing memory associated with outgoing edges of this vertex
00501     std::pair<Graph::OEIterator, Graph::OEIterator> oiterators = boost::out_edges(boost::vertex(vIndex, *graph_), *graph_);
00502     for (Graph::OEIterator iter = oiterators.first; iter != oiterators.second; ++iter)
00503         delete edgePropertyMap[*iter];
00504 
00505     // Freeing memory associated with incoming edges of this vertex
00506     std::pair<Graph::IEIterator, Graph::IEIterator> initerators = boost::in_edges(boost::vertex(vIndex, *graph_), *graph_);
00507     for (Graph::IEIterator iter = initerators.first; iter != initerators.second; ++iter)
00508         delete edgePropertyMap[*iter];
00509 
00510     // Remove this vertex from stateIndexMap_, and update the map
00511     stateIndexMap_.erase(getVertex(vIndex).getState());
00512     boost::property_map<Graph::Type, vertex_type_t>::type vertices = get(vertex_type_t(), *graph_);
00513     for (unsigned int i = vIndex+1; i < boost::num_vertices(*graph_); ++i)
00514          stateIndexMap_[vertices[boost::vertex(i, *graph_)]->getState()]--;
00515 
00516     // Remove this vertex from the start and/or goal index list, if it exists.  Update the lists.
00517     std::vector<unsigned int>::iterator it = std::find(startVertexIndices_.begin(), startVertexIndices_.end(), vIndex);
00518     if (it != startVertexIndices_.end())
00519         startVertexIndices_.erase(it);
00520     for (size_t i = 0; i < startVertexIndices_.size(); ++i)
00521         if (startVertexIndices_[i] > vIndex)
00522             startVertexIndices_[i]--;
00523 
00524     it = std::find(goalVertexIndices_.begin(), goalVertexIndices_.end(), vIndex);
00525     if (it != goalVertexIndices_.end())
00526         goalVertexIndices_.erase(it);
00527     for (size_t i = 0; i < goalVertexIndices_.size(); ++i)
00528         if (goalVertexIndices_[i] > vIndex)
00529             goalVertexIndices_[i]--;
00530 
00531     // If the state attached to this vertex was decoupled, free it here
00532     State *vtxState = const_cast<State*>(getVertex(vIndex).getState());
00533     if (decoupledStates_.find(vtxState) != decoupledStates_.end())
00534     {
00535         decoupledStates_.erase(vtxState);
00536         si_->freeState(vtxState);
00537         vtxState = NULL;
00538     }
00539 
00540     // Slay the vertex
00541     boost::clear_vertex(boost::vertex(vIndex, *graph_), *graph_);
00542     boost::property_map<Graph::Type, vertex_type_t>::type vertexTypeMap = get(vertex_type_t(), *graph_);
00543     delete vertexTypeMap[boost::vertex(vIndex, *graph_)];
00544     boost::remove_vertex(boost::vertex(vIndex, *graph_), *graph_);
00545 
00546     return true;
00547 }
00548 
00549 bool ompl::base::PlannerData::removeEdge (unsigned int v1, unsigned int v2)
00550 {
00551     Graph::Edge e;
00552     bool exists;
00553     boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
00554 
00555     if (!exists)
00556         return false;
00557 
00558     // Freeing memory associated with this edge
00559     boost::property_map<Graph::Type, edge_type_t>::type edges = get(edge_type_t(), *graph_);
00560     delete edges[e];
00561 
00562     boost::remove_edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
00563     return true;
00564 }
00565 
00566 bool ompl::base::PlannerData::removeEdge (const PlannerDataVertex &v1, const PlannerDataVertex &v2)
00567 {
00568     unsigned int index1, index2;
00569     index1 = vertexIndex(v1);
00570     index2 = vertexIndex(v2);
00571 
00572     if (index1 == INVALID_INDEX || index2 == INVALID_INDEX)
00573         return false;
00574 
00575     return removeEdge (index1, index2);
00576 }
00577 
00578 bool ompl::base::PlannerData::tagState (const base::State *st, int tag)
00579 {
00580     std::map<const State*, unsigned int>::const_iterator it = stateIndexMap_.find(st);
00581     if (it != stateIndexMap_.end())
00582     {
00583         getVertex(it->second).setTag(tag);
00584         return true;
00585     }
00586     return false;
00587 }
00588 
00589 bool ompl::base::PlannerData::markStartState (const base::State *st)
00590 {
00591     // Find the index in the stateIndexMap_
00592     std::map<const State*, unsigned int>::const_iterator it = stateIndexMap_.find(st);
00593     if (it != stateIndexMap_.end())
00594     {
00595         if (!isStartVertex(it->second))
00596         {
00597             startVertexIndices_.push_back(it->second);
00598             // Sort the indices for quick lookup
00599             std::sort(startVertexIndices_.begin(), startVertexIndices_.end());
00600         }
00601         return true;
00602     }
00603     return false;
00604 }
00605 
00606 bool ompl::base::PlannerData::markGoalState (const base::State *st)
00607 {
00608     // Find the index in the stateIndexMap_
00609     std::map<const State*, unsigned int>::const_iterator it = stateIndexMap_.find(st);
00610     if (it != stateIndexMap_.end())
00611     {
00612         if (!isGoalVertex(it->second))
00613         {
00614             goalVertexIndices_.push_back(it->second);
00615             // Sort the indices for quick lookup
00616             std::sort(startVertexIndices_.begin(), startVertexIndices_.end());
00617         }
00618         return true;
00619     }
00620     return false;
00621 }
00622 
00623 void ompl::base::PlannerData::computeEdgeWeights (const OptimizationObjective &opt)
00624 {
00625     unsigned int nv = numVertices();
00626     for (unsigned int i = 0; i < nv; ++i)
00627     {
00628         std::map<unsigned int, const PlannerDataEdge*> nbrs;
00629         getEdges(i, nbrs);
00630 
00631         std::map<unsigned int, const PlannerDataEdge*>::const_iterator it;
00632         for (it = nbrs.begin(); it != nbrs.end(); ++it)
00633         {
00634             setEdgeWeight(i, it->first, opt.motionCost(getVertex(i).getState(),
00635                                                        getVertex(it->first).getState()));
00636         }
00637     }
00638 }
00639 
00640 void ompl::base::PlannerData::computeEdgeWeights ()
00641 {
00642     // Create a PathLengthOptimizationObjective to compute the edge
00643     // weights according to state space distance
00644     ompl::base::PathLengthOptimizationObjective opt(si_);
00645     computeEdgeWeights(opt);
00646 }
00647 
00648 namespace
00649 {
00650     // Used in minimum spanning tree
00651     ompl::base::Cost project2nd (ompl::base::Cost /*unused*/, ompl::base::Cost second)
00652     {
00653         return second;
00654     }
00655 }
00656 
00657 void ompl::base::PlannerData::extractMinimumSpanningTree (unsigned int v,
00658                                                           const base::OptimizationObjective &opt,
00659                                                           base::PlannerData &mst) const
00660 {
00661     std::vector<ompl::base::PlannerData::Graph::Vertex> pred(numVertices());
00662 
00663     // This is how boost's minimum spanning tree is actually
00664     // implemented, except it lacks the generality for specifying our
00665     // own comparison function or zero/inf values.
00666     //
00667     // \TODO Once (https://svn.boost.org/trac/boost/ticket/9368) gets
00668     // into boost we can use the far more direct
00669     // boost::prim_minimum_spanning_tree().
00670     boost::dijkstra_shortest_paths
00671         (*graph_, v,
00672          boost::predecessor_map(&pred[0]).
00673          distance_compare(boost::bind(&base::OptimizationObjective::
00674                                       isCostBetterThan, &opt, _1, _2)).
00675          distance_combine(&project2nd).
00676          distance_inf(opt.infiniteCost()).
00677          distance_zero(opt.identityCost()));
00678 
00679     // Adding vertices to MST
00680     for (std::size_t i = 0; i < pred.size(); ++i)
00681     {
00682         if (isStartVertex(i))
00683             mst.addStartVertex(getVertex(i));
00684         else if (isGoalVertex(i))
00685             mst.addGoalVertex(getVertex(i));
00686         else
00687             mst.addVertex(getVertex(i));
00688     }
00689 
00690     // Adding edges to MST
00691     for (std::size_t i = 0; i < pred.size(); ++i)
00692     {
00693         if (pred[i] != i)
00694         {
00695             Cost c;
00696             getEdgeWeight(pred[i], i, &c);
00697             mst.addEdge(pred[i], i, getEdge(pred[i], i), c);
00698         }
00699     }
00700 }
00701 
00702 void ompl::base::PlannerData::extractReachable(unsigned int v, base::PlannerData &data) const
00703 {
00704     // If this vertex already exists in data, return
00705     if (data.vertexExists(getVertex(v)))
00706         return;
00707 
00708     // Adding the vertex corresponding to v into data
00709     unsigned int idx;
00710     if (isStartVertex(v))
00711         idx = data.addStartVertex(getVertex(v));
00712     else if (isGoalVertex(v))
00713         idx = data.addGoalVertex(getVertex(v));
00714     else
00715         idx = data.addVertex(getVertex(v));
00716 
00717     assert (idx != INVALID_INDEX);
00718 
00719     std::map<unsigned int, const PlannerDataEdge*> neighbors;
00720     getEdges(v, neighbors);
00721 
00722     // Depth-first traversal of reachable graph
00723     std::map<unsigned int, const PlannerDataEdge*>::iterator it;
00724     for (it = neighbors.begin(); it != neighbors.end(); ++it)
00725     {
00726         extractReachable(it->first, data);
00727         Cost weight;
00728         getEdgeWeight(v, it->first, &weight);
00729         data.addEdge(idx, data.vertexIndex(getVertex(it->first)), *(it->second), weight);
00730     }
00731 }
00732 
00733 ompl::base::StateStoragePtr ompl::base::PlannerData::extractStateStorage() const
00734 {
00735     GraphStateStorage *store = new GraphStateStorage(si_->getStateSpace());
00736     if (graph_)
00737     {
00738         // copy the states
00739         std::map<unsigned int, unsigned int> indexMap;
00740         for (std::map<const State*, unsigned int>::const_iterator it = stateIndexMap_.begin() ; it != stateIndexMap_.end() ; ++it)
00741         {
00742             indexMap[it->second] = store->size();
00743             store->addState(it->first);
00744         }
00745 
00746         // add the edges
00747         for (std::map<unsigned int, unsigned int>::const_iterator it = indexMap.begin() ; it != indexMap.end() ; ++it)
00748         {
00749             std::vector<unsigned int> edgeList;
00750             getEdges(it->first, edgeList);
00751             GraphStateStorage::MetadataType &md = store->getMetadata(it->second);
00752             md.resize(edgeList.size());
00753             // map node indices to index values in StateStorage
00754             for (std::size_t k = 0 ; k < edgeList.size() ; ++k)
00755                 md[k] = indexMap[edgeList[k]];
00756         }
00757     }
00758     return StateStoragePtr(store);
00759 }
00760 
00761 ompl::base::PlannerData::Graph& ompl::base::PlannerData::toBoostGraph()
00762 {
00763     ompl::base::PlannerData::Graph *boostgraph = reinterpret_cast<ompl::base::PlannerData::Graph*>(graphRaw_);
00764     return *boostgraph;
00765 }
00766 
00767 const ompl::base::PlannerData::Graph& ompl::base::PlannerData::toBoostGraph() const
00768 {
00769     const ompl::base::PlannerData::Graph *boostgraph = reinterpret_cast<const ompl::base::PlannerData::Graph*>(graphRaw_);
00770     return *boostgraph;
00771 }
00772 
00773 const ompl::base::SpaceInformationPtr& ompl::base::PlannerData::getSpaceInformation() const
00774 {
00775     return si_;
00776 }
00777 
00778 void ompl::base::PlannerData::freeMemory()
00779 {
00780     // Freeing decoupled states, if any
00781     for (std::set<State*>::iterator it = decoupledStates_.begin(); it != decoupledStates_.end(); ++it)
00782         si_->freeState(*it);
00783 
00784     if (graph_)
00785     {
00786         std::pair<Graph::EIterator, Graph::EIterator> eiterators = boost::edges(*graph_);
00787         boost::property_map<Graph::Type, edge_type_t>::type edges = get(edge_type_t(), *graph_);
00788         for (Graph::EIterator iter = eiterators.first; iter != eiterators.second; ++iter)
00789             delete boost::get(edges, *iter);
00790 
00791         std::pair<Graph::VIterator, Graph::VIterator> viterators = boost::vertices(*graph_);
00792         boost::property_map<Graph::Type, vertex_type_t>::type vertices = get(vertex_type_t(), *graph_);
00793         for (Graph::VIterator iter = viterators.first; iter != viterators.second; ++iter)
00794            delete vertices[*iter];
00795 
00796         graph_->clear();
00797     }
00798 }
00799 
00800 bool ompl::base::PlannerData::hasControls() const
00801 {
00802     return false;
00803 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines