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-2018 German Aerospace Center (DLR) and others.
4 // This program and the accompanying materials
5 // are made available under the terms of the Eclipse Public License v2.0
6 // which accompanies this distribution, and is available at
7 // http://www.eclipse.org/legal/epl-v20.html
8 // SPDX-License-Identifier: EPL-2.0
9 /****************************************************************************/
15 // Precomputed landmark distances to speed up the A* routing algorithm
16 /****************************************************************************/
17 #ifndef AStarLookupTable_h
18 #define AStarLookupTable_h
19 
20 
21 // ===========================================================================
22 // included modules
23 // ===========================================================================
24 #include <config.h>
25 
26 #include <iostream>
27 #include <fstream>
28 
29 #ifdef HAVE_FOX
31 #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  BinaryInputDevice dev(filename);
75  for (int i = 0; i < size; i++) {
76  for (int j = 0; j < size; j++) {
77  double val;
78  dev >> val;
79  myTable[i].push_back(val);
80  }
81  }
82  }
83 
84  double lowerBound(const E* from, const E* to, double /*speed*/, double speedFactor, double /*fromEffort*/, double /*toEffort*/) const {
85  return myTable[from->getNumericalID()][to->getNumericalID()] / speedFactor;
86  }
87 
88  bool consistent() const {
89  return true;
90  }
91 
92 private:
93  std::vector<std::vector<double> > myTable;
94 };
95 
96 
97 template<class E, class V>
99 public:
100  LandmarkLookupTable(const std::string& filename, const std::vector<E*>& edges, SUMOAbstractRouter<E, V>* router, const V* defaultVehicle, const std::string& outfile, const int maxNumThreads) {
101  myFirstNonInternal = -1;
102  std::map<std::string, int> numericID;
103  for (E* e : edges) {
104  if (!e->isInternal()) {
105  if (myFirstNonInternal == -1) {
106  myFirstNonInternal = e->getNumericalID();
107  }
108  numericID[e->getID()] = e->getNumericalID() - myFirstNonInternal;
109  }
110  }
111  std::ifstream strm(filename.c_str());
112  if (!strm.good()) {
113  throw ProcessError("Could not load landmark-lookup-table from '" + filename + "'.");
114  }
115  std::ofstream* ostrm = nullptr;
116  if (!outfile.empty()) {
117  ostrm = new std::ofstream(outfile.c_str());
118  if (!ostrm->good()) {
119  throw ProcessError("Could not open file '" + outfile + "' for writing.");
120  }
121  }
122  std::string line;
123  int numLandMarks = 0;
124  while (std::getline(strm, line)) {
125  if (line == "") {
126  break;
127  }
128  //std::cout << "'" << line << "'" << "\n";
129  StringTokenizer st(line);
130  if (st.size() == 1) {
131  const std::string lm = st.get(0);
132  myLandmarks[lm] = numLandMarks++;
133  myFromLandmarkDists.push_back(std::vector<double>(0));
134  myToLandmarkDists.push_back(std::vector<double>(0));
135  if (ostrm != nullptr) {
136  (*ostrm) << lm << "\n";
137  }
138  } else {
139  assert(st.size() == 4);
140  const std::string lm = st.get(0);
141  const std::string edge = st.get(1);
142  if (numericID[edge] != (int)myFromLandmarkDists[myLandmarks[lm]].size()) {
143  WRITE_WARNING("Unknown or unordered edge '" + edge + "' in landmark file.");
144  }
145  const double distFrom = StringUtils::toDouble(st.get(2));
146  const double distTo = StringUtils::toDouble(st.get(3));
147  myFromLandmarkDists[myLandmarks[lm]].push_back(distFrom);
148  myToLandmarkDists[myLandmarks[lm]].push_back(distTo);
149  }
150  }
151  if (myLandmarks.empty()) {
152  WRITE_WARNING("No landmarks in '" + filename + "', falling back to standard A*.");
153  delete ostrm;
154  return;
155  }
156 #ifdef HAVE_FOX
157  FXWorkerThread::Pool threadPool;
158 #endif
159  for (int i = 0; i < (int)myLandmarks.size(); ++i) {
160  if ((int)myFromLandmarkDists[i].size() != (int)edges.size() - myFirstNonInternal) {
161  const std::string landmarkID = getLandmark(i);
162  const E* landmark = nullptr;
163  // retrieve landmark edge
164  for (const E* const edge : edges) {
165  if (edge->getID() == landmarkID) {
166  landmark = edge;
167  break;
168  }
169  }
170  if (landmark == nullptr) {
171  WRITE_WARNING("Landmark '" + landmarkID + "' does not exist in the network.");
172  continue;
173  }
174  if (router != nullptr) {
175  const std::string missing = outfile.empty() ? filename + ".missing" : outfile;
176  WRITE_WARNING("Not all network edges were found in the lookup table '" + filename + "' for landmark '" + landmarkID + "'. Saving missing values to '" + missing + "'.");
177  if (ostrm == nullptr) {
178  ostrm = new std::ofstream(missing.c_str());
179  if (!ostrm->good()) {
180  throw ProcessError("Could not open file '" + missing + "' for writing.");
181  }
182  }
183  } else {
184  throw ProcessError("Not all network edges were found in the lookup table '" + filename + "' for landmark '" + landmarkID + "'.");
185  }
186  std::vector<const E*> routeLM(1, landmark);
187  const double lmCost = router->recomputeCosts(routeLM, defaultVehicle, 0);
188  std::vector<const E*> route;
189 #ifdef HAVE_FOX
190  if (maxNumThreads > 0) {
191  if (threadPool.size() == 0) {
192  // The CHRouter needs initialization
193  // before it gets cloned, so we do a dummy routing which is not in parallel
194  router->compute(landmark, landmark, defaultVehicle, 0, route);
195  route.clear();
196  while ((int)threadPool.size() < maxNumThreads) {
197  new WorkerThread(threadPool, router->clone(), defaultVehicle);
198  }
199  }
200  std::vector<RoutingTask*> currentTasks;
201  for (int j = (int)myFromLandmarkDists[i].size() + myFirstNonInternal; j < (int)edges.size(); ++j) {
202  const E* edge = edges[j];
203  if (landmark != edge) {
204  std::vector<const E*> routeE(1, edge);
205  const double sourceDestCost = lmCost + router->recomputeCosts(routeE, defaultVehicle, 0);
206  // compute from-distance (skip taz-sources and other unreachable edges)
207  if (edge->getPredecessors().size() > 0 && landmark->getSuccessors().size() > 0) {
208  currentTasks.push_back(new RoutingTask(landmark, edge, sourceDestCost));
209  threadPool.add(currentTasks.back());
210  }
211  // compute to-distance (skip unreachable landmarks)
212  if (landmark->getPredecessors().size() > 0 && edge->getSuccessors().size() > 0) {
213  currentTasks.push_back(new RoutingTask(edge, landmark, sourceDestCost));
214  threadPool.add(currentTasks.back());
215  }
216  }
217  }
218  threadPool.waitAll(false);
219  int taskIndex = 0;
220  for (int j = (int)myFromLandmarkDists[i].size() + myFirstNonInternal; j < (int)edges.size(); ++j) {
221  const E* edge = edges[j];
222  double distFrom = -1;
223  double distTo = -1;
224  if (landmark == edge) {
225  distFrom = 0;
226  distTo = 0;
227  } else {
228  if (edge->getPredecessors().size() > 0 && landmark->getSuccessors().size() > 0) {
229  distFrom = currentTasks[taskIndex]->getCost();
230  delete currentTasks[taskIndex++];
231  }
232  if (landmark->getPredecessors().size() > 0 && edge->getSuccessors().size() > 0) {
233  distTo = currentTasks[taskIndex]->getCost();
234  delete currentTasks[taskIndex++];
235  }
236  }
237  myFromLandmarkDists[i].push_back(distFrom);
238  myToLandmarkDists[i].push_back(distTo);
239  (*ostrm) << landmarkID << " " << edge->getID() << " " << distFrom << " " << distTo << "\n";
240  }
241  currentTasks.clear();
242  continue;
243  }
244 #endif
245  for (int j = (int)myFromLandmarkDists[i].size() + myFirstNonInternal; j < (int)edges.size(); ++j) {
246  const E* edge = edges[j];
247  double distFrom = -1.;
248  double distTo = -1.;
249  if (landmark == edge) {
250  distFrom = 0.;
251  distTo = 0.;
252  } else {
253  std::vector<const E*> routeE(1, edge);
254  const double sourceDestCost = lmCost + router->recomputeCosts(routeE, defaultVehicle, 0);
255  // compute from-distance (skip taz-sources and other unreachable edges)
256  if (edge->getPredecessors().size() > 0 && landmark->getSuccessors().size() > 0) {
257  if (router->compute(landmark, edge, defaultVehicle, 0, route)) {
258  distFrom = MAX2(0.0, router->recomputeCosts(route, defaultVehicle, 0) - sourceDestCost);
259  route.clear();
260  }
261  }
262  // compute to-distance (skip unreachable landmarks)
263  if (landmark->getPredecessors().size() > 0 && edge->getSuccessors().size() > 0) {
264  if (router->compute(edge, landmark, defaultVehicle, 0, route)) {
265  distTo = MAX2(0.0, router->recomputeCosts(route, defaultVehicle, 0) - sourceDestCost);
266  route.clear();
267  }
268  }
269  }
270  myFromLandmarkDists[i].push_back(distFrom);
271  myToLandmarkDists[i].push_back(distTo);
272  (*ostrm) << landmarkID << " " << edge->getID() << " " << distFrom << " " << distTo << "\n";
273  }
274  }
275  }
276  delete ostrm;
277  }
278 
279  double lowerBound(const E* from, const E* to, double speed, double speedFactor, double fromEffort, double toEffort) const {
280  double result = from->getDistanceTo(to) / speed;
281 #ifdef ASTAR_DEBUG_LOOKUPTABLE
282  if (from->getID() == ASTAR_DEBUG_LOOKUPTABLE_FROM) {
283  std::cout << " lowerBound to=" << to->getID() << " result1=" << result << "\n";
284  }
285 #endif
286  for (int i = 0; i < (int)myLandmarks.size(); ++i) {
287  // a cost of -1 is used to encode unreachability.
288  const double fl = myToLandmarkDists[i][from->getNumericalID() - myFirstNonInternal];
289  const double tl = myToLandmarkDists[i][to->getNumericalID() - myFirstNonInternal];
290  if (fl >= 0 && tl >= 0) {
291  const double bound = (fl - tl - toEffort) / speedFactor;
292 #ifdef ASTAR_DEBUG_LOOKUPTABLE
293  if (from->getID() == ASTAR_DEBUG_LOOKUPTABLE_FROM && result < bound) {
294  std::cout << " landmarkTo=" << getLandmark(i) << " result2=" << bound
295  << " fl=" << fl << " tl=" << tl << "\n";
296  }
297 #endif
298  result = MAX2(result, bound);
299  }
300  const double lt = myFromLandmarkDists[i][to->getNumericalID() - myFirstNonInternal];
301  const double lf = myFromLandmarkDists[i][from->getNumericalID() - myFirstNonInternal];
302  if (lt >= 0 && lf >= 0) {
303  const double bound = (lt - lf - fromEffort) / speedFactor;
304 #ifdef ASTAR_DEBUG_LOOKUPTABLE
305  if (from->getID() == ASTAR_DEBUG_LOOKUPTABLE_FROM && result < bound) {
306  std::cout << " landmarkFrom=" << getLandmark(i) << " result3=" << bound
307  << " lt=" << lt << " lf=" << lf << "\n";
308  }
309 #endif
310  result = MAX2(result, bound);
311  }
312  if ((tl >= 0 && fl < 0)
313  || (lf >= 0 && lt < 0)) {
314  // target unreachable.
315 #ifdef ASTAR_DEBUG_UNREACHABLE
316  std::cout << " unreachable: from=" << from->getID() << " to=" << to->getID() << " landmark=" << getLandmark(i) << " "
317  << ((tl >= 0 && fl < 0) ? " (toLandmark)" : " (fromLandmark)")
318  << " fl=" << fl << " tl=" << tl << " lt=" << lt << " lf=" << lf
319  << "\n";
320 #endif
321  return UNREACHABLE;
322  }
323  }
324  return result;
325  }
326 
327  bool consistent() const {
328  return false;
329  }
330 
331 private:
332  std::map<std::string, int> myLandmarks;
333  std::vector<std::vector<double> > myFromLandmarkDists;
334  std::vector<std::vector<double> > myToLandmarkDists;
336 
337 #ifdef HAVE_FOX
338 private:
339  class WorkerThread : public FXWorkerThread {
340  public:
341  WorkerThread(FXWorkerThread::Pool& pool,
342  SUMOAbstractRouter<E, V>* router, const V* vehicle)
343  : FXWorkerThread(pool), myRouter(router), myVehicle(vehicle) {}
344  virtual ~WorkerThread() {
345  delete myRouter;
346  }
347  double compute(const E* src, const E* dest, const double costOff) {
348  double result = -1.;
349  if (myRouter->compute(src, dest, myVehicle, 0, myRoute)) {
350  result = MAX2(0.0, myRouter->recomputeCosts(myRoute, myVehicle, 0) + costOff);
351  myRoute.clear();
352  }
353  return result;
354  }
355  private:
356  SUMOAbstractRouter<E, V>* myRouter;
357  const V* myVehicle;
358  std::vector<const E*> myRoute;
359  };
360 
361  class RoutingTask : public FXWorkerThread::Task {
362  public:
363  RoutingTask(const E* src, const E* dest, const double costOff)
364  : mySrc(src), myDest(dest), myCost(-costOff) {}
365  void run(FXWorkerThread* context) {
366  myCost = ((WorkerThread*)context)->compute(mySrc, myDest, myCost);
367  }
368  double getCost() {
369  return myCost;
370  }
371  private:
372  const E* const mySrc;
373  const E* const myDest;
374  double myCost;
375  private:
377  RoutingTask& operator=(const RoutingTask&);
378  };
379 
380 
381 private:
383 #endif
384 
385  std::string getLandmark(int i) const {
386  for (std::map<std::string, int>::const_iterator it = myLandmarks.begin(); it != myLandmarks.end(); ++it) {
387  if (it->second == i) {
388  return it->first;
389  }
390  }
391  return "";
392  }
393 };
394 
395 
396 
397 
398 #endif
399 
400 /****************************************************************************/
401 
void waitAll(const bool deleteFinished=true)
waits for all tasks to be finished
bool consistent() const
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 speed, double speedFactor, double fromEffort, double toEffort) const
provide a lower bound on the distance between from and to (excluding traveltime of both edges) ...
virtual bool compute(const E *from, const E *to, const V *const vehicle, SUMOTime msTime, std::vector< const E *> &into)=0
Builds the route between the given edges using the minimum effort at the given time The definition of...
std::string get(int pos) const
T MAX2(T a, T b)
Definition: StdDefs.h:76
#define WRITE_WARNING(msg)
Definition: MsgHandler.h:241
#define UNREACHABLE
int size() const
Returns the number of threads in the pool.
std::vector< std::vector< double > > myToLandmarkDists
static double toDouble(const std::string &sData)
converts a string into the double value described by it by calling the char-type converter ...
FullLookupTable(const std::string &filename, const int size)
std::map< std::string, int > myLandmarks
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) ...
virtual bool consistent() const =0
whether the heuristic ist consistent (found nodes are always visited on the shortest path the first t...
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. If the index is negative, assign to the next (round robin) one.
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) ...
A pool of worker threads which distributes the tasks and collects the results.
std::vector< std::vector< double > > myTable
std::string getLandmark(int i) const
Abstract superclass of a task to be run with an index to keep track of pending tasks.
double recomputeCosts(const std::vector< const E *> &edges, const V *const v, SUMOTime msTime) const
A thread repeatingly calculating incoming tasks.
Computes the shortest path through a network using the A* algorithm.
virtual SUMOAbstractRouter * clone()=0
LandmarkLookupTable(const std::string &filename, const std::vector< E *> &edges, SUMOAbstractRouter< E, V > *router, const V *defaultVehicle, const std::string &outfile, const int maxNumThreads)
Encapsulates binary reading operations on a file.
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...