PlannerData.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011, Rice University
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Rice University nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Ryan Luna */
36 
37 #include "ompl/base/PlannerData.h"
38 #include "ompl/base/PlannerDataGraph.h"
39 #include "ompl/base/StateStorage.h"
40 #include "ompl/base/OptimizationObjective.h"
41 #include "ompl/base/objectives/PathLengthOptimizationObjective.h"
42 #include "ompl/base/ScopedState.h"
43 
44 #include <boost/graph/graphviz.hpp>
45 #include <boost/graph/graphml.hpp>
46 #include <boost/graph/dijkstra_shortest_paths.hpp>
47 #include <boost/version.hpp>
48 #if BOOST_VERSION < 105100
49 #warning Boost version >=1.51 is needed for ompl::base::PlannerData::printGraphML
50 #else
51 #include <boost/property_map/function_property_map.hpp>
52 #endif
53 
54 // This is a convenient macro to cast the void* graph pointer as the
55 // Boost.Graph structure from PlannerDataGraph.h
56 #define graph_ reinterpret_cast<ompl::base::PlannerData::Graph*>(graphRaw_)
57 
60 const unsigned int ompl::base::PlannerData::INVALID_INDEX = std::numeric_limits<unsigned int>::max();
61 
63 {
64  graphRaw_ = new Graph();
65 }
66 
68 {
69  freeMemory();
70 
71  if (graph_)
72  {
73  delete graph_;
74  graphRaw_ = NULL;
75  }
76 }
77 
79 {
80  freeMemory();
81  decoupledStates_.clear();
82 }
83 
85 {
86  unsigned int count = 0;
87  for (unsigned int i = 0; i < numVertices(); ++i)
88  {
89  PlannerDataVertex &vtx = getVertex(i);
90  // If this vertex's state is not in the decoupled list, clone it and add it
91  if (decoupledStates_.find(const_cast<State*>(vtx.getState())) == decoupledStates_.end())
92  {
93  const State *oldState = vtx.getState();
94  State *clone = si_->cloneState(oldState);
95  decoupledStates_.insert(clone);
96  // Replacing the shallow state pointer with our shiny new clone
97  vtx.state_ = clone;
98 
99  // Remove oldState from stateIndexMap
100  stateIndexMap_.erase(oldState);
101  // Add the new, cloned state to stateIndexMap
102  stateIndexMap_[clone] = i;
103  count++;
104  }
105  }
106 }
107 
108 unsigned int ompl::base::PlannerData::getEdges (unsigned int v, std::vector<unsigned int>& edgeList) const
109 {
110  std::pair<Graph::AdjIterator, Graph::AdjIterator> iterators = boost::adjacent_vertices(boost::vertex(v, *graph_), *graph_);
111 
112  edgeList.clear();
113  boost::property_map<Graph::Type, boost::vertex_index_t>::type vertices = get(boost::vertex_index, *graph_);
114  for (Graph::AdjIterator iter = iterators.first; iter != iterators.second; ++iter)
115  edgeList.push_back(vertices[*iter]);
116 
117  return edgeList.size();
118 }
119 
120 unsigned int ompl::base::PlannerData::getEdges (unsigned int v, std::map<unsigned int, const PlannerDataEdge*>& edgeMap) const
121 {
122  std::pair<Graph::OEIterator, Graph::OEIterator> iterators = boost::out_edges(boost::vertex(v, *graph_), *graph_);
123 
124  edgeMap.clear();
125  boost::property_map<Graph::Type, edge_type_t>::type edges = get(edge_type_t(), *graph_);
126  boost::property_map<Graph::Type, boost::vertex_index_t>::type vertices = get(boost::vertex_index, *graph_);
127  for (Graph::OEIterator iter = iterators.first; iter != iterators.second; ++iter)
128  edgeMap[vertices[boost::target(*iter, *graph_)]] = boost::get(edges, *iter);
129 
130  return edgeMap.size();
131 }
132 
133 unsigned int ompl::base::PlannerData::getIncomingEdges (unsigned int v, std::vector<unsigned int>& edgeList) const
134 {
135  std::pair<Graph::IEIterator, Graph::IEIterator> iterators = boost::in_edges(boost::vertex(v, *graph_), *graph_);
136 
137  edgeList.clear();
138  boost::property_map<Graph::Type, boost::vertex_index_t>::type vertices = get(boost::vertex_index, *graph_);
139  for (Graph::IEIterator iter = iterators.first; iter != iterators.second; ++iter)
140  edgeList.push_back(vertices[boost::source(*iter, *graph_)]);
141 
142  return edgeList.size();
143 }
144 
145 unsigned int ompl::base::PlannerData::getIncomingEdges (unsigned int v, std::map<unsigned int, const PlannerDataEdge*> &edgeMap) const
146 {
147  std::pair<Graph::IEIterator, Graph::IEIterator> iterators = boost::in_edges(boost::vertex(v, *graph_), *graph_);
148 
149  edgeMap.clear();
150  boost::property_map<Graph::Type, edge_type_t>::type edges = get(edge_type_t(), *graph_);
151  boost::property_map<Graph::Type, boost::vertex_index_t>::type vertices = get(boost::vertex_index, *graph_);
152  for (Graph::IEIterator iter = iterators.first; iter != iterators.second; ++iter)
153  edgeMap[vertices[boost::source(*iter, *graph_)]] = boost::get(edges, *iter);
154 
155  return edgeMap.size();
156 }
157 
158 bool ompl::base::PlannerData::getEdgeWeight(unsigned int v1, unsigned int v2, Cost* weight) const
159 {
160  Graph::Edge e;
161  bool exists;
162  boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
163 
164  if (exists)
165  {
166  boost::property_map<Graph::Type, boost::edge_weight_t>::type edges = get(boost::edge_weight, *graph_);
167  *weight = edges[e];
168  return true;
169  }
170 
171  return false;
172 }
173 
174 bool ompl::base::PlannerData::setEdgeWeight(unsigned int v1, unsigned int v2, Cost weight)
175 {
176  Graph::Edge e;
177  bool exists;
178  boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
179 
180  if (exists)
181  {
182  boost::property_map<Graph::Type, boost::edge_weight_t>::type edges = get(boost::edge_weight, *graph_);
183  edges[e] = weight;
184  }
185 
186  return exists;
187 }
188 
189 bool ompl::base::PlannerData::edgeExists (unsigned int v1, unsigned int v2) const
190 {
191  Graph::Edge e;
192  bool exists;
193 
194  boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
195  return exists;
196 }
197 
199 {
200  return vertexIndex(v) != INVALID_INDEX;
201 }
202 
204 {
205  return boost::num_vertices(*graph_);
206 }
207 
209 {
210  return boost::num_edges(*graph_);
211 }
212 
214 {
215  if (index >= boost::num_vertices(*graph_))
216  return NO_VERTEX;
217 
218  boost::property_map<Graph::Type, vertex_type_t>::type vertices = get(vertex_type_t(), *graph_);
219  return *(vertices[boost::vertex(index, *graph_)]);
220 }
221 
223 {
224  if (index >= boost::num_vertices(*graph_))
225  return const_cast<ompl::base::PlannerDataVertex&>(NO_VERTEX);
226 
227  boost::property_map<Graph::Type, vertex_type_t>::type vertices = get(vertex_type_t(), *graph_);
228  return *(vertices[boost::vertex(index, *graph_)]);
229 }
230 
231 const ompl::base::PlannerDataEdge& ompl::base::PlannerData::getEdge (unsigned int v1, unsigned int v2) const
232 {
233  Graph::Edge e;
234  bool exists;
235  boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
236 
237  if (exists)
238  {
239  boost::property_map<Graph::Type, edge_type_t>::type edges = get(edge_type_t(), *graph_);
240  return *(boost::get(edges, e));
241  }
242 
243  return NO_EDGE;
244 }
245 
247 {
248  Graph::Edge e;
249  bool exists;
250  boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
251 
252  if (exists)
253  {
254  boost::property_map<Graph::Type, edge_type_t>::type edges = get(edge_type_t(), *graph_);
255  return *(boost::get(edges, e));
256  }
257 
258  return const_cast<ompl::base::PlannerDataEdge&>(NO_EDGE);
259 }
260 
261 void ompl::base::PlannerData::printGraphviz (std::ostream& out) const
262 {
263  boost::write_graphviz(out, *graph_);
264 }
265 
266 namespace
267 {
268  // Property map for extracting the edge weight of a graph edge as
269  // a double for printGraphML.
270  double edgeWeightAsDouble(ompl::base::PlannerData::Graph::Type &g,
272  {
273  return get(boost::edge_weight_t(), g)[e].value();
274  }
275 
276  // Property map for extracting states as arrays of doubles
277  std::string vertexCoords (ompl::base::PlannerData::Graph::Type &g,
280  {
281  s = *get(vertex_type_t(), g)[v]->getState();
282  std::vector<double> coords(s.reals());
283  std::ostringstream sstream;
284  if (coords.size()>0)
285  {
286  sstream << coords[0];
287  for (std::size_t i = 1; i < coords.size(); ++i)
288  sstream << ',' << coords[i];
289  }
290  return sstream.str();
291  }
292 }
293 
294 void ompl::base::PlannerData::printGraphML (std::ostream& out) const
295 {
296 #if BOOST_VERSION < 105100
297  OMPL_WARN("Boost version >=1.51 is needed for ompl::base::PlannerData::printGraphML");
298 #else
299  // For some reason, make_function_property_map can't infer its
300  // template arguments corresponding to edgeWeightAsDouble's type
301  // signature. So, we have to use this horribly verbose
302  // instantiation of the property map.
303  //
304  // \TODO Can we use make_function_property_map() here and have it
305  // infer the property template arguments?
306  boost::function_property_map<
307  boost::function<double (ompl::base::PlannerData::Graph::Edge)>,
309  double>
310  weightmap(boost::bind(&edgeWeightAsDouble, *graph_, _1));
312  boost::function_property_map<
313  boost::function<std::string (ompl::base::PlannerData::Graph::Vertex)>,
315  std::string >
316  coordsmap(boost::bind(&vertexCoords, *graph_, s, _1));
317 
318 
319  // Not writing vertex or edge structures.
320  boost::dynamic_properties dp;
321  dp.property("weight", weightmap);
322  dp.property("coords", coordsmap);
323 
324  boost::write_graphml(out, *graph_, dp);
325 #endif
326 }
327 
329 {
330  std::map<const State*, unsigned int>::const_iterator it = stateIndexMap_.find(v.getState());
331  if (it != stateIndexMap_.end())
332  return it->second;
333  return INVALID_INDEX;
334 }
335 
337 {
338  return startVertexIndices_.size();
339 }
340 
342 {
343  return goalVertexIndices_.size();
344 }
345 
346 unsigned int ompl::base::PlannerData::getStartIndex (unsigned int i) const
347 {
348  if (i >= startVertexIndices_.size())
349  return INVALID_INDEX;
350 
351  return startVertexIndices_[i];
352 }
353 
354 unsigned int ompl::base::PlannerData::getGoalIndex (unsigned int i) const
355 {
356  if (i >= goalVertexIndices_.size())
357  return INVALID_INDEX;
358 
359  return goalVertexIndices_[i];
360 }
361 
362 bool ompl::base::PlannerData::isStartVertex (unsigned int index) const
363 {
364  return std::binary_search(startVertexIndices_.begin(), startVertexIndices_.end(), index);
365 }
366 
367 bool ompl::base::PlannerData::isGoalVertex (unsigned int index) const
368 {
369  return std::binary_search(goalVertexIndices_.begin(), goalVertexIndices_.end(), index);
370 }
371 
373 {
374  if (i >= startVertexIndices_.size())
375  return NO_VERTEX;
376 
377  return getVertex(startVertexIndices_[i]);
378 }
379 
381 {
382  if (i >= startVertexIndices_.size())
383  return const_cast<ompl::base::PlannerDataVertex&>(NO_VERTEX);
384 
385  return getVertex(startVertexIndices_[i]);
386 }
387 
389 {
390  if (i >= goalVertexIndices_.size())
391  return NO_VERTEX;
392 
393  return getVertex(goalVertexIndices_[i]);
394 }
395 
397 {
398  if (i >= goalVertexIndices_.size())
399  return const_cast<ompl::base::PlannerDataVertex&>(NO_VERTEX);
400 
401  return getVertex(goalVertexIndices_[i]);
402 }
403 
405 {
406  // Do not add vertices with null states
407  if (st.getState() == NULL)
408  return INVALID_INDEX;
409 
410  unsigned int index = vertexIndex(st);
411  if (index == INVALID_INDEX) // Vertex does not already exist
412  {
413  // Clone the state to prevent object slicing when retrieving this object
414  ompl::base::PlannerDataVertex *clone = st.clone();
415  Graph::Vertex v = boost::add_vertex(clone, *graph_);
416  boost::property_map<Graph::Type, boost::vertex_index_t>::type vertexIndexMap = get(boost::vertex_index, *graph_);
417 
418  // Insert this entry into the stateIndexMap_ for fast lookup
419  stateIndexMap_[clone->getState()] = numVertices()-1;
420  return vertexIndexMap[v];
421  }
422  return index;
423 }
424 
426 {
427  unsigned int index = addVertex(v);
428  if (index != INVALID_INDEX)
430 
431  return index;
432 }
433 
435 {
436  unsigned int index = addVertex(v);
437 
438  if (index != INVALID_INDEX)
439  markGoalState(v.getState());
440 
441  return index;
442 }
443 
444 bool ompl::base::PlannerData::addEdge(unsigned int v1, unsigned int v2, const PlannerDataEdge &edge, Cost weight)
445 {
446  // If either of the vertices do not exist, don't add an edge
447  if (v1 >= numVertices() || v2 >= numVertices())
448  return false;
449 
450  // If an edge already exists, do not add one
451  if (edgeExists (v1, v2))
452  return false;
453 
454  // Clone the edge to prevent object slicing
455  ompl::base::PlannerDataEdge *clone = edge.clone();
456  const Graph::edge_property_type properties(clone, weight);
457 
458  Graph::Edge e;
459  bool added = false;
460  tie(e, added) = boost::add_edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), properties, *graph_);
461 
462  if (!added)
463  delete clone;
464 
465  return added;
466 }
467 
469 {
470  unsigned int index1 = addVertex(v1);
471  unsigned int index2 = addVertex(v2);
472 
473  // If neither vertex was added or already exists, return false
474  if (index1 == INVALID_INDEX && index2 == INVALID_INDEX)
475  return false;
476 
477  // Only add the edge if both vertices exist
478  if (index1 != INVALID_INDEX && index2 != INVALID_INDEX)
479  return addEdge (index1, index2, edge, weight);
480 
481  return true;
482 }
483 
485 {
486  unsigned int index = vertexIndex (st);
487  if (index != INVALID_INDEX)
488  return removeVertex (index);
489  return false;
490 }
491 
492 bool ompl::base::PlannerData::removeVertex (unsigned int vIndex)
493 {
494  if (vIndex >= boost::num_vertices(*graph_))
495  return false;
496 
497  // Retrieve a list of all edge structures
498  boost::property_map<Graph::Type, edge_type_t>::type edgePropertyMap = get(edge_type_t(), *graph_);
499 
500  // Freeing memory associated with outgoing edges of this vertex
501  std::pair<Graph::OEIterator, Graph::OEIterator> oiterators = boost::out_edges(boost::vertex(vIndex, *graph_), *graph_);
502  for (Graph::OEIterator iter = oiterators.first; iter != oiterators.second; ++iter)
503  delete edgePropertyMap[*iter];
504 
505  // Freeing memory associated with incoming edges of this vertex
506  std::pair<Graph::IEIterator, Graph::IEIterator> initerators = boost::in_edges(boost::vertex(vIndex, *graph_), *graph_);
507  for (Graph::IEIterator iter = initerators.first; iter != initerators.second; ++iter)
508  delete edgePropertyMap[*iter];
509 
510  // Remove this vertex from stateIndexMap_, and update the map
511  stateIndexMap_.erase(getVertex(vIndex).getState());
512  boost::property_map<Graph::Type, vertex_type_t>::type vertices = get(vertex_type_t(), *graph_);
513  for (unsigned int i = vIndex+1; i < boost::num_vertices(*graph_); ++i)
514  stateIndexMap_[vertices[boost::vertex(i, *graph_)]->getState()]--;
515 
516  // Remove this vertex from the start and/or goal index list, if it exists. Update the lists.
517  std::vector<unsigned int>::iterator it = std::find(startVertexIndices_.begin(), startVertexIndices_.end(), vIndex);
518  if (it != startVertexIndices_.end())
519  startVertexIndices_.erase(it);
520  for (size_t i = 0; i < startVertexIndices_.size(); ++i)
521  if (startVertexIndices_[i] > vIndex)
522  startVertexIndices_[i]--;
523 
524  it = std::find(goalVertexIndices_.begin(), goalVertexIndices_.end(), vIndex);
525  if (it != goalVertexIndices_.end())
526  goalVertexIndices_.erase(it);
527  for (size_t i = 0; i < goalVertexIndices_.size(); ++i)
528  if (goalVertexIndices_[i] > vIndex)
529  goalVertexIndices_[i]--;
530 
531  // If the state attached to this vertex was decoupled, free it here
532  State *vtxState = const_cast<State*>(getVertex(vIndex).getState());
533  if (decoupledStates_.find(vtxState) != decoupledStates_.end())
534  {
535  decoupledStates_.erase(vtxState);
536  si_->freeState(vtxState);
537  vtxState = NULL;
538  }
539 
540  // Slay the vertex
541  boost::clear_vertex(boost::vertex(vIndex, *graph_), *graph_);
542  boost::property_map<Graph::Type, vertex_type_t>::type vertexTypeMap = get(vertex_type_t(), *graph_);
543  delete vertexTypeMap[boost::vertex(vIndex, *graph_)];
544  boost::remove_vertex(boost::vertex(vIndex, *graph_), *graph_);
545 
546  return true;
547 }
548 
549 bool ompl::base::PlannerData::removeEdge (unsigned int v1, unsigned int v2)
550 {
551  Graph::Edge e;
552  bool exists;
553  boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
554 
555  if (!exists)
556  return false;
557 
558  // Freeing memory associated with this edge
559  boost::property_map<Graph::Type, edge_type_t>::type edges = get(edge_type_t(), *graph_);
560  delete edges[e];
561 
562  boost::remove_edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
563  return true;
564 }
565 
567 {
568  unsigned int index1, index2;
569  index1 = vertexIndex(v1);
570  index2 = vertexIndex(v2);
571 
572  if (index1 == INVALID_INDEX || index2 == INVALID_INDEX)
573  return false;
574 
575  return removeEdge (index1, index2);
576 }
577 
579 {
580  std::map<const State*, unsigned int>::const_iterator it = stateIndexMap_.find(st);
581  if (it != stateIndexMap_.end())
582  {
583  getVertex(it->second).setTag(tag);
584  return true;
585  }
586  return false;
587 }
588 
590 {
591  // Find the index in the stateIndexMap_
592  std::map<const State*, unsigned int>::const_iterator it = stateIndexMap_.find(st);
593  if (it != stateIndexMap_.end())
594  {
595  if (!isStartVertex(it->second))
596  {
597  startVertexIndices_.push_back(it->second);
598  // Sort the indices for quick lookup
599  std::sort(startVertexIndices_.begin(), startVertexIndices_.end());
600  }
601  return true;
602  }
603  return false;
604 }
605 
607 {
608  // Find the index in the stateIndexMap_
609  std::map<const State*, unsigned int>::const_iterator it = stateIndexMap_.find(st);
610  if (it != stateIndexMap_.end())
611  {
612  if (!isGoalVertex(it->second))
613  {
614  goalVertexIndices_.push_back(it->second);
615  // Sort the indices for quick lookup
616  std::sort(startVertexIndices_.begin(), startVertexIndices_.end());
617  }
618  return true;
619  }
620  return false;
621 }
622 
624 {
625  unsigned int nv = numVertices();
626  for (unsigned int i = 0; i < nv; ++i)
627  {
628  std::map<unsigned int, const PlannerDataEdge*> nbrs;
629  getEdges(i, nbrs);
630 
631  std::map<unsigned int, const PlannerDataEdge*>::const_iterator it;
632  for (it = nbrs.begin(); it != nbrs.end(); ++it)
633  {
634  setEdgeWeight(i, it->first, opt.motionCost(getVertex(i).getState(),
635  getVertex(it->first).getState()));
636  }
637  }
638 }
639 
641 {
642  // Create a PathLengthOptimizationObjective to compute the edge
643  // weights according to state space distance
645  computeEdgeWeights(opt);
646 }
647 
648 namespace
649 {
650  // Used in minimum spanning tree
651  ompl::base::Cost project2nd (ompl::base::Cost /*unused*/, ompl::base::Cost second)
652  {
653  return second;
654  }
655 }
656 
658  const base::OptimizationObjective &opt,
659  base::PlannerData &mst) const
660 {
661  std::vector<ompl::base::PlannerData::Graph::Vertex> pred(numVertices());
662 
663  // This is how boost's minimum spanning tree is actually
664  // implemented, except it lacks the generality for specifying our
665  // own comparison function or zero/inf values.
666  //
667  // \TODO Once (https://svn.boost.org/trac/boost/ticket/9368) gets
668  // into boost we can use the far more direct
669  // boost::prim_minimum_spanning_tree().
670  boost::dijkstra_shortest_paths
671  (*graph_, v,
672  boost::predecessor_map(&pred[0]).
673  distance_compare(boost::bind(&base::OptimizationObjective::
674  isCostBetterThan, &opt, _1, _2)).
675  distance_combine(&project2nd).
676  distance_inf(opt.infiniteCost()).
677  distance_zero(opt.identityCost()));
678 
679  // Adding vertices to MST
680  for (std::size_t i = 0; i < pred.size(); ++i)
681  {
682  if (isStartVertex(i))
683  mst.addStartVertex(getVertex(i));
684  else if (isGoalVertex(i))
685  mst.addGoalVertex(getVertex(i));
686  else
687  mst.addVertex(getVertex(i));
688  }
689 
690  // Adding edges to MST
691  for (std::size_t i = 0; i < pred.size(); ++i)
692  {
693  if (pred[i] != i)
694  {
695  Cost c;
696  getEdgeWeight(pred[i], i, &c);
697  mst.addEdge(pred[i], i, getEdge(pred[i], i), c);
698  }
699  }
700 }
701 
703 {
704  // If this vertex already exists in data, return
705  if (data.vertexExists(getVertex(v)))
706  return;
707 
708  // Adding the vertex corresponding to v into data
709  unsigned int idx;
710  if (isStartVertex(v))
711  idx = data.addStartVertex(getVertex(v));
712  else if (isGoalVertex(v))
713  idx = data.addGoalVertex(getVertex(v));
714  else
715  idx = data.addVertex(getVertex(v));
716 
717  assert (idx != INVALID_INDEX);
718 
719  std::map<unsigned int, const PlannerDataEdge*> neighbors;
720  getEdges(v, neighbors);
721 
722  // Depth-first traversal of reachable graph
723  std::map<unsigned int, const PlannerDataEdge*>::iterator it;
724  for (it = neighbors.begin(); it != neighbors.end(); ++it)
725  {
726  extractReachable(it->first, data);
727  Cost weight;
728  getEdgeWeight(v, it->first, &weight);
729  data.addEdge(idx, data.vertexIndex(getVertex(it->first)), *(it->second), weight);
730  }
731 }
732 
733 ompl::base::StateStoragePtr ompl::base::PlannerData::extractStateStorage() const
734 {
735  GraphStateStorage *store = new GraphStateStorage(si_->getStateSpace());
736  if (graph_)
737  {
738  // copy the states
739  std::map<unsigned int, unsigned int> indexMap;
740  for (std::map<const State*, unsigned int>::const_iterator it = stateIndexMap_.begin() ; it != stateIndexMap_.end() ; ++it)
741  {
742  indexMap[it->second] = store->size();
743  store->addState(it->first);
744  }
745 
746  // add the edges
747  for (std::map<unsigned int, unsigned int>::const_iterator it = indexMap.begin() ; it != indexMap.end() ; ++it)
748  {
749  std::vector<unsigned int> edgeList;
750  getEdges(it->first, edgeList);
751  GraphStateStorage::MetadataType &md = store->getMetadata(it->second);
752  md.resize(edgeList.size());
753  // map node indices to index values in StateStorage
754  for (std::size_t k = 0 ; k < edgeList.size() ; ++k)
755  md[k] = indexMap[edgeList[k]];
756  }
757  }
758  return StateStoragePtr(store);
759 }
760 
762 {
763  ompl::base::PlannerData::Graph *boostgraph = reinterpret_cast<ompl::base::PlannerData::Graph*>(graphRaw_);
764  return *boostgraph;
765 }
766 
768 {
769  const ompl::base::PlannerData::Graph *boostgraph = reinterpret_cast<const ompl::base::PlannerData::Graph*>(graphRaw_);
770  return *boostgraph;
771 }
772 
774 {
775  return si_;
776 }
777 
778 void ompl::base::PlannerData::freeMemory()
779 {
780  // Freeing decoupled states, if any
781  for (std::set<State*>::iterator it = decoupledStates_.begin(); it != decoupledStates_.end(); ++it)
782  si_->freeState(*it);
783 
784  if (graph_)
785  {
786  std::pair<Graph::EIterator, Graph::EIterator> eiterators = boost::edges(*graph_);
787  boost::property_map<Graph::Type, edge_type_t>::type edges = get(edge_type_t(), *graph_);
788  for (Graph::EIterator iter = eiterators.first; iter != eiterators.second; ++iter)
789  delete boost::get(edges, *iter);
790 
791  std::pair<Graph::VIterator, Graph::VIterator> viterators = boost::vertices(*graph_);
792  boost::property_map<Graph::Type, vertex_type_t>::type vertices = get(vertex_type_t(), *graph_);
793  for (Graph::VIterator iter = viterators.first; iter != viterators.second; ++iter)
794  delete vertices[*iter];
795 
796  graph_->clear();
797  }
798 }
799 
801 {
802  return false;
803 }
const State * state_
The state represented by this vertex.
Definition: PlannerData.h:108
SpaceInformationPtr si_
The space information instance for this data.
Definition: PlannerData.h:405
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:164
void computeEdgeWeights()
Computes all edge weights using state space distance (i.e. getSpaceInformation()->distance()) ...
Wrapper class for the Boost.Graph representation of the PlannerData. This class inherits from a boost...
StateStoragePtr extractStateStorage() const
Extract a ompl::base::GraphStateStorage object from this PlannerData. Memory for states is copied (th...
std::vector< unsigned int > startVertexIndices_
A mutable listing of the vertices marked as start states. Stored in sorted order. ...
Definition: PlannerData.h:400
boost::graph_traits< Type >::edge_descriptor Edge
Boost.Graph edge descriptor.
bool vertexExists(const PlannerDataVertex &v) const
Check whether a vertex exists with the given vertex data.
virtual void decoupleFromPlanner()
Creates a deep copy of the states contained in the vertices of this PlannerData structure so that whe...
Definition: PlannerData.cpp:84
boost::graph_traits< Type >::adjacency_iterator AdjIterator
Boost.Graph adjacency iterator.
PlannerDataGraph Type
Data type for the Boost.Graph representation.
Definition of a scoped state.
Definition: ScopedState.h:56
virtual bool hasControls() const
Indicate whether any information about controls (ompl::control::Control) is stored in this instance...
unsigned int addGoalVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
boost::graph_traits< Type >::in_edge_iterator IEIterator
Boost.Graph input edge iterator.
void extractReachable(unsigned int v, PlannerData &data) const
Extracts the subset of PlannerData reachable from the vertex with index v. For tree structures...
bool markStartState(const State *st)
Mark the given state as a start vertex. If the given state does not exist in a vertex, false is returned.
Graph & toBoostGraph()
Extract a Boost.Graph object from this PlannerData.
unsigned int addVertex(const PlannerDataVertex &st)
Adds the given vertex to the graph data. The vertex index is returned. Duplicates are not added...
unsigned int numGoalVertices() const
Returns the number of goal vertices.
const PlannerDataVertex & getStartVertex(unsigned int i) const
Retrieve a reference to the ith start vertex object. If i is greater than the number of start vertice...
std::vector< unsigned int > goalVertexIndices_
A mutable listing of the vertices marked as goal states. Stored in sorted order.
Definition: PlannerData.h:402
unsigned int getStartIndex(unsigned int i) const
Returns the index of the ith start state. INVALID_INDEX is returned if i is out of range...
boost::graph_traits< Type >::edge_iterator EIterator
Boost.Graph edge iterator.
M MetadataType
the datatype of the metadata
Definition: StateStorage.h:215
bool getEdgeWeight(unsigned int v1, unsigned int v2, Cost *weight) const
Returns the weight of the edge between the given vertex indices. If there exists an edge between v1 a...
unsigned int getIncomingEdges(unsigned int v, std::vector< unsigned int > &edgeList) const
Returns a list of vertices with outgoing edges to the vertex with index v. The number of edges connec...
PlannerData(const SpaceInformationPtr &si)
Constructor. Accepts a SpaceInformationPtr for the space planned in.
Definition: PlannerData.cpp:62
virtual Cost infiniteCost() const
Get a cost which is greater than all other costs in this OptimizationObjective; required for use in D...
virtual void addState(const State *state)
Add a state to the set of states maintained by this storage structure. The state is copied to interna...
Definition: StateStorage.h:226
virtual Cost motionCost(const State *s1, const State *s2) const =0
Get the cost that corresponds to the motion segment between s1 and s2.
boost::graph_traits< Type >::vertex_iterator VIterator
Boost.Graph vertex iterator.
bool setEdgeWeight(unsigned int v1, unsigned int v2, Cost weight)
Sets the weight of the edge between the given vertex indices. If an edge between v1 and v2 does not e...
unsigned int getEdges(unsigned int v, std::vector< unsigned int > &edgeList) const
Returns a list of the vertex indexes directly connected to vertex with index v (outgoing edges)...
std::set< State * > decoupledStates_
A list of states that are allocated during the decoupleFromPlanner method. These states are freed by ...
Definition: PlannerData.h:408
boost::graph_traits< Type >::out_edge_iterator OEIterator
Boost.Graph output edge iterator.
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:60
virtual bool removeVertex(const PlannerDataVertex &st)
Removes the vertex associated with the given data. If the vertex does not exist, false is returned...
bool isGoalVertex(unsigned int index) const
Returns true if the given vertex index is marked as a goal vertex.
const PlannerDataVertex & getGoalVertex(unsigned int i) const
Retrieve a reference to the ith goal vertex object. If i is greater than the number of goal vertices...
bool tagState(const State *st, int tag)
Set the integer tag associated with the given state. If the given state does not exist in a vertex...
unsigned int numEdges() const
Retrieve the number of edges in this structure.
void printGraphML(std::ostream &out=std::cout) const
Writes a GraphML file of this structure to the given stream.
unsigned int numVertices() const
Retrieve the number of vertices in this structure.
boost::graph_traits< Type >::vertex_descriptor Vertex
Boost.Graph vertex descriptor.
StateStorageWithMetadata< std::vector< std::size_t > > GraphStateStorage
Storage of states where the metadata is a vector of indices. This is is typically used to store a gra...
Definition: StateStorage.h:277
unsigned int vertexIndex(const PlannerDataVertex &v) const
Return the index for the vertex associated with the given data. INVALID_INDEX is returned if this ver...
const M & getMetadata(unsigned int index) const
Get const access to the metadata of a state at a particular index.
Definition: StateStorage.h:246
virtual bool addEdge(unsigned int v1, unsigned int v2, const PlannerDataEdge &edge=PlannerDataEdge(), Cost weight=Cost(1.0))
Adds a directed edge between the given vertex indexes. An optional edge structure and weight can be s...
A boost shared pointer wrapper for ompl::base::SpaceInformation.
const PlannerDataVertex & getVertex(unsigned int index) const
Retrieve a reference to the vertex object with the given index. If this vertex does not exist...
void printGraphviz(std::ostream &out=std::cout) const
Writes a Graphviz dot file of this structure to the given stream.
An optimization objective which corresponds to optimizing path length.
virtual void clear()
Clears the entire data structure.
Definition: PlannerData.cpp:78
unsigned int addStartVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
Definition of an abstract state.
Definition: State.h:50
std::vector< double > reals() const
Return the real values corresponding to this state. If a conversion is not possible, an exception is thrown.
Definition: ScopedState.h:356
virtual ~PlannerData()
Destructor.
Definition: PlannerData.cpp:67
virtual PlannerDataEdge * clone() const
Return a clone of this object, allocated from the heap.
Definition: PlannerData.h:123
static const PlannerDataVertex NO_VERTEX
Representation for a non-existant vertex.
Definition: PlannerData.h:172
State storage that allows storing state metadata as well.
Definition: StateStorage.h:210
Abstract definition of optimization objectives.
bool edgeExists(unsigned int v1, unsigned int v2) const
Check whether an edge between vertex index v1 and index v2 exists.
static const unsigned int INVALID_INDEX
Representation of an invalid vertex index.
Definition: PlannerData.h:174
const PlannerDataEdge & getEdge(unsigned int v1, unsigned int v2) const
Retrieve a reference to the edge object connecting vertices with indexes v1 and v2. If this edge does not exist, NO_EDGE is returned.
bool markGoalState(const State *st)
Mark the given state as a goal vertex. If the given state does not exist in a vertex, false is returned.
bool isStartVertex(unsigned int index) const
Returns true if the given vertex index is marked as a start vertex.
virtual void setTag(int tag)
Set the integer tag associated with this vertex.
Definition: PlannerData.h:72
unsigned int numStartVertices() const
Returns the number of start vertices.
void extractMinimumSpanningTree(unsigned int v, const OptimizationObjective &opt, PlannerData &mst) const
Extracts the minimum spanning tree of the data rooted at the vertex with index v. The minimum spannin...
Base class for a PlannerData edge.
Definition: PlannerData.h:117
const SpaceInformationPtr & getSpaceInformation() const
Return the instance of SpaceInformation used in this PlannerData.
std::map< const State *, unsigned int > stateIndexMap_
A mapping of states to vertex indexes. For fast lookup of vertex index.
Definition: PlannerData.h:398
virtual PlannerDataVertex * clone() const
Return a clone of this object, allocated from the heap.
Definition: PlannerData.h:77
virtual const State * getState() const
Retrieve the state associated with this vertex.
Definition: PlannerData.h:74
unsigned int getGoalIndex(unsigned int i) const
Returns the index of the ith goal state. INVALID_INDEX is returned if i is out of range Indexes are v...
virtual bool removeEdge(unsigned int v1, unsigned int v2)
Removes the edge between vertex indexes v1 and v2. Success is returned.
virtual Cost identityCost() const
Get the identity cost value. The identity cost value is the cost c_i such that, for all costs c...
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
std::size_t size() const
Return the number of stored states.
Definition: StateStorage.h:98
static const PlannerDataEdge NO_EDGE
Representation for a non-existant edge.
Definition: PlannerData.h:167
std::map< std::string, std::string > properties
Any extra properties (key-value pairs) the planner can set.
Definition: PlannerData.h:394
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66