AdjacencyList.cpp
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2015, 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/datastructures/AdjacencyList.h"
38 #include "ompl/datastructures/BinaryHeap.h"
39 #include "ompl/util/Console.h"
40 #include <boost/graph/adjacency_list.hpp>
41 #include <boost/graph/graph_traits.hpp>
42 #include <boost/graph/dijkstra_shortest_paths.hpp>
43 #include <boost/pending/disjoint_sets.hpp>
44 
45 #include <boost/graph/incremental_components.hpp> // needed?
46 
47 #include <iostream>
48 
49 using Graph = boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS,
50  boost::property<boost::vertex_index_t, int,
51  boost::property<boost::vertex_rank_t, int, // disjoint sets
52  boost::property<boost::vertex_predecessor_t, int>>>,
53  boost::property<boost::edge_weight_t, double>>;
54 
55 using Vertex = boost::graph_traits<Graph>::vertex_descriptor;
56 using Edge = boost::graph_traits<Graph>::edge_descriptor;
57 using VIterator = boost::graph_traits<Graph>::vertex_iterator;
58 using EIterator = boost::graph_traits<Graph>::edge_iterator;
59 using AdjIterator = boost::graph_traits<Graph>::adjacency_iterator;
60 
61 using DisjointSets = boost::disjoint_sets<boost::property_map<Graph, boost::vertex_rank_t>::type,
62  boost::property_map<Graph, boost::vertex_predecessor_t>::type>;
63 
64 #define graph_ reinterpret_cast<Graph *>(graphRaw_)
65 #define disjointSets_ reinterpret_cast<DisjointSets *>(disjointSetsRaw_)
66 
67 ompl::AdjacencyList::AdjacencyList()
68 {
69  graphRaw_ = new Graph();
70  disjointSetsRaw_ =
71  new DisjointSets(boost::get(boost::vertex_rank, *graph_), boost::get(boost::vertex_predecessor, *graph_));
72 }
73 
74 ompl::AdjacencyList::AdjacencyList(int n)
75 {
76  graphRaw_ = new Graph(n);
77  disjointSetsRaw_ =
78  new DisjointSets(boost::get(boost::vertex_rank, *graph_), boost::get(boost::vertex_predecessor, *graph_));
79 
80  std::pair<VIterator, VIterator> vRange = boost::vertices(*graph_);
81  for (VIterator it = vRange.first; it != vRange.second; ++it)
82  disjointSets_->make_set(*it);
83 }
84 
85 ompl::AdjacencyList::~AdjacencyList()
86 {
87  delete graph_;
88  delete disjointSets_;
89  graphRaw_ = NULL;
90  disjointSetsRaw_ = NULL;
91 }
92 
93 void ompl::AdjacencyList::clear()
94 {
95  boost::mutex::scoped_lock lock(lock_);
96  graph_->clear();
97 }
98 
99 int ompl::AdjacencyList::addVertex()
100 {
101  boost::mutex::scoped_lock lock(lock_);
102  Vertex v = boost::add_vertex(*graph_);
103  disjointSets_->make_set(v);
104 
105  boost::property_map<Graph, boost::vertex_index_t>::type vertexIndexMap = get(boost::vertex_index, *graph_);
106  return vertexIndexMap[v];
107 }
108 
109 int ompl::AdjacencyList::numVertices() const
110 {
111  return boost::num_vertices(*graph_);
112 }
113 
114 bool ompl::AdjacencyList::vertexExists(int v) const
115 {
116  return v >= 0 && v < numVertices();
117 }
118 
119 bool ompl::AdjacencyList::inSameComponent(int v1, int v2) const
120 {
121  if (!vertexExists(v1) || !vertexExists(v2))
122  return false;
123 
124  return boost::same_component(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *disjointSets_);
125 }
126 
127 int ompl::AdjacencyList::numConnectedComponents() const
128 {
129  std::pair<VIterator, VIterator> vRange = boost::vertices(*graph_);
130  return disjointSets_->count_sets(vRange.first, vRange.second);
131 }
132 
133 int ompl::AdjacencyList::getComponentID(int vtx) const
134 {
135  return disjointSets_->find_set(vtx);
136 }
137 
138 bool ompl::AdjacencyList::addEdge(int v1, int v2, double weight)
139 {
140  boost::mutex::scoped_lock lock(lock_);
141 
142  // If either of the vertices do not exist, don't add an edge
143  if (v1 < 0 || v1 >= numVertices() || v2 < 0 || v2 >= numVertices())
144  return false;
145 
146  if (edgeExists(v1, v2))
147  return false;
148 
149  // No self-transitions
150  if (v1 == v2)
151  return false;
152 
153  if (weight < 0)
154  {
155  std::cout << "weight = " << weight << std::endl;
156  throw std::runtime_error("addEdge: Edge weight must be >= 0");
157  }
158 
159  Edge e;
160  bool added = false;
161  Vertex vt1 = boost::vertex(v1, *graph_);
162  Vertex vt2 = boost::vertex(v2, *graph_);
163  tie(e, added) = boost::add_edge(vt1, vt2, *graph_);
164 
165  assert(added); // this should never fail
166 
167  // Set edge weight
168  boost::property_map<Graph, boost::edge_weight_t>::type weights = get(boost::edge_weight, *graph_);
169  weights[e] = weight;
170 
171  disjointSets_->union_set(vt1, vt2);
172 
173  return added;
174 }
175 
176 bool ompl::AdjacencyList::removeEdge(int v1, int v2)
177 {
178  boost::mutex::scoped_lock lock(lock_);
179 
180  Edge e;
181  bool exists;
182  boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
183 
184  if (exists)
185  boost::remove_edge(e, *graph_);
186  return exists;
187 }
188 
189 int ompl::AdjacencyList::numEdges() const
190 {
191  return boost::num_edges(*graph_);
192 }
193 
194 double ompl::AdjacencyList::getEdgeWeight(int v1, int v2) const
195 {
196  Edge e;
197  bool exists;
198  boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
199 
200  if (exists)
201  {
202  boost::property_map<Graph, boost::edge_weight_t>::type weights = get(boost::edge_weight, *graph_);
203  return weights[e];
204  }
205 
206  throw std::runtime_error("Edge does not exist");
207 }
208 
209 bool ompl::AdjacencyList::setEdgeWeight(int v1, int v2, double weight)
210 {
211  boost::mutex::scoped_lock lock(lock_);
212 
213  if (weight < 0)
214  {
215  std::cout << "Weight: " << weight << std::endl;
216  throw std::runtime_error("setEdgeWeight: Edge weight must be >= 0");
217  }
218 
219  Edge e;
220  bool exists;
221  boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
222 
223  if (exists)
224  {
225  boost::property_map<Graph, boost::edge_weight_t>::type weights = get(boost::edge_weight, *graph_);
226  weights[e] = weight;
227  return true;
228  }
229 
230  return false;
231 }
232 
233 bool ompl::AdjacencyList::edgeExists(int v1, int v2) const
234 {
235  Edge e;
236  bool exists;
237  boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);
238 
239  return exists;
240 }
241 
242 int ompl::AdjacencyList::numNeighbors(int vtx) const
243 {
244  return boost::degree(boost::vertex(vtx, *graph_), *graph_);
245 }
246 
247 void ompl::AdjacencyList::getNeighbors(int vtx, std::vector<int> &nbrs) const
248 {
249  nbrs.resize(numNeighbors(vtx));
250 
251  std::pair<AdjIterator, AdjIterator> iterators = boost::adjacent_vertices(boost::vertex(vtx, *graph_), *graph_);
252  boost::property_map<Graph, boost::vertex_index_t>::type vertices = get(boost::vertex_index, *graph_);
253  int count = 0;
254  for (AdjIterator iter = iterators.first; iter != iterators.second; ++iter, ++count)
255  nbrs[count] = vertices[*iter];
256 }
257 
258 void ompl::AdjacencyList::getNeighbors(int vtx, std::vector<std::pair<int, double>> &nbrs) const
259 {
260  nbrs.resize(numNeighbors(vtx));
261 
262  std::pair<AdjIterator, AdjIterator> iterators = boost::adjacent_vertices(boost::vertex(vtx, *graph_), *graph_);
263  boost::property_map<Graph, boost::vertex_index_t>::type vertices = get(boost::vertex_index, *graph_);
264  boost::property_map<Graph, boost::edge_weight_t>::type weights = get(boost::edge_weight, *graph_);
265 
266  int count = 0;
267  for (AdjIterator iter = iterators.first; iter != iterators.second; ++iter, ++count)
268  {
269  Edge e;
270  bool exists;
271  boost::tie(e, exists) = boost::edge(boost::vertex(vtx, *graph_), *iter, *graph_);
272  assert(exists);
273  nbrs[count] = std::make_pair(vertices[*iter], weights[e]);
274  }
275 }
276 
277 bool ompl::AdjacencyList::dijkstra(int v1, int v2, std::vector<int> &path) const
278 {
279  std::vector<double> distances;
280  std::vector<int> predecessors;
281  dijkstra(v1, predecessors, distances);
282 
283  // no solution when predecessor of (non-start) vertex is itself
284  if (v2 != v1 && predecessors[v2] == v2)
285  return false;
286 
287  boost::property_map<Graph, boost::vertex_index_t>::type indexMap = get(boost::vertex_index, *graph_);
288 
289  path.clear();
290  // Extracting solution path
291  Vertex v = boost::vertex(v2, *graph_);
292  Vertex start = boost::vertex(v1, *graph_);
293  while (v != start)
294  {
295  path.insert(path.begin(), indexMap[v]);
296  v = predecessors[v];
297  }
298  path.insert(path.begin(), indexMap[v]);
299  return true;
300 }
301 
302 void ompl::AdjacencyList::dijkstra(int vtx, std::vector<int> &predecessors, std::vector<double> &distances) const
303 {
304  // Locking the graph, since modifications to the graph during search are not supported
305  // This call is also the reason why lock_ must be mutable
306  boost::mutex::scoped_lock lock(lock_);
307 
308  distances.resize(numVertices());
309  predecessors.resize(numVertices());
310 
311  boost::dijkstra_shortest_paths(*graph_, boost::vertex(vtx, *graph_),
312  boost::predecessor_map(&predecessors[0]).distance_map(&distances[0]));
313 }