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