LPAstarOnGraph.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2015, Tel Aviv 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 Tel Aviv 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: Oren Salzman */
36 /* Implementation based on
37 Sven Koenig, Maxim Likhachev, David Furcy:
38 Lifelong Planning A. Artif. Intell. 155(1-2): 93-146 (2004)
39 */
40 
41 #ifndef OMPL_DATASTRUCTURES_LPA_STAR_ON_G_H
42 #define OMPL_DATASTRUCTURES_LPA_STAR_ON_G_H
43 
44 #include <vector>
45 #include <limits>
46 #include <set>
47 #include <map>
48 #include <list>
49 #include <unordered_map>
50 
51 #include <iterator>
52 #include <iostream>
53 #include <cassert>
54 
55 // workaround for bug in Boost 1.60; see https://svn.boost.org/trac/boost/ticket/11880
56 #include <boost/version.hpp>
57 #if BOOST_VERSION > 105900
58 #include <boost/type_traits/ice.hpp>
59 #endif
60 
61 #include <boost/graph/adjacency_matrix.hpp>
62 #include <boost/graph/adjacency_list.hpp>
63 
64 namespace ompl
65 {
66  // Data is of type std::size_t
67  template <typename Graph, // Boost graph
68  typename Heuristic> // heuristic to estimate cost
70  {
71  public:
72  LPAstarOnGraph(std::size_t source, std::size_t target, Graph &graph, Heuristic &h)
73  : costEstimator_(h), graph_(graph)
74  {
75  // initialization
76  double c = std::numeric_limits<double>::infinity();
77  source_ = new Node(c, costEstimator_(source), 0, source);
78  addNewNode(source_);
79  target_ = new Node(c, 0, c, target);
80  addNewNode(target_);
81  insertQueue(source_);
82  }
84  {
85  clear();
86  }
87 
88  void insertEdge(std::size_t u, std::size_t v, double c)
89  {
90  Node *n_u = getNode(u);
91  Node *n_v = getNode(v);
92 
93  if (n_v->rhs() > n_u->costToCome() + c)
94  {
95  n_v->setParent(n_u);
96  n_v->setRhs(n_u->costToCome() + c);
97  updateVertex(n_v);
98  }
99  }
100  void removeEdge(std::size_t u, std::size_t v)
101  {
102  assert(v != source_->getId());
103 
104  Node *n_u = getNode(u);
105  Node *n_v = getNode(v);
106 
107  if (n_v->getParent() == n_u)
108  {
109  WeightMap weights = boost::get(boost::edge_weight_t(), graph_);
110  chooseBestIncomingNode(n_v, weights);
111  }
112 
113  updateVertex(n_v);
114  }
115  double computeShortestPath(std::list<std::size_t> &path)
116  {
117  WeightMap weights = boost::get(boost::edge_weight_t(), graph_);
118 
119  if (queue_.empty())
120  return std::numeric_limits<double>::infinity();
121 
122  while (topHead()->key() < target_->calculateKey() || target_->rhs() != target_->costToCome())
123  {
124  // pop from queue and process
125  Node *u = topHead();
126 
127  if (u->costToCome() > u->rhs()) // the node is overconsistent
128  {
129  u->setCostToCome(u->rhs());
130  popHead();
131 
132  // iterate over all (outgoing) neighbors of the node and get the best parent for each one
133  typename boost::graph_traits<Graph>::out_edge_iterator ei, ei_end;
134  for (boost::tie(ei, ei_end) = boost::out_edges(u->getId(), graph_); ei != ei_end; ++ei)
135  {
136  std::size_t v = boost::target(*ei, graph_);
137  Node *n_v = getNode(v);
138  double c = boost::get(weights, *ei); // edge weight from u to v
139 
140  if (n_v->rhs() > u->costToCome() + c)
141  {
142  n_v->setParent(u);
143  n_v->setRhs(u->costToCome() + c);
144  updateVertex(n_v);
145  }
146  }
147  }
148  else // (n->costToCome() < n->rhs()) // the node is underconsistent
149  {
150  u->setCostToCome(std::numeric_limits<double>::infinity());
151  updateVertex(u);
152 
153  // get all (outgoing) neighbors of the node
154  typename boost::graph_traits<Graph>::out_edge_iterator ei, ei_end;
155  for (boost::tie(ei, ei_end) = boost::out_edges(u->getId(), graph_); ei != ei_end; ++ei)
156  {
157  std::size_t v = boost::target(*ei, graph_);
158  Node *n_v = getNode(v);
159 
160  if ((n_v == source_) || (n_v->getParent() != u))
161  continue;
162 
163  chooseBestIncomingNode(n_v, weights);
164  updateVertex(n_v);
165  }
166  }
167 
168  if (queue_.empty())
169  break;
170  }
171 
172  // now get path
173  Node *res = (target_->costToCome() == std::numeric_limits<double>::infinity() ? nullptr : target_);
174  while (res != nullptr)
175  {
176  path.push_front(res->getId());
177  res = res->getParent();
178  }
179 
180  return target_->costToCome();
181  }
182 
184  double operator()(std::size_t u)
185  {
186  auto iter = idNodeMap_.find(u);
187  if (iter != idNodeMap_.end())
188  return iter->second->costToCome();
189  return std::numeric_limits<double>::infinity();
190  }
191 
192  private:
193  struct Key
194  {
195  Key(double first_ = -1, double second_ = -1) : first(first_), second(second_)
196  {
197  }
198  bool operator<(const Key &other)
199  {
200  return (first != other.first) ? (first < other.first) : (second < other.second);
201  }
202  double first, second;
203  };
204 
205  class Node
206  {
207  public:
208  Node(double costToCome, double costToGo, double rhs, std::size_t &dataId, Node *parentNode = nullptr)
209  : g(costToCome), h(costToGo), r(rhs), isInQ(false), parent(parentNode), id(dataId)
210  {
211  calculateKey();
212  }
213  // cost accesors
214  double costToCome() const
215  {
216  return g;
217  }
218  double costToGo() const
219  {
220  return h;
221  }
222  double rhs() const
223  {
224  return r;
225  }
226  Key key() const
227  {
228  return k;
229  }
230  Key calculateKey()
231  {
232  k = Key(std::min(g, r + h), std::min(g, r));
233  return k;
234  }
235  // cost modifiers
236  double setCostToCome(double val)
237  {
238  return g = val;
239  }
240  double setRhs(double val)
241  {
242  return r = val;
243  }
244  // is in queue field
245  bool isInQueue() const
246  {
247  return isInQ;
248  }
249  void inQueue(bool in)
250  {
251  isInQ = in;
252  }
253  // parent field
254  Node *getParent() const
255  {
256  return parent;
257  }
258  void setParent(Node *p)
259  {
260  parent = p;
261  }
262  // data field
263  std::size_t getId() const
264  {
265  return id;
266  }
267  bool isConsistent() const
268  {
269  return g == r;
270  }
271 
272  private:
273  double g; // cost to come
274  double h; // cost to go
275  double r; // rhs
276  Key k; // key
277  bool isInQ;
278  Node *parent;
279  std::size_t id; // unique data associated with node
280  }; // Node
281 
282  struct LessThanNodeK
283  {
284  bool operator()(const Node *n1, const Node *n2) const
285  {
286  return n1->key() < n2->key();
287  }
288  }; // LessThanNodeK
289 
290  struct Hash
291  {
292  std::size_t operator()(const std::size_t id) const
293  {
294  return h(id);
295  }
296  std::hash<std::size_t> h;
297  }; // Hash
298 
299  using Queue = std::multiset<Node *, LessThanNodeK>;
300  using IdNodeMap = std::unordered_map<std::size_t, Node *, Hash>;
301  using IdNodeMapIter = typename IdNodeMap::iterator;
302  using WeightMap = typename boost::property_map<Graph, boost::edge_weight_t>::type;
303 
304  // LPA* subprocedures
305  void updateVertex(Node *n)
306  {
307  if (!n->isConsistent())
308  {
309  if (n->isInQueue())
310  updateQueue(n);
311  else
312  insertQueue(n);
313  }
314  else if (n->isInQueue())
315  removeQueue(n);
316  }
317  // queue utils
318  Node *popHead()
319  {
320  Node *n = topHead();
321  n->inQueue(false);
322  queue_.erase(queue_.begin());
323 
324  return n;
325  }
326  Node *topHead()
327  {
328  return *queue_.begin();
329  }
330 
331  void insertQueue(Node *node)
332  {
333  assert(node->isInQueue() == false);
334 
335  node->calculateKey();
336  node->inQueue(true);
337  queue_.insert(node);
338  }
339  void removeQueue(Node *node)
340  {
341  if (node->isInQueue())
342  {
343  node->inQueue(false);
344  queue_.erase(node);
345  }
346  }
347  void updateQueue(Node *node)
348  {
349  removeQueue(node);
350  insertQueue(node);
351  }
352 
353  void chooseBestIncomingNode(Node *n_v, WeightMap &weights)
354  {
355  // iterate over all incoming neighbors of the node n_v and get the best parent
356  double min = std::numeric_limits<double>::infinity();
357  Node *best = nullptr;
358 
359  typename boost::graph_traits<Graph>::in_edge_iterator ei, ei_end;
360  for (boost::tie(ei, ei_end) = boost::in_edges(n_v->getId(), graph_); ei != ei_end; ++ei)
361  {
362  std::size_t u = boost::source(*ei, graph_);
363  Node *n_u = getNode(u);
364  double c = boost::get(weights, *ei); // edge weight from u to v
365 
366  double curr = n_u->costToCome() + c;
367  if (curr < min)
368  {
369  min = curr;
370  best = n_u;
371  }
372  }
373 
374  n_v->setRhs(min);
375  n_v->setParent(best);
376  }
377 
378  void addNewNode(Node *n)
379  {
380  idNodeMap_[n->getId()] = n;
381  }
382 
383  Node *getNode(std::size_t id)
384  {
385  auto iter = idNodeMap_.find(id);
386  if (iter != idNodeMap_.end())
387  return iter->second;
388 
389  double c = std::numeric_limits<double>::infinity();
390  auto *n = new Node(c, costEstimator_(id), c, id);
391  addNewNode(n);
392 
393  return n;
394  }
395 
396  void clear()
397  {
398  for (auto iter = idNodeMap_.begin(); iter != idNodeMap_.end(); ++iter)
399  {
400  Node *n = iter->second;
401  delete n;
402  }
403  }
404 
405  Heuristic &costEstimator_;
406  Graph &graph_;
407  Node *source_;
408  Node *target_;
409  Queue queue_;
410  IdNodeMap idNodeMap_;
411 
412  }; // LPAstarOnGraph
413 }
414 
415 #endif // OMPL_DATASTRUCTURES_LPA_STAR_ON_G_H
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
double operator()(std::size_t u)
using LPA* to approximate costToCome