Loading...
Searching...
No Matches
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
37Sven Koenig, Maxim Likhachev, David Furcy:
38Lifelong 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
65namespace 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
double operator()(std::size_t u)
using LPA* to approximate costToCome
Main namespace. Contains everything in this library.