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/property_map/function_property_map.hpp>
48 #include <utility>
49 
50 // This is a convenient macro to cast the void* graph pointer as the
51 // Boost.Graph structure from PlannerDataGraph.h
52 #define graph_ (reinterpret_cast<ompl::base::PlannerData::Graph *>(graphRaw_))
53 
56 const unsigned int ompl::base::PlannerData::INVALID_INDEX = std::numeric_limits<unsigned int>::max();
57 
58 ompl::base::PlannerData::PlannerData(SpaceInformationPtr si) : si_(std::move(si))
59 {
60  graphRaw_ = new Graph();
61 }
62 
64 {
65  freeMemory();
66 
67  if (graph_)
68  {
69  delete graph_;
70  graphRaw_ = nullptr;
71  }
72 }
73 
75 {
76  freeMemory();
77  decoupledStates_.clear();
78 }
79 
81 {
82  unsigned int count = 0;
83  for (unsigned int i = 0; i < numVertices(); ++i)
84  {
85  PlannerDataVertex &vtx = getVertex(i);
86  // If this vertex's state is not in the decoupled list, clone it and add it
87  if (decoupledStates_.find(const_cast<State *>(vtx.getState())) == decoupledStates_.end())
88  {
89  const State *oldState = vtx.getState();
90  State *clone = si_->cloneState(oldState);
91  decoupledStates_.insert(clone);
92  // Replacing the shallow state pointer with our shiny new clone
93  vtx.state_ = clone;
94 
95  // Remove oldState from stateIndexMap
96  stateIndexMap_.erase(oldState);
97  // Add the new, cloned state to stateIndexMap
98  stateIndexMap_[clone] = i;
99  count++;
100  }
101  }
102 }
103 
104 unsigned int ompl::base::PlannerData::getEdges(unsigned int v, std::vector<unsigned int> &edgeList) const
105 {
106  std::pair<Graph::AdjIterator, Graph::AdjIterator> iterators =
107  boost::adjacent_vertices(boost::vertex(v, *graph_), *graph_);
108 
109  edgeList.clear();
110  boost::property_map<Graph::Type, boost::vertex_index_t>::type vertices = get(boost::vertex_index, *graph_);
111  for (Graph::AdjIterator iter = iterators.first; iter != iterators.second; ++iter)
112  edgeList.push_back(vertices[*iter]);
113 
114  return edgeList.size();
115 }
116 
117 unsigned int ompl::base::PlannerData::getEdges(unsigned int v,
118  std::map<unsigned int, const PlannerDataEdge *> &edgeMap) const
119 {
120  std::pair<Graph::OEIterator, Graph::OEIterator> iterators = boost::out_edges(boost::vertex(v, *graph_), *graph_);
121 
122  edgeMap.clear();
123  boost::property_map<Graph::Type, edge_type_t>::type edges = get(edge_type_t(), *graph_);
124  boost::property_map<Graph::Type, boost::vertex_index_t>::type vertices = get(boost::vertex_index, *graph_);
125  for (Graph::OEIterator iter = iterators.first; iter != iterators.second; ++iter)
126  edgeMap[vertices[boost::target(*iter, *graph_)]] = boost::get(edges, *iter);
127 
128  return edgeMap.size();
129 }
130 
131 unsigned int ompl::base::PlannerData::getIncomingEdges(unsigned int v, std::vector<unsigned int> &edgeList) const
132 {
133  std::pair<Graph::IEIterator, Graph::IEIterator> iterators = boost::in_edges(boost::vertex(v, *graph_), *graph_);
134 
135  edgeList.clear();
136  boost::property_map<Graph::Type, boost::vertex_index_t>::type vertices = get(boost::vertex_index, *graph_);
137  for (Graph::IEIterator iter = iterators.first; iter != iterators.second; ++iter)
138  edgeList.push_back(vertices[boost::source(*iter, *graph_)]);
139 
140  return edgeList.size();
141 }
142 
143 unsigned int ompl::base::PlannerData::getIncomingEdges(unsigned int v,
144  std::map<unsigned int, const PlannerDataEdge *> &edgeMap) const
145 {
146  std::pair<Graph::IEIterator, Graph::IEIterator> iterators = boost::in_edges(boost::vertex(v, *graph_), *graph_);
147 
148  edgeMap.clear();
149  boost::property_map<Graph::Type, edge_type_t>::type edges = get(edge_type_t(), *graph_);
150  boost::property_map<Graph::Type, boost::vertex_index_t>::type vertices = get(boost::vertex_index, *graph_);
151  for (Graph::IEIterator iter = iterators.first; iter != iterators.second; ++iter)
152  edgeMap[vertices[boost::source(*iter, *graph_)]] = boost::get(edges, *iter);
153 
154  return edgeMap.size();
155 }
156 
157 bool ompl::base::PlannerData::getEdgeWeight(unsigned int v1, unsigned int v2, Cost *weight) const
158 {
159  Graph::Edge e;
160  bool exists;
161  boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
162 
163  if (exists)
164  {
165  boost::property_map<Graph::Type, boost::edge_weight_t>::type edges = get(boost::edge_weight, *graph_);
166  *weight = edges[e];
167  return true;
168  }
169 
170  return false;
171 }
172 
173 bool ompl::base::PlannerData::setEdgeWeight(unsigned int v1, unsigned int v2, Cost weight)
174 {
175  Graph::Edge e;
176  bool exists;
177  boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
178 
179  if (exists)
180  {
181  boost::property_map<Graph::Type, boost::edge_weight_t>::type edges = get(boost::edge_weight, *graph_);
182  edges[e] = weight;
183  }
184 
185  return exists;
186 }
187 
188 bool ompl::base::PlannerData::edgeExists(unsigned int v1, unsigned int v2) const
189 {
190  Graph::Edge e;
191  bool exists;
192 
193  boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
194  return exists;
195 }
196 
198 {
199  return vertexIndex(v) != INVALID_INDEX;
200 }
201 
203 {
204  return boost::num_vertices(*graph_);
205 }
206 
208 {
209  return boost::num_edges(*graph_);
210 }
211 
213 {
214  if (index >= boost::num_vertices(*graph_))
215  return NO_VERTEX;
216 
217  boost::property_map<Graph::Type, vertex_type_t>::type vertices = get(vertex_type_t(), *graph_);
218  return *(vertices[boost::vertex(index, *graph_)]);
219 }
220 
222 {
223  if (index >= boost::num_vertices(*graph_))
224  return const_cast<ompl::base::PlannerDataVertex &>(NO_VERTEX);
225 
226  boost::property_map<Graph::Type, vertex_type_t>::type vertices = get(vertex_type_t(), *graph_);
227  return *(vertices[boost::vertex(index, *graph_)]);
228 }
229 
230 const ompl::base::PlannerDataEdge &ompl::base::PlannerData::getEdge(unsigned int v1, unsigned int v2) const
231 {
232  Graph::Edge e;
233  bool exists;
234  boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
235 
236  if (exists)
237  {
238  boost::property_map<Graph::Type, edge_type_t>::type edges = get(edge_type_t(), *graph_);
239  return *(boost::get(edges, e));
240  }
241 
242  return NO_EDGE;
243 }
244 
246 {
247  Graph::Edge e;
248  bool exists;
249  boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
250 
251  if (exists)
252  {
253  boost::property_map<Graph::Type, edge_type_t>::type edges = get(edge_type_t(), *graph_);
254  return *(boost::get(edges, e));
255  }
256 
257  return const_cast<ompl::base::PlannerDataEdge &>(NO_EDGE);
258 }
259 
260 void ompl::base::PlannerData::printGraphviz(std::ostream &out) const
261 {
262  boost::write_graphviz(out, *graph_);
263 }
264 
265 namespace
266 {
267  // Property map for extracting states as arrays of doubles
268  std::string vertexCoords(ompl::base::PlannerData::Graph::Type &g, ompl::base::ScopedState<> &s,
270  {
271  s = *get(vertex_type_t(), g)[v]->getState();
272  std::vector<double> coords(s.reals());
273  std::ostringstream sstream;
274  if (!coords.empty())
275  {
276  sstream << coords[0];
277  for (std::size_t i = 1; i < coords.size(); ++i)
278  sstream << ',' << coords[i];
279  }
280  return sstream.str();
281  }
282 }
283 
284 void ompl::base::PlannerData::printGraphML(std::ostream &out) const
285 {
286  // For some reason, make_function_property_map can't infer its
287  // template arguments corresponding to edgeWeightAsDouble's type
288  // signature. So, we have to use this horribly verbose
289  // instantiation of the property map.
290  //
291  // \todo Can we use make_function_property_map() here and have it
292  // infer the property template arguments?
294  boost::function_property_map<std::function<double(Edge)>, Edge> weightmap([this](Edge e)
295  {
296  return get(boost::edge_weight_t(),
297  *graph_)[e].value();
298  });
301  boost::function_property_map<std::function<std::string(Vertex)>, Vertex> coordsmap([this, &s](Vertex v)
302  {
303  return vertexCoords(*graph_,
304  s, v);
305  });
306 
307  // Not writing vertex or edge structures.
308  boost::dynamic_properties dp;
309  dp.property("weight", weightmap);
310  dp.property("coords", coordsmap);
311 
312  boost::write_graphml(out, *graph_, dp);
313 }
314 
316 {
317  auto it = stateIndexMap_.find(v.getState());
318  if (it != stateIndexMap_.end())
319  return it->second;
320  return INVALID_INDEX;
321 }
322 
324 {
325  return startVertexIndices_.size();
326 }
327 
329 {
330  return goalVertexIndices_.size();
331 }
332 
333 unsigned int ompl::base::PlannerData::getStartIndex(unsigned int i) const
334 {
335  if (i >= startVertexIndices_.size())
336  return INVALID_INDEX;
337 
338  return startVertexIndices_[i];
339 }
340 
341 unsigned int ompl::base::PlannerData::getGoalIndex(unsigned int i) const
342 {
343  if (i >= goalVertexIndices_.size())
344  return INVALID_INDEX;
345 
346  return goalVertexIndices_[i];
347 }
348 
349 bool ompl::base::PlannerData::isStartVertex(unsigned int index) const
350 {
351  return std::binary_search(startVertexIndices_.begin(), startVertexIndices_.end(), index);
352 }
353 
354 bool ompl::base::PlannerData::isGoalVertex(unsigned int index) const
355 {
356  return std::binary_search(goalVertexIndices_.begin(), goalVertexIndices_.end(), index);
357 }
358 
360 {
361  if (i >= startVertexIndices_.size())
362  return NO_VERTEX;
363 
364  return getVertex(startVertexIndices_[i]);
365 }
366 
368 {
369  if (i >= startVertexIndices_.size())
370  return const_cast<ompl::base::PlannerDataVertex &>(NO_VERTEX);
371 
372  return getVertex(startVertexIndices_[i]);
373 }
374 
376 {
377  if (i >= goalVertexIndices_.size())
378  return NO_VERTEX;
379 
380  return getVertex(goalVertexIndices_[i]);
381 }
382 
384 {
385  if (i >= goalVertexIndices_.size())
386  return const_cast<ompl::base::PlannerDataVertex &>(NO_VERTEX);
387 
388  return getVertex(goalVertexIndices_[i]);
389 }
390 
392 {
393  // Do not add vertices with null states
394  if (st.getState() == nullptr)
395  return INVALID_INDEX;
396 
397  unsigned int index = vertexIndex(st);
398  if (index == INVALID_INDEX) // Vertex does not already exist
399  {
400  // Clone the state to prevent object slicing when retrieving this object
401  ompl::base::PlannerDataVertex *clone = st.clone();
402  Graph::Vertex v = boost::add_vertex(clone, *graph_);
403  boost::property_map<Graph::Type, boost::vertex_index_t>::type vertexIndexMap =
404  get(boost::vertex_index, *graph_);
405 
406  // Insert this entry into the stateIndexMap_ for fast lookup
407  stateIndexMap_[clone->getState()] = numVertices() - 1;
408  return vertexIndexMap[v];
409  }
410  return index;
411 }
412 
414 {
415  unsigned int index = addVertex(v);
416  if (index != INVALID_INDEX)
417  markStartState(v.getState());
418 
419  return index;
420 }
421 
423 {
424  unsigned int index = addVertex(v);
425 
426  if (index != INVALID_INDEX)
427  markGoalState(v.getState());
428 
429  return index;
430 }
431 
432 bool ompl::base::PlannerData::addEdge(unsigned int v1, unsigned int v2, const PlannerDataEdge &edge, Cost weight)
433 {
434  // If either of the vertices do not exist, don't add an edge
435  if (v1 >= numVertices() || v2 >= numVertices())
436  return false;
437 
438  // If an edge already exists, do not add one
439  if (edgeExists(v1, v2))
440  return false;
441 
442  // Clone the edge to prevent object slicing
443  ompl::base::PlannerDataEdge *clone = edge.clone();
444  const Graph::edge_property_type properties(clone, weight);
445 
446  Graph::Edge e;
447  bool added = false;
448  tie(e, added) = boost::add_edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), properties, *graph_);
449 
450  if (!added)
451  delete clone;
452 
453  return added;
454 }
455 
457  const PlannerDataEdge &edge, Cost weight)
458 {
459  unsigned int index1 = addVertex(v1);
460  unsigned int index2 = addVertex(v2);
461 
462  // If neither vertex was added or already exists, return false
463  if (index1 == INVALID_INDEX && index2 == INVALID_INDEX)
464  return false;
465 
466  // Only add the edge if both vertices exist
467  if (index1 != INVALID_INDEX && index2 != INVALID_INDEX)
468  return addEdge(index1, index2, edge, weight);
469 
470  return true;
471 }
472 
474 {
475  unsigned int index = vertexIndex(st);
476  if (index != INVALID_INDEX)
477  return removeVertex(index);
478  return false;
479 }
480 
481 bool ompl::base::PlannerData::removeVertex(unsigned int vIndex)
482 {
483  if (vIndex >= boost::num_vertices(*graph_))
484  return false;
485 
486  // Retrieve a list of all edge structures
487  boost::property_map<Graph::Type, edge_type_t>::type edgePropertyMap = get(edge_type_t(), *graph_);
488 
489  // Freeing memory associated with outgoing edges of this vertex
490  std::pair<Graph::OEIterator, Graph::OEIterator> oiterators =
491  boost::out_edges(boost::vertex(vIndex, *graph_), *graph_);
492  for (Graph::OEIterator iter = oiterators.first; iter != oiterators.second; ++iter)
493  delete edgePropertyMap[*iter];
494 
495  // Freeing memory associated with incoming edges of this vertex
496  std::pair<Graph::IEIterator, Graph::IEIterator> initerators =
497  boost::in_edges(boost::vertex(vIndex, *graph_), *graph_);
498  for (Graph::IEIterator iter = initerators.first; iter != initerators.second; ++iter)
499  delete edgePropertyMap[*iter];
500 
501  // Remove this vertex from stateIndexMap_, and update the map
502  stateIndexMap_.erase(getVertex(vIndex).getState());
503  boost::property_map<Graph::Type, vertex_type_t>::type vertices = get(vertex_type_t(), *graph_);
504  for (unsigned int i = vIndex + 1; i < boost::num_vertices(*graph_); ++i)
505  stateIndexMap_[vertices[boost::vertex(i, *graph_)]->getState()]--;
506 
507  // Remove this vertex from the start and/or goal index list, if it exists. Update the lists.
508  auto it = std::find(startVertexIndices_.begin(), startVertexIndices_.end(), vIndex);
509  if (it != startVertexIndices_.end())
510  startVertexIndices_.erase(it);
511  for (unsigned int &startVertexIndex : startVertexIndices_)
512  if (startVertexIndex > vIndex)
513  startVertexIndex--;
514 
515  it = std::find(goalVertexIndices_.begin(), goalVertexIndices_.end(), vIndex);
516  if (it != goalVertexIndices_.end())
517  goalVertexIndices_.erase(it);
518  for (unsigned int &goalVertexIndex : goalVertexIndices_)
519  if (goalVertexIndex > vIndex)
520  goalVertexIndex--;
521 
522  // If the state attached to this vertex was decoupled, free it here
523  auto *vtxState = const_cast<State *>(getVertex(vIndex).getState());
524  if (decoupledStates_.find(vtxState) != decoupledStates_.end())
525  {
526  decoupledStates_.erase(vtxState);
527  si_->freeState(vtxState);
528  vtxState = nullptr;
529  }
530 
531  // Slay the vertex
532  boost::clear_vertex(boost::vertex(vIndex, *graph_), *graph_);
533  boost::property_map<Graph::Type, vertex_type_t>::type vertexTypeMap = get(vertex_type_t(), *graph_);
534  delete vertexTypeMap[boost::vertex(vIndex, *graph_)];
535  boost::remove_vertex(boost::vertex(vIndex, *graph_), *graph_);
536 
537  return true;
538 }
539 
540 bool ompl::base::PlannerData::removeEdge(unsigned int v1, unsigned int v2)
541 {
542  Graph::Edge e;
543  bool exists;
544  boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
545 
546  if (!exists)
547  return false;
548 
549  // Freeing memory associated with this edge
550  boost::property_map<Graph::Type, edge_type_t>::type edges = get(edge_type_t(), *graph_);
551  delete edges[e];
552 
553  boost::remove_edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
554  return true;
555 }
556 
558 {
559  unsigned int index1, index2;
560  index1 = vertexIndex(v1);
561  index2 = vertexIndex(v2);
562 
563  if (index1 == INVALID_INDEX || index2 == INVALID_INDEX)
564  return false;
565 
566  return removeEdge(index1, index2);
567 }
568 
570 {
571  std::map<const State *, unsigned int>::const_iterator it = stateIndexMap_.find(st);
572  if (it != stateIndexMap_.end())
573  {
574  getVertex(it->second).setTag(tag);
575  return true;
576  }
577  return false;
578 }
579 
581 {
582  // Find the index in the stateIndexMap_
583  std::map<const State *, unsigned int>::const_iterator it = stateIndexMap_.find(st);
584  if (it != stateIndexMap_.end())
585  {
586  if (!isStartVertex(it->second))
587  {
588  startVertexIndices_.push_back(it->second);
589  // Sort the indices for quick lookup
590  std::sort(startVertexIndices_.begin(), startVertexIndices_.end());
591  }
592  return true;
593  }
594  return false;
595 }
596 
598 {
599  // Find the index in the stateIndexMap_
600  std::map<const State *, unsigned int>::const_iterator it = stateIndexMap_.find(st);
601  if (it != stateIndexMap_.end())
602  {
603  if (!isGoalVertex(it->second))
604  {
605  goalVertexIndices_.push_back(it->second);
606  // Sort the indices for quick lookup
607  std::sort(startVertexIndices_.begin(), startVertexIndices_.end());
608  }
609  return true;
610  }
611  return false;
612 }
613 
615 {
616  unsigned int nv = numVertices();
617  for (unsigned int i = 0; i < nv; ++i)
618  {
619  std::map<unsigned int, const PlannerDataEdge *> nbrs;
620  getEdges(i, nbrs);
621 
622  std::map<unsigned int, const PlannerDataEdge *>::const_iterator it;
623  for (it = nbrs.begin(); it != nbrs.end(); ++it)
624  {
625  setEdgeWeight(i, it->first, opt.motionCost(getVertex(i).getState(), getVertex(it->first).getState()));
626  }
627  }
628 }
629 
631 {
632  // Create a PathLengthOptimizationObjective to compute the edge
633  // weights according to state space distance
635  computeEdgeWeights(opt);
636 }
637 
639  base::PlannerData &mst) const
640 {
641  std::vector<ompl::base::PlannerData::Graph::Vertex> pred(numVertices());
642 
643  // This is how boost's minimum spanning tree is actually
644  // implemented, except it lacks the generality for specifying our
645  // own comparison function or zero/inf values.
646  //
647  // \todo Once (https://svn.boost.org/trac/boost/ticket/9368) gets
648  // into boost we can use the far more direct
649  // boost::prim_minimum_spanning_tree().
650  boost::dijkstra_shortest_paths(*graph_, v, boost::predecessor_map(&pred[0])
651  .distance_compare([&opt](Cost c1, Cost c2)
652  {
653  return opt.isCostBetterThan(c1, c2);
654  })
655  .distance_combine([](Cost, Cost c)
656  {
657  return c;
658  })
659  .distance_inf(opt.infiniteCost())
660  .distance_zero(opt.identityCost()));
661 
662  // Adding vertices to MST
663  for (std::size_t i = 0; i < pred.size(); ++i)
664  {
665  if (isStartVertex(i))
666  mst.addStartVertex(getVertex(i));
667  else if (isGoalVertex(i))
668  mst.addGoalVertex(getVertex(i));
669  else
670  mst.addVertex(getVertex(i));
671  }
672 
673  // Adding edges to MST
674  for (std::size_t i = 0; i < pred.size(); ++i)
675  {
676  if (pred[i] != i)
677  {
678  Cost c;
679  getEdgeWeight(pred[i], i, &c);
680  mst.addEdge(pred[i], i, getEdge(pred[i], i), c);
681  }
682  }
683 }
684 
686 {
687  // If this vertex already exists in data, return
688  if (data.vertexExists(getVertex(v)))
689  return;
690 
691  // Adding the vertex corresponding to v into data
692  unsigned int idx;
693  if (isStartVertex(v))
694  idx = data.addStartVertex(getVertex(v));
695  else if (isGoalVertex(v))
696  idx = data.addGoalVertex(getVertex(v));
697  else
698  idx = data.addVertex(getVertex(v));
699 
700  assert(idx != INVALID_INDEX);
701 
702  std::map<unsigned int, const PlannerDataEdge *> neighbors;
703  getEdges(v, neighbors);
704 
705  // Depth-first traversal of reachable graph
706  std::map<unsigned int, const PlannerDataEdge *>::iterator it;
707  for (auto &it : neighbors)
708  {
709  extractReachable(it.first, data);
710  Cost weight;
711  getEdgeWeight(v, it.first, &weight);
712  data.addEdge(idx, data.vertexIndex(getVertex(it.first)), *it.second, weight);
713  }
714 }
715 
716 ompl::base::StateStoragePtr ompl::base::PlannerData::extractStateStorage() const
717 {
718  auto store(std::make_shared<GraphStateStorage>(si_->getStateSpace()));
719  if (graph_)
720  {
721  // copy the states
722  std::map<unsigned int, unsigned int> indexMap;
723  for (const auto &it : stateIndexMap_)
724  {
725  indexMap[it.second] = store->size();
726  store->addState(it.first);
727  }
728 
729  // add the edges
730  for (const auto &it : indexMap)
731  {
732  std::vector<unsigned int> edgeList;
733  getEdges(it.first, edgeList);
734  GraphStateStorage::MetadataType &md = store->getMetadata(it.second);
735  md.resize(edgeList.size());
736  // map node indices to index values in StateStorage
737  for (std::size_t k = 0; k < edgeList.size(); ++k)
738  md[k] = indexMap[edgeList[k]];
739  }
740  }
741  return store;
742 }
743 
745 {
746  auto *boostgraph = reinterpret_cast<ompl::base::PlannerData::Graph *>(graphRaw_);
747  return *boostgraph;
748 }
749 
751 {
752  const auto *boostgraph =
753  reinterpret_cast<const ompl::base::PlannerData::Graph *>(graphRaw_);
754  return *boostgraph;
755 }
756 
758 {
759  return si_;
760 }
761 
762 void ompl::base::PlannerData::freeMemory()
763 {
764  // Freeing decoupled states, if any
765  for (auto decoupledState : decoupledStates_)
766  si_->freeState(decoupledState);
767 
768  if (graph_)
769  {
770  std::pair<Graph::EIterator, Graph::EIterator> eiterators = boost::edges(*graph_);
771  boost::property_map<Graph::Type, edge_type_t>::type edges = get(edge_type_t(), *graph_);
772  for (Graph::EIterator iter = eiterators.first; iter != eiterators.second; ++iter)
773  delete boost::get(edges, *iter);
774 
775  std::pair<Graph::VIterator, Graph::VIterator> viterators = boost::vertices(*graph_);
776  boost::property_map<Graph::Type, vertex_type_t>::type vertices = get(vertex_type_t(), *graph_);
777  for (Graph::VIterator iter = viterators.first; iter != viterators.second; ++iter)
778  delete vertices[*iter];
779 
780  graph_->clear();
781  }
782 }
783 
785 {
786  return false;
787 }
788 
789 void ompl::base::PlannerData::printPLY(std::ostream &out, const bool asIs) const
790 {
791  const base::StateSpace *space(si_->getStateSpace().get());
792 
793  unsigned int dim = space->getDimension();
794  if (dim > 3)
795  throw Exception("Cannot output mesh of path in more than 3 dimensions!");
796 
797  std::vector<double> reals;
798  std::stringstream v, f;
799  std::size_t vcount = 0;
800  std::size_t fcount = 0;
801 
802  auto stateOutput = [&](const ompl::base::State *state) {
803  space->copyToReals(reals, state);
804  std::copy(reals.begin(), reals.end(), std::ostream_iterator<double>(v, " "));
805  v << std::endl;
806  };
807 
808  const Graph &graph = toBoostGraph();
809 
810  BGL_FORALL_EDGES(edge, graph, PlannerData::Graph)
811  {
812  std::vector<ompl::base::State *> stateList;
813  const State *source = boost::get(vertex_type, graph, boost::source(edge, graph))->getState();
814  const State *target = boost::get(vertex_type, graph, boost::target(edge, graph))->getState();
815 
816  unsigned int n = 0;
817  if (!asIs)
818  n = si_->getStateSpace()->validSegmentCount(source, target);
819  si_->getMotionStates(source, target, stateList, n, true, true);
820 
821  stateOutput(stateList[0]);
822  vcount++;
823  for (std::size_t i = 1; i < stateList.size(); i++)
824  {
825  stateOutput(stateList[i]);
826  stateOutput(stateList[i - 1]);
827  vcount += 2;
828  f << 3 << " " << vcount - 3 << " " << vcount - 2 << " " << vcount - 1 << "\n";
829  fcount++;
830  si_->freeState(stateList[i - 1]);
831  }
832  si_->freeState(stateList.back());
833  }
834 
835  out << "ply\n";
836  out << "format ascii 1.0\n";
837  out << "element vertex " << vcount << "\n";
838  out << "property float x\n";
839 
840  if (dim > 1)
841  out << "property float y\n";
842 
843  if (dim > 2)
844  out << "property float z\n";
845 
846  out << "element face " << fcount << "\n";
847  out << "property list uint uint vertex_index\n";
848  out << "end_header\n";
849  out << v.str() << f.str();
850 }
bool edgeExists(unsigned int v1, unsigned int v2) const
Check whether an edge between vertex index v1 and index v2 exists.
unsigned int numGoalVertices() const
Returns the number of goal vertices.
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...
virtual Cost identityCost() const
Get the identity cost value. The identity cost value is the cost c_i such that, for all costs c,...
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....
virtual bool removeEdge(unsigned int v1, unsigned int v2)
Removes the edge between vertex indexes v1 and v2. Success is returned.
A shared pointer wrapper for ompl::base::SpaceInformation.
virtual PlannerDataEdge * clone() const
Return a clone of this object, allocated from the heap.
Definition: PlannerData.h:196
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)....
Representation of a space in which planning can be performed. Topology specific sampling,...
Definition: StateSpace.h:134
const SpaceInformationPtr & getSpaceInformation() const
Return the instance of SpaceInformation used in this PlannerData.
bool isStartVertex(unsigned int index) const
Returns true if the given vertex index is marked as a start vertex.
Wrapper class for the Boost.Graph representation of the PlannerData. This class inherits from a boost...
An optimization objective which corresponds to optimizing path length.
bool isGoalVertex(unsigned int index) const
Returns true if the given vertex index is marked as a goal vertex.
static const PlannerDataEdge NO_EDGE
Representation for a non-existant edge.
Definition: PlannerData.h:241
Definition of an abstract state.
Definition: State.h:113
virtual PlannerDataVertex * clone() const
Return a clone of this object, allocated from the heap.
Definition: PlannerData.h:182
bool vertexExists(const PlannerDataVertex &v) const
Check whether a vertex exists with the given vertex data.
void printPLY(std::ostream &out, bool asIs=false) const
Write a mesh of the planner graph to a stream. Insert additional vertices if asIs == true.
virtual Cost infiniteCost() const
Get a cost which is greater than all other costs in this OptimizationObjective; required for use in D...
virtual void copyToReals(std::vector< double > &reals, const State *source) const
Copy all the real values from a state source to the array reals using getValueAddressAtLocation()
Definition: StateSpace.cpp:329
M MetadataType
the datatype of the metadata
Definition: StateStorage.h:282
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:111
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,...
virtual void decoupleFromPlanner()
Creates a deep copy of the states contained in the vertices of this PlannerData structure so that whe...
Definition: PlannerData.cpp:80
unsigned int numEdges() const
Retrieve the number of edges in this structure.
virtual unsigned int getDimension() const =0
Get the dimension of the space (not the dimension of the surrounding ambient space)
StateStoragePtr extractStateStorage() const
Extract a ompl::base::GraphStateStorage object from this PlannerData. Memory for states is copied (th...
boost::graph_traits< Type >::vertex_descriptor Vertex
Boost.Graph vertex descriptor.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:238
static const unsigned int INVALID_INDEX
Representation of an invalid vertex index.
Definition: PlannerData.h:248
Abstract definition of optimization objectives.
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.
unsigned int numStartVertices() const
Returns the number of start vertices.
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 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...
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...
virtual Cost motionCost(const State *s1, const State *s2) const =0
Get the cost that corresponds to the motion segment between s1 and s2.
bool markGoalState(const State *st)
Mark the given state as a goal vertex. If the given state does not exist in a vertex,...
void printGraphviz(std::ostream &out=std::cout) const
Writes a Graphviz dot file of this structure to the given stream.
void extractReachable(unsigned int v, PlannerData &data) const
Extracts the subset of PlannerData reachable from the vertex with index v. For tree structures,...
const PlannerDataVertex & getVertex(unsigned int index) const
Retrieve a reference to the vertex object with the given index. If this vertex does not exist,...
virtual const State * getState() const
Retrieve the state associated with this vertex.
Definition: PlannerData.h:176
bool markStartState(const State *st)
Mark the given state as a start vertex. If the given state does not exist in a vertex,...
Base class for a PlannerData edge.
Definition: PlannerData.h:190
virtual bool removeVertex(const PlannerDataVertex &st)
Removes the vertex associated with the given data. If the vertex does not exist, false is returned....
virtual bool isCostBetterThan(Cost c1, Cost c2) const
Check whether the the cost c1 is considered better than the cost c2. By default, this returns true if...
unsigned int addVertex(const PlannerDataVertex &st)
Adds the given vertex to the graph data. The vertex index is returned. Duplicates are not added....
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,...
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...
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...
virtual void clear()
Clears the entire data structure.
Definition: PlannerData.cpp:74
void computeEdgeWeights()
Computes all edge weights using state space distance (i.e. getSpaceInformation()->distance())
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...
const State * state_
The state represented by this vertex.
Definition: PlannerData.h:213
static const PlannerDataVertex NO_VERTEX
Representation for a non-existant vertex.
Definition: PlannerData.h:246
boost::graph_traits< Type >::edge_descriptor Edge
Boost.Graph edge descriptor.
Definition of a scoped state.
Definition: ScopedState.h:120
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...
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...
The exception type for ompl.
Definition: Exception.h:78
Graph & toBoostGraph()
Extract a Boost.Graph object from this PlannerData.
PlannerDataGraph Type
Data type for the Boost.Graph representation.
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:122
virtual ~PlannerData()
Destructor.
Definition: PlannerData.cpp:63
virtual bool hasControls() const
Indicate whether any information about controls (ompl::control::Control) is stored in this instance.
const PlannerDataEdge & getEdge(unsigned int v1, unsigned int v2) const
Retrieve a reference to the edge object connecting vertices with indexes v1 and v2....
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...