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