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