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