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  return;
101  }
102  void removeEdge(std::size_t u, std::size_t v)
103  {
104  assert(v != source_->getId());
105 
106  Node *n_u = getNode(u);
107  Node *n_v = getNode(v);
108 
109  if (n_v->getParent() == n_u)
110  {
111  WeightMap weights = boost::get(boost::edge_weight_t(), graph_);
112  chooseBestIncomingNode(n_v, weights);
113  }
114 
115  updateVertex(n_v);
116  }
117  double computeShortestPath(std::list<std::size_t> &path)
118  {
119  WeightMap weights = boost::get(boost::edge_weight_t(), graph_);
120 
121  if (queue_.empty())
122  return std::numeric_limits<double>::infinity();
123 
124  while (topHead()->key() < target_->calculateKey() || target_->rhs() != target_->costToCome())
125  {
126  // pop from queue and process
127  Node *u = topHead();
128 
129  if (u->costToCome() > u->rhs()) // the node is overconsistent
130  {
131  u->setCostToCome(u->rhs());
132  popHead();
133 
134  // iterate over all (outgoing) neighbors of the node and get the best parent for each one
135  typename boost::graph_traits<Graph>::out_edge_iterator ei, ei_end;
136  for (boost::tie(ei, ei_end) = boost::out_edges(u->getId(), graph_); ei != ei_end; ++ei)
137  {
138  std::size_t v = boost::target(*ei, graph_);
139  Node *n_v = getNode(v);
140  double c = boost::get(weights, *ei); // edge weight from u to v
141 
142  if (n_v->rhs() > u->costToCome() + c)
143  {
144  n_v->setParent(u);
145  n_v->setRhs(u->costToCome() + c);
146  updateVertex(n_v);
147  }
148  }
149  }
150  else // (n->costToCome() < n->rhs()) // the node is underconsistent
151  {
152  u->setCostToCome(std::numeric_limits<double>::infinity());
153  updateVertex(u);
154 
155  // get all (outgoing) neighbors of the node
156  typename boost::graph_traits<Graph>::out_edge_iterator ei, ei_end;
157  for (boost::tie(ei, ei_end) = boost::out_edges(u->getId(), graph_); ei != ei_end; ++ei)
158  {
159  std::size_t v = boost::target(*ei, graph_);
160  Node *n_v = getNode(v);
161 
162  if ((n_v == source_) || (n_v->getParent() != u))
163  continue;
164 
165  chooseBestIncomingNode(n_v, weights);
166  updateVertex(n_v);
167  }
168  }
169 
170  if (queue_.empty())
171  break;
172  }
173 
174  // now get path
175  Node *res = (target_->costToCome() == std::numeric_limits<double>::infinity() ? nullptr : target_);
176  while (res != nullptr)
177  {
178  path.push_front(res->getId());
179  res = res->getParent();
180  }
181 
182  return target_->costToCome();
183  }
184 
186  double operator()(std::size_t u)
187  {
188  auto iter = idNodeMap_.find(u);
189  if (iter != idNodeMap_.end())
190  return iter->second->costToCome();
191  return std::numeric_limits<double>::infinity();
192  }
193 
194  private:
195  struct Key
196  {
197  Key(double first_ = -1, double second_ = -1) : first(first_), second(second_)
198  {
199  }
200  bool operator<(const Key &other)
201  {
202  return (first != other.first) ? (first < other.first) : (second < other.second);
203  }
204  double first, second;
205  };
206 
207  class Node
208  {
209  public:
210  Node(double costToCome, double costToGo, double rhs, std::size_t &dataId, Node *parentNode = nullptr)
211  : g(costToCome), h(costToGo), r(rhs), isInQ(false), parent(parentNode), id(dataId)
212  {
213  calculateKey();
214  }
215  // cost accesors
216  double costToCome() const
217  {
218  return g;
219  }
220  double costToGo() const
221  {
222  return h;
223  }
224  double rhs() const
225  {
226  return r;
227  }
228  Key key() const
229  {
230  return k;
231  }
232  Key calculateKey()
233  {
234  k = Key(std::min(g, r + h), std::min(g, r));
235  return k;
236  }
237  // cost modifiers
238  double setCostToCome(double val)
239  {
240  return g = val;
241  }
242  double setRhs(double val)
243  {
244  return r = val;
245  }
246  // is in queue field
247  bool isInQueue() const
248  {
249  return isInQ;
250  }
251  void inQueue(bool in)
252  {
253  isInQ = in;
254  }
255  // parent field
256  Node *getParent() const
257  {
258  return parent;
259  }
260  void setParent(Node *p)
261  {
262  parent = p;
263  }
264  // data field
265  std::size_t getId() const
266  {
267  return id;
268  }
269  bool isConsistent() const
270  {
271  return g == r;
272  }
273 
274  private:
275  double g; // cost to come
276  double h; // cost to go
277  double r; // rhs
278  Key k; // key
279  bool isInQ;
280  Node *parent;
281  std::size_t id; // unique data associated with node
282  }; // Node
283 
284  struct LessThanNodeK
285  {
286  bool operator()(const Node *n1, const Node *n2) const
287  {
288  return n1->key() < n2->key();
289  }
290  }; // LessThanNodeK
291 
292  struct Hash
293  {
294  std::size_t operator()(const std::size_t id) const
295  {
296  return h(id);
297  }
298  std::hash<std::size_t> h;
299  }; // Hash
300 
301  using Queue = std::multiset<Node *, LessThanNodeK>;
302  using IdNodeMap = std::unordered_map<std::size_t, Node *, Hash>;
303  using IdNodeMapIter = typename IdNodeMap::iterator;
304  using WeightMap = typename boost::property_map<Graph, boost::edge_weight_t>::type;
305 
306  // LPA* subprocedures
307  void updateVertex(Node *n)
308  {
309  if (!n->isConsistent())
310  {
311  if (n->isInQueue())
312  updateQueue(n);
313  else
314  insertQueue(n);
315  }
316  else if (n->isInQueue())
317  removeQueue(n);
318  return;
319  }
320  // queue utils
321  Node *popHead()
322  {
323  Node *n = topHead();
324  n->inQueue(false);
325  queue_.erase(queue_.begin());
326 
327  return n;
328  }
329  Node *topHead()
330  {
331  return *queue_.begin();
332  }
333 
334  void insertQueue(Node *node)
335  {
336  assert(node->isInQueue() == false);
337 
338  node->calculateKey();
339  node->inQueue(true);
340  queue_.insert(node);
341 
342  return;
343  }
344  void removeQueue(Node *node)
345  {
346  if (node->isInQueue())
347  {
348  node->inQueue(false);
349  queue_.erase(node);
350  }
351  return;
352  }
353  void updateQueue(Node *node)
354  {
355  removeQueue(node);
356  insertQueue(node);
357  return;
358  }
359 
360  void chooseBestIncomingNode(Node *n_v, WeightMap &weights)
361  {
362  // iterate over all incoming neighbors of the node n_v and get the best parent
363  double min = std::numeric_limits<double>::infinity();
364  Node *best = nullptr;
365 
366  typename boost::graph_traits<Graph>::in_edge_iterator ei, ei_end;
367  for (boost::tie(ei, ei_end) = boost::in_edges(n_v->getId(), graph_); ei != ei_end; ++ei)
368  {
369  std::size_t u = boost::source(*ei, graph_);
370  Node *n_u = getNode(u);
371  double c = boost::get(weights, *ei); // edge weight from u to v
372 
373  double curr = n_u->costToCome() + c;
374  if (curr < min)
375  {
376  min = curr;
377  best = n_u;
378  }
379  }
380 
381  n_v->setRhs(min);
382  n_v->setParent(best);
383  }
384 
385  void addNewNode(Node *n)
386  {
387  idNodeMap_[n->getId()] = n;
388  }
389 
390  Node *getNode(std::size_t id)
391  {
392  auto iter = idNodeMap_.find(id);
393  if (iter != idNodeMap_.end())
394  return iter->second;
395 
396  double c = std::numeric_limits<double>::infinity();
397  auto *n = new Node(c, costEstimator_(id), c, id);
398  addNewNode(n);
399 
400  return n;
401  }
402 
403  void clear()
404  {
405  for (auto iter = idNodeMap_.begin(); iter != idNodeMap_.end(); ++iter)
406  {
407  Node *n = iter->second;
408  delete n;
409  }
410  }
411 
412  Heuristic &costEstimator_;
413  Graph &graph_;
414  Node *source_;
415  Node *target_;
416  Queue queue_;
417  IdNodeMap idNodeMap_;
418 
419  }; // LPAstarOnGraph
420 }
421 
422 #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