Loading...
Searching...
No Matches
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
56const unsigned int ompl::base::PlannerData::INVALID_INDEX = std::numeric_limits<unsigned int>::max();
57
58ompl::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
104unsigned 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
117unsigned 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
131unsigned 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
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
157bool 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
173bool 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
188bool 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
230const 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
260void ompl::base::PlannerData::printGraphviz(std::ostream &out) const
261{
262 boost::write_graphviz(out, *graph_);
263}
264
265namespace
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,
269 ompl::base::PlannerData::Graph::Vertex v)
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
284void 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?
293 using Edge = ompl::base::PlannerData::Graph::Edge;
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 });
300 using Vertex = ompl::base::PlannerData::Graph::Vertex;
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
333unsigned 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
341unsigned 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
349bool ompl::base::PlannerData::isStartVertex(unsigned int index) const
350{
351 return std::binary_search(startVertexIndices_.begin(), startVertexIndices_.end(), index);
352}
353
354bool 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
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
432bool 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
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
540bool 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
716ompl::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
761
762void 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
789void 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}
The exception type for ompl.
Definition Exception.h:47
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition Cost.h:48
Abstract definition of optimization objectives.
virtual Cost identityCost() const
Get the identity cost value. The identity cost value is the cost c_i such that, for all costs c,...
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...
virtual Cost motionCost(const State *s1, const State *s2) const =0
Get the cost that corresponds to the motion segment between s1 and s2.
virtual Cost infiniteCost() const
Get a cost which is greater than all other costs in this OptimizationObjective; required for use in D...
An optimization objective which corresponds to optimizing path length.
Base class for a PlannerData edge.
virtual PlannerDataEdge * clone() const
Return a clone of this object, allocated from the heap.
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition PlannerData.h:59
const State * state_
The state represented by this vertex.
virtual PlannerDataVertex * clone() const
Return a clone of this object, allocated from the heap.
Definition PlannerData.h:86
virtual const State * getState() const
Retrieve the state associated with this vertex.
Definition PlannerData.h:80
Wrapper class for the Boost.Graph representation of the PlannerData. This class inherits from a boost...
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
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 vertexIndex(const PlannerDataVertex &v) const
Return the index for the vertex associated with the given data. INVALID_INDEX is returned if this ver...
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...
bool edgeExists(unsigned int v1, unsigned int v2) const
Check whether an edge between vertex index v1 and index v2 exists.
bool isGoalVertex(unsigned int index) const
Returns true if the given vertex index is marked as a goal vertex.
void printGraphviz(std::ostream &out=std::cout) const
Writes a Graphviz dot file of this structure to the given stream.
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....
void computeEdgeWeights()
Computes all edge weights using state space distance (i.e. getSpaceInformation()->distance())
const SpaceInformationPtr & getSpaceInformation() const
Return the instance of SpaceInformation used in this PlannerData.
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 addStartVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
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...
unsigned int numEdges() const
Retrieve the number of edges in this structure.
StateStoragePtr extractStateStorage() const
Extract a ompl::base::GraphStateStorage object from this PlannerData. Memory for states is copied (th...
bool markStartState(const State *st)
Mark the given state as a start vertex. If the given state does not exist in a vertex,...
static const PlannerDataVertex NO_VERTEX
Representation for a non-existant vertex.
static const PlannerDataEdge NO_EDGE
Representation for a non-existant edge.
virtual void clear()
Clears the entire data structure.
virtual bool removeVertex(const PlannerDataVertex &st)
Removes the vertex associated with the given data. If the vertex does not exist, false 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,...
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...
unsigned int numVertices() const
Retrieve the number of vertices in this structure.
Graph & toBoostGraph()
Extract a Boost.Graph object from this PlannerData.
virtual bool removeEdge(unsigned int v1, unsigned int v2)
Removes the edge between vertex indexes v1 and v2. Success is returned.
unsigned int numStartVertices() const
Returns the number of start vertices.
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)....
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...
const PlannerDataEdge & getEdge(unsigned int v1, unsigned int v2) const
Retrieve a reference to the edge object connecting vertices with indexes v1 and v2....
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.
static const unsigned int INVALID_INDEX
Representation of an invalid vertex index.
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...
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...
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 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...
virtual bool hasControls() const
Indicate whether any information about controls (ompl::control::Control) is stored in this instance.
void extractReachable(unsigned int v, PlannerData &data) const
Extracts the subset of PlannerData reachable from the vertex with index v. For tree structures,...
unsigned int addVertex(const PlannerDataVertex &st)
Adds the given vertex to the graph data. The vertex index is returned. Duplicates are not added....
bool vertexExists(const PlannerDataVertex &v) const
Check whether a vertex exists with the given vertex data.
unsigned int numGoalVertices() const
Returns the number of goal vertices.
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 printGraphML(std::ostream &out=std::cout) const
Writes a GraphML file of this structure to the given stream.
bool isStartVertex(unsigned int index) const
Returns true if the given vertex index is marked as a start vertex.
virtual ~PlannerData()
Destructor.
virtual void decoupleFromPlanner()
Creates a deep copy of the states contained in the vertices of this PlannerData structure so that whe...
Definition of a scoped state.
Definition ScopedState.h:57
A shared pointer wrapper for ompl::base::SpaceInformation.
Representation of a space in which planning can be performed. Topology specific sampling,...
Definition StateSpace.h:71
virtual unsigned int getDimension() const =0
Get the dimension of the space (not the dimension of the surrounding ambient space)
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()
M MetadataType
the datatype of the metadata
Definition of an abstract state.
Definition State.h:50
STL namespace.