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 }