Eclipse SUMO - Simulation of Urban MObility
AStarLookupTable.h
Go to the documentation of this file.
1 /****************************************************************************/
2 // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.org/sumo
3 // Copyright (C) 2012-2022 German Aerospace Center (DLR) and others.
4 // This program and the accompanying materials are made available under the
5 // terms of the Eclipse Public License 2.0 which is available at
6 // https://www.eclipse.org/legal/epl-2.0/
7 // This Source Code may also be made available under the following Secondary
8 // Licenses when the conditions for such availability set forth in the Eclipse
9 // Public License 2.0 are satisfied: GNU General Public License, version 2
10 // or later which is available at
11 // https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
12 // SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later
13 /****************************************************************************/
18 // Precomputed landmark distances to speed up the A* routing algorithm
19 /****************************************************************************/
20 #pragma once
21 #include <config.h>
22 
23 #include <iostream>
24 #include <fstream>
25 #ifdef HAVE_ZLIB
26 #include <foreign/zstr/zstr.hpp>
27 #endif
28 #ifdef HAVE_FOX
30 #endif
32 
33 #define UNREACHABLE (std::numeric_limits<double>::max() / 1000.0)
34 
35 //#define ASTAR_DEBUG_LOOKUPTABLE
36 //#define ASTAR_DEBUG_LOOKUPTABLE_FROM "disabled"
37 //#define ASTAR_DEBUG_UNREACHABLE
38 
39 // ===========================================================================
40 // class definitions
41 // ===========================================================================
58 template<class E, class V>
60 public:
62  virtual double lowerBound(const E* from, const E* to, double speed, double speedFactor, double fromEffort, double toEffort) const = 0;
63 
65  virtual bool consistent() const = 0;
66 };
67 
68 
69 template<class E, class V>
70 class FullLookupTable : public AbstractLookupTable<E, V> {
71 public:
72  FullLookupTable(const std::string& filename, const int size) :
73  myTable(size) {
74  std::ifstream strm(filename.c_str());
75  for (int i = 0; i < size; i++) {
76  for (int j = 0; j < size; j++) {
77  double val;
78  strm >> val;
79  myTable[i].push_back(val);
80  }
81  }
82  }
83 
84  virtual ~FullLookupTable() {
85  }
86 
87  double lowerBound(const E* from, const E* to, double /*speed*/, double speedFactor, double /*fromEffort*/, double /*toEffort*/) const {
88  return myTable[from->getNumericalID()][to->getNumericalID()] / speedFactor;
89  }
90 
91  bool consistent() const {
92  return true;
93  }
94 
95 private:
96  std::vector<std::vector<double> > myTable;
97 };
98 
99 
100 template<class E, class V>
102 public:
103  LandmarkLookupTable(const std::string& filename, const std::vector<E*>& edges, SUMOAbstractRouter<E, V>* router,
104  SUMOAbstractRouter<ReversedEdge<E, V>, V>* reverseRouter,
105  const V* defaultVehicle, const std::string& outfile, const int maxNumThreads) {
106  myFirstNonInternal = -1;
107  std::map<std::string, int> numericID;
108  for (E* e : edges) {
109  if (!e->isInternal()) {
110  if (myFirstNonInternal == -1) {
111  myFirstNonInternal = e->getNumericalID();
112  }
113  numericID[e->getID()] = e->getNumericalID() - myFirstNonInternal;
114  }
115  }
116 #ifdef HAVE_ZLIB
117  zstr::ifstream strm(filename.c_str(), std::fstream::in | std::fstream::binary);
118 #else
119  std::ifstream strm(filename.c_str());
120 #endif
121  if (!strm.good()) {
122  throw ProcessError("Could not load landmark-lookup-table from '" + filename + "'.");
123  }
124  std::string line;
125  int numLandMarks = 0;
126  while (std::getline(strm, line)) {
127  if (line == "") {
128  break;
129  }
130  //std::cout << "'" << line << "'" << "\n";
131  StringTokenizer st(line);
132  if (st.size() == 1) {
133  const std::string lm = st.get(0);
134  myLandmarks[lm] = numLandMarks++;
135  myFromLandmarkDists.push_back(std::vector<double>(0));
136  myToLandmarkDists.push_back(std::vector<double>(0));
137  } else if (st.size() == 4) {
138  // legacy style landmark table
139  const std::string lm = st.get(0);
140  const std::string edge = st.get(1);
141  if (numericID[edge] != (int)myFromLandmarkDists[myLandmarks[lm]].size()) {
142  WRITE_WARNING("Unknown or unordered edge '" + edge + "' in landmark file.");
143  }
144  const double distFrom = StringUtils::toDouble(st.get(2));
145  const double distTo = StringUtils::toDouble(st.get(3));
146  myFromLandmarkDists[myLandmarks[lm]].push_back(distFrom);
147  myToLandmarkDists[myLandmarks[lm]].push_back(distTo);
148  } else {
149  assert((int)st.size() == 2 * numLandMarks + 1);
150  const std::string edge = st.get(0);
151  if (numericID[edge] != (int)myFromLandmarkDists[0].size()) {
152  WRITE_WARNING("Unknown or unordered edge '" + edge + "' in landmark file.");
153  }
154  for (int i = 0; i < numLandMarks; i++) {
155  const double distFrom = StringUtils::toDouble(st.get(2 * i + 1));
156  const double distTo = StringUtils::toDouble(st.get(2 * i + 2));
157  myFromLandmarkDists[i].push_back(distFrom);
158  myToLandmarkDists[i].push_back(distTo);
159  }
160  }
161  }
162  if (myLandmarks.empty()) {
163  WRITE_WARNING("No landmarks in '" + filename + "', falling back to standard A*.");
164  return;
165  }
166 #ifdef HAVE_FOX
167  FXWorkerThread::Pool threadPool;
168  std::vector<RoutingTask*> currentTasks;
169 #endif
170  std::vector<const E*> landmarks;
171  for (int i = 0; i < numLandMarks; ++i) {
172  if ((int)myFromLandmarkDists[i].size() != (int)edges.size() - myFirstNonInternal) {
173  const std::string landmarkID = getLandmark(i);
174  const E* landmark = nullptr;
175  // retrieve landmark edge
176  for (const E* const edge : edges) {
177  if (edge->getID() == landmarkID) {
178  landmark = edge;
179  landmarks.push_back(edge);
180  break;
181  }
182  }
183  if (landmark == nullptr) {
184  WRITE_WARNING("Landmark edge '" + landmarkID + "' does not exist in the network.");
185  continue;
186  }
187  if (!outfile.empty()) {
188  WRITE_WARNING("Not all network edges were found in the lookup table '" + filename + "' for landmark edge '" + landmarkID + "'. Saving new matrix to '" + outfile + "'.");
189  } else {
190  if (myFromLandmarkDists[i].empty()) {
191  WRITE_WARNING("No lookup table for landmark edge '" + landmarkID + "', recalculating.");
192  } else {
193  throw ProcessError("Not all network edges were found in the lookup table '" + filename + "' for landmark edge '" + landmarkID + "'.");
194  }
195  }
196 #ifdef HAVE_FOX
197  if (maxNumThreads > 0) {
198  const double lmCost = router->recomputeCosts({landmark}, defaultVehicle, 0);
199  router->setAutoBulkMode(true);
200  if (threadPool.size() == 0) {
201  if (reverseRouter == nullptr) {
202  // The CHRouter needs initialization
203  // before it gets cloned, so we do a dummy routing which is not in parallel
204  std::vector<const E*> route;
205  router->compute(landmark, landmark, defaultVehicle, 0, route);
206  } else {
207  reverseRouter->setAutoBulkMode(true);
208  }
209  while ((int)threadPool.size() < maxNumThreads) {
210  auto revClone = reverseRouter == nullptr ? nullptr : reverseRouter->clone();
211  new WorkerThread(threadPool, router->clone(), revClone, defaultVehicle);
212  }
213  }
214  for (int j = (int)myFromLandmarkDists[i].size() + myFirstNonInternal; j < (int)edges.size(); ++j) {
215  const E* const edge = edges[j];
216  if (landmark != edge) {
217  const double sourceDestCost = lmCost + router->recomputeCosts({edge}, defaultVehicle, 0);
218  currentTasks.push_back(new RoutingTask(landmark, edge, sourceDestCost));
219  threadPool.add(currentTasks.back(), i % maxNumThreads);
220  }
221  }
222  }
223 #endif
224  }
225  }
226 #ifdef HAVE_FOX
227  threadPool.waitAll(false);
228  int taskIndex = 0;
229 #endif
230  for (int i = 0; i < numLandMarks; ++i) {
231  if ((int)myFromLandmarkDists[i].size() != (int)edges.size() - myFirstNonInternal) {
232  const E* landmark = landmarks[i];
233  const double lmCost = router->recomputeCosts({landmark}, defaultVehicle, 0);
234  for (int j = (int)myFromLandmarkDists[i].size() + myFirstNonInternal; j < (int)edges.size(); ++j) {
235  const E* edge = edges[j];
236  double distFrom = -1;
237  double distTo = -1;
238  if (landmark == edge) {
239  distFrom = 0;
240  distTo = 0;
241  } else {
242  if (maxNumThreads > 0) {
243 #ifdef HAVE_FOX
244  distFrom = currentTasks[taskIndex]->getFromCost();
245  distTo = currentTasks[taskIndex]->getToCost();
246  delete currentTasks[taskIndex++];
247 #endif
248  } else {
249  const double sourceDestCost = lmCost + router->recomputeCosts({edge}, defaultVehicle, 0);
250  std::vector<const E*> route;
251  std::vector<const ReversedEdge<E, V>*> reversedRoute;
252  // compute from-distance (skip taz-sources and other unreachable edges)
253  if (edge->getPredecessors().size() > 0 && landmark->getSuccessors().size() > 0) {
254  if (router->compute(landmark, edge, defaultVehicle, 0, route)) {
255  distFrom = MAX2(0.0, router->recomputeCosts(route, defaultVehicle, 0) - sourceDestCost);
256  route.clear();
257  }
258  }
259  // compute to-distance (skip unreachable landmarks)
260  if (landmark->getPredecessors().size() > 0 && edge->getSuccessors().size() > 0) {
261  if (router->compute(edge, landmark, defaultVehicle, 0, route)) {
262  distTo = MAX2(0.0, router->recomputeCosts(route, defaultVehicle, 0) - sourceDestCost);
263  route.clear();
264  }
265  }
266  }
267  }
268  myFromLandmarkDists[i].push_back(distFrom);
269  myToLandmarkDists[i].push_back(distTo);
270  }
271  }
272  }
273  if (!outfile.empty()) {
274  std::ostream* ostrm = nullptr;
275 #ifdef HAVE_ZLIB
276  if (StringUtils::endsWith(outfile, ".gz")) {
277  ostrm = new zstr::ofstream(outfile.c_str(), std::ios_base::out);
278  } else {
279 #endif
280  ostrm = new std::ofstream(outfile.c_str());
281 #ifdef HAVE_ZLIB
282  }
283 #endif
284  if (!ostrm->good()) {
285  delete ostrm;
286  throw ProcessError("Could not open file '" + outfile + "' for writing.");
287  }
288  for (int i = 0; i < numLandMarks; ++i) {
289  (*ostrm) << getLandmark(i) << "\n";
290  }
291  for (int j = 0; j < (int)myFromLandmarkDists[0].size(); ++j) {
292  (*ostrm) << edges[j + myFirstNonInternal]->getID();
293  for (int i = 0; i < numLandMarks; ++i) {
294  (*ostrm) << " " << myFromLandmarkDists[i][j] << " " << myToLandmarkDists[i][j];
295  }
296  (*ostrm) << "\n";
297  }
298  delete ostrm;
299  }
300  }
301 
303  }
304 
305  double lowerBound(const E* from, const E* to, double speed, double speedFactor, double fromEffort, double toEffort) const {
306  double result = from->getDistanceTo(to) / speed;
307 #ifdef ASTAR_DEBUG_LOOKUPTABLE
308  if (from->getID() == ASTAR_DEBUG_LOOKUPTABLE_FROM) {
309  std::cout << " lowerBound to=" << to->getID() << " result1=" << result << "\n";
310  }
311 #endif
312  for (int i = 0; i < (int)myLandmarks.size(); ++i) {
313  // a cost of -1 is used to encode unreachability.
314  const double fl = myToLandmarkDists[i][from->getNumericalID() - myFirstNonInternal];
315  const double tl = myToLandmarkDists[i][to->getNumericalID() - myFirstNonInternal];
316  if (fl >= 0 && tl >= 0) {
317  const double bound = (fl - tl - toEffort) / speedFactor;
318 #ifdef ASTAR_DEBUG_LOOKUPTABLE
319  if (from->getID() == ASTAR_DEBUG_LOOKUPTABLE_FROM && result < bound) {
320  std::cout << " landmarkTo=" << getLandmark(i) << " result2=" << bound
321  << " fl=" << fl << " tl=" << tl << "\n";
322  }
323 #endif
324  result = MAX2(result, bound);
325  }
326  const double lt = myFromLandmarkDists[i][to->getNumericalID() - myFirstNonInternal];
327  const double lf = myFromLandmarkDists[i][from->getNumericalID() - myFirstNonInternal];
328  if (lt >= 0 && lf >= 0) {
329  const double bound = (lt - lf - fromEffort) / speedFactor;
330 #ifdef ASTAR_DEBUG_LOOKUPTABLE
331  if (from->getID() == ASTAR_DEBUG_LOOKUPTABLE_FROM && result < bound) {
332  std::cout << " landmarkFrom=" << getLandmark(i) << " result3=" << bound
333  << " lt=" << lt << " lf=" << lf << "\n";
334  }
335 #endif
336  result = MAX2(result, bound);
337  }
338  if ((tl >= 0 && fl < 0)
339  || (lf >= 0 && lt < 0)) {
340  // target unreachable.
341 #ifdef ASTAR_DEBUG_UNREACHABLE
342  std::cout << " unreachable: from=" << from->getID() << " to=" << to->getID() << " landmark=" << getLandmark(i) << " "
343  << ((tl >= 0 && fl < 0) ? " (toLandmark)" : " (fromLandmark)")
344  << " fl=" << fl << " tl=" << tl << " lt=" << lt << " lf=" << lf
345  << "\n";
346 #endif
347  return UNREACHABLE;
348  }
349  }
350  return result;
351  }
352 
353  bool consistent() const {
354  return false;
355  }
356 
357 private:
358  std::map<std::string, int> myLandmarks;
359  std::vector<std::vector<double> > myFromLandmarkDists;
360  std::vector<std::vector<double> > myToLandmarkDists;
362 
363 #ifdef HAVE_FOX
364 private:
365  class WorkerThread : public FXWorkerThread {
366  public:
367  WorkerThread(FXWorkerThread::Pool& pool,
368  SUMOAbstractRouter<E, V>* router,
369  SUMOAbstractRouter<ReversedEdge<E, V>, V>* reverseRouter, const V* vehicle)
370  : FXWorkerThread(pool), myRouter(router), myReversedRouter(reverseRouter), myVehicle(vehicle) {}
371 
372  virtual ~WorkerThread() {
373  delete myRouter;
374  }
375 
376  const std::pair<double, double> compute(const E* src, const E* dest, const double costOff) {
377  double fromResult = -1.;
378  if (myRouter->compute(src, dest, myVehicle, 0, myRoute)) {
379  fromResult = MAX2(0.0, myRouter->recomputeCosts(myRoute, myVehicle, 0) + costOff);
380  myRoute.clear();
381  }
382  double toResult = -1.;
383  if (myReversedRouter != nullptr) {
384  if (myReversedRouter->compute(src->getReversedRoutingEdge(), dest->getReversedRoutingEdge(), myVehicle, 0, myReversedRoute)) {
385  toResult = MAX2(0.0, myReversedRouter->recomputeCosts(myReversedRoute, myVehicle, 0) + costOff);
386  myReversedRoute.clear();
387  }
388  } else {
389  if (myRouter->compute(dest, src, myVehicle, 0, myRoute)) {
390  toResult = MAX2(0.0, myRouter->recomputeCosts(myRoute, myVehicle, 0) + costOff);
391  myRoute.clear();
392  }
393  }
394  return std::make_pair(fromResult, toResult);
395  }
396 
397  private:
398  SUMOAbstractRouter<E, V>* myRouter;
399  SUMOAbstractRouter<ReversedEdge<E, V>, V>* myReversedRouter;
400  const V* myVehicle;
401  std::vector<const E*> myRoute;
402  std::vector<const ReversedEdge<E, V>*> myReversedRoute;
403  };
404 
405  class RoutingTask : public FXWorkerThread::Task {
406  public:
407  RoutingTask(const E* src, const E* dest, const double costOff)
408  : mySrc(src), myDest(dest), myCostOff(-costOff) {}
409  void run(FXWorkerThread* context) {
410  myCost = ((WorkerThread*)context)->compute(mySrc, myDest, myCostOff);
411  }
412  double getFromCost() {
413  return myCost.first;
414  }
415  double getToCost() {
416  return myCost.second;
417  }
418  private:
419  const E* const mySrc;
420  const E* const myDest;
421  const double myCostOff;
422  std::pair<double, double> myCost;
423  private:
425  RoutingTask& operator=(const RoutingTask&) = delete;
426  };
427 
428 private:
430 #endif
431 
432  std::string getLandmark(int i) const {
433  for (std::map<std::string, int>::const_iterator it = myLandmarks.begin(); it != myLandmarks.end(); ++it) {
434  if (it->second == i) {
435  return it->first;
436  }
437  }
438  return "";
439  }
440 };
#define UNREACHABLE
#define WRITE_WARNING(msg)
Definition: MsgHandler.h:280
T MAX2(T a, T b)
Definition: StdDefs.h:80
virtual double lowerBound(const E *from, const E *to, double speed, double speedFactor, double fromEffort, double toEffort) const =0
provide a lower bound on the distance between from and to (excluding traveltime of both edges)
virtual bool consistent() const =0
whether the heuristic ist consistent (found nodes are always visited on the shortest path the first t...
A pool of worker threads which distributes the tasks and collects the results.
void add(Task *const t, int index=-1)
Gives a number to the given task and assigns it to the worker with the given index....
int size() const
Returns the number of threads in the pool.
void waitAll(const bool deleteFinished=true)
waits for all tasks to be finished
Abstract superclass of a task to be run with an index to keep track of pending tasks.
A thread repeatingly calculating incoming tasks.
double lowerBound(const E *from, const E *to, double, double speedFactor, double, double) const
provide a lower bound on the distance between from and to (excluding traveltime of both edges)
bool consistent() const
whether the heuristic ist consistent (found nodes are always visited on the shortest path the first t...
std::vector< std::vector< double > > myTable
virtual ~FullLookupTable()
FullLookupTable(const std::string &filename, const int size)
Computes the shortest path through a network using the A* algorithm.
std::vector< std::vector< double > > myToLandmarkDists
double lowerBound(const E *from, const E *to, double speed, double speedFactor, double fromEffort, double toEffort) const
provide a lower bound on the distance between from and to (excluding traveltime of both edges)
std::map< std::string, int > myLandmarks
std::string getLandmark(int i) const
virtual ~LandmarkLookupTable()
LandmarkLookupTable(const std::string &filename, const std::vector< E * > &edges, SUMOAbstractRouter< E, V > *router, SUMOAbstractRouter< ReversedEdge< E, V >, V > *reverseRouter, const V *defaultVehicle, const std::string &outfile, const int maxNumThreads)
std::vector< std::vector< double > > myFromLandmarkDists
bool consistent() const
whether the heuristic ist consistent (found nodes are always visited on the shortest path the first t...
the edge type representing backward edges
Definition: ReversedEdge.h:30
virtual SUMOAbstractRouter * clone()=0
double recomputeCosts(const std::vector< const E * > &edges, const V *const v, SUMOTime msTime, double *lengthp=nullptr) const
void setAutoBulkMode(const bool mode)
virtual bool compute(const E *from, const E *to, const V *const vehicle, SUMOTime msTime, std::vector< const E * > &into, bool silent=false)=0
Builds the route between the given edges using the minimum effort at the given time The definition of...
int size() const
returns the number of existing substrings
std::string get(int pos) const
returns the item at the given position
static double toDouble(const std::string &sData)
converts a string into the double value described by it by calling the char-type converter
static bool endsWith(const std::string &str, const std::string suffix)
Checks whether a given string ends with the suffix.