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:
61  virtual ~AbstractLookupTable() {}
62 
64  virtual double lowerBound(const E* from, const E* to, double speed, double speedFactor, double fromEffort, double toEffort) const = 0;
65 
67  virtual bool consistent() const = 0;
68 };
69 
70 
71 template<class E, class V>
72 class FullLookupTable : public AbstractLookupTable<E, V> {
73 public:
74  FullLookupTable(const std::string& filename, const int size) :
75  myTable(size) {
76  std::ifstream strm(filename.c_str());
77  for (int i = 0; i < size; i++) {
78  for (int j = 0; j < size; j++) {
79  double val;
80  strm >> val;
81  myTable[i].push_back(val);
82  }
83  }
84  }
85 
86  virtual ~FullLookupTable() {
87  }
88 
89  double lowerBound(const E* from, const E* to, double /*speed*/, double speedFactor, double /*fromEffort*/, double /*toEffort*/) const {
90  return myTable[from->getNumericalID()][to->getNumericalID()] / speedFactor;
91  }
92 
93  bool consistent() const {
94  return true;
95  }
96 
97 private:
98  std::vector<std::vector<double> > myTable;
99 };
100 
101 
102 template<class E, class V>
104 public:
105  LandmarkLookupTable(const std::string& filename, const std::vector<E*>& edges, SUMOAbstractRouter<E, V>* router,
106  SUMOAbstractRouter<ReversedEdge<E, V>, V>* reverseRouter,
107  const V* defaultVehicle, const std::string& outfile, const int maxNumThreads) {
108  myFirstNonInternal = -1;
109  std::map<std::string, int> numericID;
110  for (E* e : edges) {
111  if (!e->isInternal()) {
112  if (myFirstNonInternal == -1) {
113  myFirstNonInternal = e->getNumericalID();
114  }
115  numericID[e->getID()] = e->getNumericalID() - myFirstNonInternal;
116  }
117  }
118 #ifdef HAVE_ZLIB
119  zstr::ifstream strm(filename.c_str(), std::fstream::in | std::fstream::binary);
120 #else
121  std::ifstream strm(filename.c_str());
122 #endif
123  if (!strm.good()) {
124  throw ProcessError("Could not load landmark-lookup-table from '" + filename + "'.");
125  }
126  std::string line;
127  int numLandMarks = 0;
128  while (std::getline(strm, line)) {
129  if (line == "") {
130  break;
131  }
132  //std::cout << "'" << line << "'" << "\n";
133  StringTokenizer st(line);
134  if (st.size() == 1) {
135  const std::string lm = st.get(0);
136  myLandmarks[lm] = numLandMarks++;
137  myFromLandmarkDists.push_back(std::vector<double>(0));
138  myToLandmarkDists.push_back(std::vector<double>(0));
139  } else if (st.size() == 4) {
140  // legacy style landmark table
141  const std::string lm = st.get(0);
142  const std::string edge = st.get(1);
143  if (numericID[edge] != (int)myFromLandmarkDists[myLandmarks[lm]].size()) {
144  WRITE_WARNING("Unknown or unordered edge '" + edge + "' in landmark file.");
145  }
146  const double distFrom = StringUtils::toDouble(st.get(2));
147  const double distTo = StringUtils::toDouble(st.get(3));
148  myFromLandmarkDists[myLandmarks[lm]].push_back(distFrom);
149  myToLandmarkDists[myLandmarks[lm]].push_back(distTo);
150  } else {
151  assert((int)st.size() == 2 * numLandMarks + 1);
152  const std::string edge = st.get(0);
153  if (numericID[edge] != (int)myFromLandmarkDists[0].size()) {
154  WRITE_WARNING("Unknown or unordered edge '" + edge + "' in landmark file.");
155  }
156  for (int i = 0; i < numLandMarks; i++) {
157  const double distFrom = StringUtils::toDouble(st.get(2 * i + 1));
158  const double distTo = StringUtils::toDouble(st.get(2 * i + 2));
159  myFromLandmarkDists[i].push_back(distFrom);
160  myToLandmarkDists[i].push_back(distTo);
161  }
162  }
163  }
164  if (myLandmarks.empty()) {
165  WRITE_WARNING("No landmarks in '" + filename + "', falling back to standard A*.");
166  return;
167  }
168 #ifdef HAVE_FOX
169  MFXWorkerThread::Pool threadPool;
170  std::vector<RoutingTask*> currentTasks;
171 #endif
172  std::vector<const E*> landmarks;
173  for (int i = 0; i < numLandMarks; ++i) {
174  if ((int)myFromLandmarkDists[i].size() != (int)edges.size() - myFirstNonInternal) {
175  const std::string landmarkID = getLandmark(i);
176  const E* landmark = nullptr;
177  // retrieve landmark edge
178  for (const E* const edge : edges) {
179  if (edge->getID() == landmarkID) {
180  landmark = edge;
181  landmarks.push_back(edge);
182  break;
183  }
184  }
185  if (landmark == nullptr) {
186  WRITE_WARNING("Landmark edge '" + landmarkID + "' does not exist in the network.");
187  continue;
188  }
189  if (!outfile.empty()) {
190  WRITE_WARNING("Not all network edges were found in the lookup table '" + filename + "' for landmark edge '" + landmarkID + "'. Saving new matrix to '" + outfile + "'.");
191  } else {
192  if (myFromLandmarkDists[i].empty()) {
193  WRITE_WARNING("No lookup table for landmark edge '" + landmarkID + "', recalculating.");
194  } else {
195  throw ProcessError("Not all network edges were found in the lookup table '" + filename + "' for landmark edge '" + landmarkID + "'.");
196  }
197  }
198 #ifdef HAVE_FOX
199  if (maxNumThreads > 0) {
200  const double lmCost = router->recomputeCosts({landmark}, defaultVehicle, 0);
201  router->setAutoBulkMode(true);
202  if (threadPool.size() == 0) {
203  if (reverseRouter == nullptr) {
204  // The CHRouter needs initialization
205  // before it gets cloned, so we do a dummy routing which is not in parallel
206  std::vector<const E*> route;
207  router->compute(landmark, landmark, defaultVehicle, 0, route);
208  } else {
209  reverseRouter->setAutoBulkMode(true);
210  }
211  while ((int)threadPool.size() < maxNumThreads) {
212  auto revClone = reverseRouter == nullptr ? nullptr : reverseRouter->clone();
213  new WorkerThread(threadPool, router->clone(), revClone, defaultVehicle);
214  }
215  }
216  for (int j = (int)myFromLandmarkDists[i].size() + myFirstNonInternal; j < (int)edges.size(); ++j) {
217  const E* const edge = edges[j];
218  if (landmark != edge) {
219  const double sourceDestCost = lmCost + router->recomputeCosts({edge}, defaultVehicle, 0);
220  currentTasks.push_back(new RoutingTask(landmark, edge, sourceDestCost));
221  threadPool.add(currentTasks.back(), i % maxNumThreads);
222  }
223  }
224  }
225 #else
226  UNUSED_PARAMETER(reverseRouter);
227 #endif
228  }
229  }
230 #ifdef HAVE_FOX
231  threadPool.waitAll(false);
232  int taskIndex = 0;
233 #endif
234  for (int i = 0; i < numLandMarks; ++i) {
235  if ((int)myFromLandmarkDists[i].size() != (int)edges.size() - myFirstNonInternal) {
236  const E* landmark = landmarks[i];
237  const double lmCost = router->recomputeCosts({landmark}, defaultVehicle, 0);
238  for (int j = (int)myFromLandmarkDists[i].size() + myFirstNonInternal; j < (int)edges.size(); ++j) {
239  const E* edge = edges[j];
240  double distFrom = -1;
241  double distTo = -1;
242  if (landmark == edge) {
243  distFrom = 0;
244  distTo = 0;
245  } else {
246  if (maxNumThreads > 0) {
247 #ifdef HAVE_FOX
248  distFrom = currentTasks[taskIndex]->getFromCost();
249  distTo = currentTasks[taskIndex]->getToCost();
250  delete currentTasks[taskIndex++];
251 #endif
252  } else {
253  const double sourceDestCost = lmCost + router->recomputeCosts({edge}, defaultVehicle, 0);
254  std::vector<const E*> route;
255  std::vector<const ReversedEdge<E, V>*> reversedRoute;
256  // compute from-distance (skip taz-sources and other unreachable edges)
257  if (edge->getPredecessors().size() > 0 && landmark->getSuccessors().size() > 0) {
258  if (router->compute(landmark, edge, defaultVehicle, 0, route)) {
259  distFrom = MAX2(0.0, router->recomputeCosts(route, defaultVehicle, 0) - sourceDestCost);
260  route.clear();
261  }
262  }
263  // compute to-distance (skip unreachable landmarks)
264  if (landmark->getPredecessors().size() > 0 && edge->getSuccessors().size() > 0) {
265  if (router->compute(edge, landmark, defaultVehicle, 0, route)) {
266  distTo = MAX2(0.0, router->recomputeCosts(route, defaultVehicle, 0) - sourceDestCost);
267  route.clear();
268  }
269  }
270  }
271  }
272  myFromLandmarkDists[i].push_back(distFrom);
273  myToLandmarkDists[i].push_back(distTo);
274  }
275  }
276  }
277  if (!outfile.empty()) {
278  std::ostream* ostrm = nullptr;
279 #ifdef HAVE_ZLIB
280  if (StringUtils::endsWith(outfile, ".gz")) {
281  ostrm = new zstr::ofstream(outfile.c_str(), std::ios_base::out);
282  } else {
283 #endif
284  ostrm = new std::ofstream(outfile.c_str());
285 #ifdef HAVE_ZLIB
286  }
287 #endif
288  if (!ostrm->good()) {
289  delete ostrm;
290  throw ProcessError("Could not open file '" + outfile + "' for writing.");
291  }
292  for (int i = 0; i < numLandMarks; ++i) {
293  (*ostrm) << getLandmark(i) << "\n";
294  }
295  for (int j = 0; j < (int)myFromLandmarkDists[0].size(); ++j) {
296  (*ostrm) << edges[j + myFirstNonInternal]->getID();
297  for (int i = 0; i < numLandMarks; ++i) {
298  (*ostrm) << " " << myFromLandmarkDists[i][j] << " " << myToLandmarkDists[i][j];
299  }
300  (*ostrm) << "\n";
301  }
302  delete ostrm;
303  }
304  }
305 
307  }
308 
309  double lowerBound(const E* from, const E* to, double speed, double speedFactor, double fromEffort, double toEffort) const {
310  double result = from->getDistanceTo(to) / speed;
311 #ifdef ASTAR_DEBUG_LOOKUPTABLE
312  if (from->getID() == ASTAR_DEBUG_LOOKUPTABLE_FROM) {
313  std::cout << " lowerBound to=" << to->getID() << " result1=" << result << "\n";
314  }
315 #endif
316  for (int i = 0; i < (int)myLandmarks.size(); ++i) {
317  // a cost of -1 is used to encode unreachability.
318  const double fl = myToLandmarkDists[i][from->getNumericalID() - myFirstNonInternal];
319  const double tl = myToLandmarkDists[i][to->getNumericalID() - myFirstNonInternal];
320  if (fl >= 0 && tl >= 0) {
321  const double bound = (fl - tl - toEffort) / speedFactor;
322 #ifdef ASTAR_DEBUG_LOOKUPTABLE
323  if (from->getID() == ASTAR_DEBUG_LOOKUPTABLE_FROM && result < bound) {
324  std::cout << " landmarkTo=" << getLandmark(i) << " result2=" << bound
325  << " fl=" << fl << " tl=" << tl << "\n";
326  }
327 #endif
328  result = MAX2(result, bound);
329  }
330  const double lt = myFromLandmarkDists[i][to->getNumericalID() - myFirstNonInternal];
331  const double lf = myFromLandmarkDists[i][from->getNumericalID() - myFirstNonInternal];
332  if (lt >= 0 && lf >= 0) {
333  const double bound = (lt - lf - fromEffort) / speedFactor;
334 #ifdef ASTAR_DEBUG_LOOKUPTABLE
335  if (from->getID() == ASTAR_DEBUG_LOOKUPTABLE_FROM && result < bound) {
336  std::cout << " landmarkFrom=" << getLandmark(i) << " result3=" << bound
337  << " lt=" << lt << " lf=" << lf << "\n";
338  }
339 #endif
340  result = MAX2(result, bound);
341  }
342  if ((tl >= 0 && fl < 0)
343  || (lf >= 0 && lt < 0)) {
344  // target unreachable.
345 #ifdef ASTAR_DEBUG_UNREACHABLE
346  std::cout << " unreachable: from=" << from->getID() << " to=" << to->getID() << " landmark=" << getLandmark(i) << " "
347  << ((tl >= 0 && fl < 0) ? " (toLandmark)" : " (fromLandmark)")
348  << " fl=" << fl << " tl=" << tl << " lt=" << lt << " lf=" << lf
349  << "\n";
350 #endif
351  return UNREACHABLE;
352  }
353  }
354  return result;
355  }
356 
357  bool consistent() const {
358  return false;
359  }
360 
361 private:
362  std::map<std::string, int> myLandmarks;
363  std::vector<std::vector<double> > myFromLandmarkDists;
364  std::vector<std::vector<double> > myToLandmarkDists;
366 
367 #ifdef HAVE_FOX
368 private:
369  class WorkerThread : public MFXWorkerThread {
370  public:
371  WorkerThread(MFXWorkerThread::Pool& pool,
372  SUMOAbstractRouter<E, V>* router,
373  SUMOAbstractRouter<ReversedEdge<E, V>, V>* reverseRouter, const V* vehicle)
374  : MFXWorkerThread(pool), myRouter(router), myReversedRouter(reverseRouter), myVehicle(vehicle) {}
375 
376  virtual ~WorkerThread() {
377  delete myRouter;
378  }
379 
380  const std::pair<double, double> compute(const E* src, const E* dest, const double costOff) {
381  double fromResult = -1.;
382  if (myRouter->compute(src, dest, myVehicle, 0, myRoute)) {
383  fromResult = MAX2(0.0, myRouter->recomputeCosts(myRoute, myVehicle, 0) + costOff);
384  myRoute.clear();
385  }
386  double toResult = -1.;
387  if (myReversedRouter != nullptr) {
388  if (myReversedRouter->compute(src->getReversedRoutingEdge(), dest->getReversedRoutingEdge(), myVehicle, 0, myReversedRoute)) {
389  toResult = MAX2(0.0, myReversedRouter->recomputeCosts(myReversedRoute, myVehicle, 0) + costOff);
390  myReversedRoute.clear();
391  }
392  } else {
393  if (myRouter->compute(dest, src, myVehicle, 0, myRoute)) {
394  toResult = MAX2(0.0, myRouter->recomputeCosts(myRoute, myVehicle, 0) + costOff);
395  myRoute.clear();
396  }
397  }
398  return std::make_pair(fromResult, toResult);
399  }
400 
401  private:
402  SUMOAbstractRouter<E, V>* myRouter;
403  SUMOAbstractRouter<ReversedEdge<E, V>, V>* myReversedRouter;
404  const V* myVehicle;
405  std::vector<const E*> myRoute;
406  std::vector<const ReversedEdge<E, V>*> myReversedRoute;
407  };
408 
409  class RoutingTask : public MFXWorkerThread::Task {
410  public:
411  RoutingTask(const E* src, const E* dest, const double costOff)
412  : mySrc(src), myDest(dest), myCostOff(-costOff) {}
413  void run(MFXWorkerThread* context) {
414  myCost = ((WorkerThread*)context)->compute(mySrc, myDest, myCostOff);
415  }
416  double getFromCost() {
417  return myCost.first;
418  }
419  double getToCost() {
420  return myCost.second;
421  }
422  private:
423  const E* const mySrc;
424  const E* const myDest;
425  const double myCostOff;
426  std::pair<double, double> myCost;
427  private:
429  RoutingTask& operator=(const RoutingTask&) = delete;
430  };
431 
432 private:
434 #endif
435 
436  std::string getLandmark(int i) const {
437  for (std::map<std::string, int>::const_iterator it = myLandmarks.begin(); it != myLandmarks.end(); ++it) {
438  if (it->second == i) {
439  return it->first;
440  }
441  }
442  return "";
443  }
444 };
#define UNREACHABLE
#define WRITE_WARNING(msg)
Definition: MsgHandler.h:265
#define UNUSED_PARAMETER(x)
Definition: StdDefs.h:30
T MAX2(T a, T b)
Definition: StdDefs.h:77
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 ~AbstractLookupTable()
virtual bool consistent() const =0
whether the heuristic ist consistent (found nodes are always visited on the shortest path the first t...
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...
A pool of worker threads which distributes the tasks and collects the results.
void waitAll(const bool deleteFinished=true)
waits for all tasks to be finished
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.
Abstract superclass of a task to be run with an index to keep track of pending tasks.
A thread repeatingly calculating incoming tasks.
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.