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