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-2020 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_FOX
27 #endif
29 
30 #define UNREACHABLE (std::numeric_limits<double>::max() / 1000.0)
31 
32 //#define ASTAR_DEBUG_LOOKUPTABLE
33 //#define ASTAR_DEBUG_LOOKUPTABLE_FROM "disabled"
34 //#define ASTAR_DEBUG_UNREACHABLE
35 
36 // ===========================================================================
37 // class definitions
38 // ===========================================================================
55 template<class E, class V>
57 public:
59  virtual double lowerBound(const E* from, const E* to, double speed, double speedFactor, double fromEffort, double toEffort) const = 0;
60 
62  virtual bool consistent() const = 0;
63 };
64 
65 
66 template<class E, class V>
67 class FullLookupTable : public AbstractLookupTable<E, V> {
68 public:
69  FullLookupTable(const std::string& filename, const int size) :
70  myTable(size) {
71  std::ifstream strm(filename.c_str());
72  for (int i = 0; i < size; i++) {
73  for (int j = 0; j < size; j++) {
74  double val;
75  strm >> val;
76  myTable[i].push_back(val);
77  }
78  }
79  }
80 
81  virtual ~FullLookupTable() {
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,
101  SUMOAbstractRouter<ReversedEdge<E, V>, V>* reverseRouter,
102  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  std::vector<RoutingTask*> currentTasks;
161 #endif
162  std::vector<const E*> landmarks;
163  for (int i = 0; i < (int)myLandmarks.size(); ++i) {
164  if ((int)myFromLandmarkDists[i].size() != (int)edges.size() - myFirstNonInternal) {
165  const std::string landmarkID = getLandmark(i);
166  const E* landmark = nullptr;
167  // retrieve landmark edge
168  for (const E* const edge : edges) {
169  if (edge->getID() == landmarkID) {
170  landmark = edge;
171  landmarks.push_back(edge);
172  break;
173  }
174  }
175  if (landmark == nullptr) {
176  WRITE_WARNING("Landmark '" + landmarkID + "' does not exist in the network.");
177  continue;
178  }
179  if (router != nullptr) {
180  const std::string missing = outfile.empty() ? filename + ".missing" : outfile;
181  WRITE_WARNING("Not all network edges were found in the lookup table '" + filename + "' for landmark '" + landmarkID + "'. Saving missing values to '" + missing + "'.");
182  if (ostrm == nullptr) {
183  ostrm = new std::ofstream(missing.c_str());
184  if (!ostrm->good()) {
185  throw ProcessError("Could not open file '" + missing + "' for writing.");
186  }
187  }
188  } else {
189  throw ProcessError("Not all network edges were found in the lookup table '" + filename + "' for landmark '" + landmarkID + "'.");
190  }
191 #ifdef HAVE_FOX
192  if (maxNumThreads > 0) {
193  const double lmCost = router->recomputeCosts({landmark}, defaultVehicle, 0);
194  router->setAutoBulkMode(true);
195  if (threadPool.size() == 0) {
196  if (reverseRouter == nullptr) {
197  // The CHRouter needs initialization
198  // before it gets cloned, so we do a dummy routing which is not in parallel
199  std::vector<const E*> route;
200  router->compute(landmark, landmark, defaultVehicle, 0, route);
201  } else {
202  reverseRouter->setAutoBulkMode(true);
203  }
204  while ((int)threadPool.size() < maxNumThreads) {
205  auto revClone = reverseRouter == nullptr ? nullptr : reverseRouter->clone();
206  new WorkerThread(threadPool, router->clone(), revClone, defaultVehicle);
207  }
208  }
209  for (int j = (int)myFromLandmarkDists[i].size() + myFirstNonInternal; j < (int)edges.size(); ++j) {
210  const E* const edge = edges[j];
211  if (landmark != edge) {
212  const double sourceDestCost = lmCost + router->recomputeCosts({edge}, defaultVehicle, 0);
213  currentTasks.push_back(new RoutingTask(landmark, edge, sourceDestCost));
214  threadPool.add(currentTasks.back(), i % maxNumThreads);
215  }
216  }
217  }
218 #endif
219  }
220  }
221 #ifdef HAVE_FOX
222  threadPool.waitAll(false);
223  int taskIndex = 0;
224 #endif
225  for (int i = 0; i < (int)myLandmarks.size(); ++i) {
226  if ((int)myFromLandmarkDists[i].size() != (int)edges.size() - myFirstNonInternal) {
227  const E* landmark = landmarks[i];
228  const double lmCost = router->recomputeCosts({landmark}, defaultVehicle, 0);
229  for (int j = (int)myFromLandmarkDists[i].size() + myFirstNonInternal; j < (int)edges.size(); ++j) {
230  const E* edge = edges[j];
231  double distFrom = -1;
232  double distTo = -1;
233  if (landmark == edge) {
234  distFrom = 0;
235  distTo = 0;
236  } else {
237  if (maxNumThreads > 0) {
238 #ifdef HAVE_FOX
239  distFrom = currentTasks[taskIndex]->getFromCost();
240  distTo = currentTasks[taskIndex]->getToCost();
241  delete currentTasks[taskIndex++];
242 #endif
243  } else {
244  const double sourceDestCost = lmCost + router->recomputeCosts({edge}, defaultVehicle, 0);
245  std::vector<const E*> route;
246  std::vector<const ReversedEdge<E, V>*> reversedRoute;
247  // compute from-distance (skip taz-sources and other unreachable edges)
248  if (edge->getPredecessors().size() > 0 && landmark->getSuccessors().size() > 0) {
249  if (router->compute(landmark, edge, defaultVehicle, 0, route)) {
250  distFrom = MAX2(0.0, router->recomputeCosts(route, defaultVehicle, 0) - sourceDestCost);
251  route.clear();
252  }
253  }
254  // compute to-distance (skip unreachable landmarks)
255  if (landmark->getPredecessors().size() > 0 && edge->getSuccessors().size() > 0) {
256  if (router->compute(edge, landmark, defaultVehicle, 0, route)) {
257  distTo = MAX2(0.0, router->recomputeCosts(route, defaultVehicle, 0) - sourceDestCost);
258  route.clear();
259  }
260  }
261  }
262  }
263  myFromLandmarkDists[i].push_back(distFrom);
264  myToLandmarkDists[i].push_back(distTo);
265  (*ostrm) << landmark->getID() << " " << edge->getID() << " " << distFrom << " " << distTo << "\n";
266  }
267  }
268  }
269  delete ostrm;
270  }
271 
273  }
274 
275  double lowerBound(const E* from, const E* to, double speed, double speedFactor, double fromEffort, double toEffort) const {
276  double result = from->getDistanceTo(to) / speed;
277 #ifdef ASTAR_DEBUG_LOOKUPTABLE
278  if (from->getID() == ASTAR_DEBUG_LOOKUPTABLE_FROM) {
279  std::cout << " lowerBound to=" << to->getID() << " result1=" << result << "\n";
280  }
281 #endif
282  for (int i = 0; i < (int)myLandmarks.size(); ++i) {
283  // a cost of -1 is used to encode unreachability.
284  const double fl = myToLandmarkDists[i][from->getNumericalID() - myFirstNonInternal];
285  const double tl = myToLandmarkDists[i][to->getNumericalID() - myFirstNonInternal];
286  if (fl >= 0 && tl >= 0) {
287  const double bound = (fl - tl - toEffort) / speedFactor;
288 #ifdef ASTAR_DEBUG_LOOKUPTABLE
289  if (from->getID() == ASTAR_DEBUG_LOOKUPTABLE_FROM && result < bound) {
290  std::cout << " landmarkTo=" << getLandmark(i) << " result2=" << bound
291  << " fl=" << fl << " tl=" << tl << "\n";
292  }
293 #endif
294  result = MAX2(result, bound);
295  }
296  const double lt = myFromLandmarkDists[i][to->getNumericalID() - myFirstNonInternal];
297  const double lf = myFromLandmarkDists[i][from->getNumericalID() - myFirstNonInternal];
298  if (lt >= 0 && lf >= 0) {
299  const double bound = (lt - lf - fromEffort) / speedFactor;
300 #ifdef ASTAR_DEBUG_LOOKUPTABLE
301  if (from->getID() == ASTAR_DEBUG_LOOKUPTABLE_FROM && result < bound) {
302  std::cout << " landmarkFrom=" << getLandmark(i) << " result3=" << bound
303  << " lt=" << lt << " lf=" << lf << "\n";
304  }
305 #endif
306  result = MAX2(result, bound);
307  }
308  if ((tl >= 0 && fl < 0)
309  || (lf >= 0 && lt < 0)) {
310  // target unreachable.
311 #ifdef ASTAR_DEBUG_UNREACHABLE
312  std::cout << " unreachable: from=" << from->getID() << " to=" << to->getID() << " landmark=" << getLandmark(i) << " "
313  << ((tl >= 0 && fl < 0) ? " (toLandmark)" : " (fromLandmark)")
314  << " fl=" << fl << " tl=" << tl << " lt=" << lt << " lf=" << lf
315  << "\n";
316 #endif
317  return UNREACHABLE;
318  }
319  }
320  return result;
321  }
322 
323  bool consistent() const {
324  return false;
325  }
326 
327 private:
328  std::map<std::string, int> myLandmarks;
329  std::vector<std::vector<double> > myFromLandmarkDists;
330  std::vector<std::vector<double> > myToLandmarkDists;
332 
333 #ifdef HAVE_FOX
334 private:
335  class WorkerThread : public FXWorkerThread {
336  public:
337  WorkerThread(FXWorkerThread::Pool& pool,
338  SUMOAbstractRouter<E, V>* router,
339  SUMOAbstractRouter<ReversedEdge<E, V>, V>* reverseRouter, const V* vehicle)
340  : FXWorkerThread(pool), myRouter(router), myReversedRouter(reverseRouter), myVehicle(vehicle) {}
341 
342  virtual ~WorkerThread() {
343  delete myRouter;
344  }
345 
346  const std::pair<double, double> compute(const E* src, const E* dest, const double costOff) {
347  double fromResult = -1.;
348  if (myRouter->compute(src, dest, myVehicle, 0, myRoute)) {
349  fromResult = MAX2(0.0, myRouter->recomputeCosts(myRoute, myVehicle, 0) + costOff);
350  myRoute.clear();
351  }
352  double toResult = -1.;
353  if (myReversedRouter != nullptr) {
354  if (myReversedRouter->compute(src->getReversedRoutingEdge(), dest->getReversedRoutingEdge(), myVehicle, 0, myReversedRoute)) {
355  toResult = MAX2(0.0, myReversedRouter->recomputeCosts(myReversedRoute, myVehicle, 0) + costOff);
356  myReversedRoute.clear();
357  }
358  } else {
359  if (myRouter->compute(dest, src, myVehicle, 0, myRoute)) {
360  toResult = MAX2(0.0, myRouter->recomputeCosts(myRoute, myVehicle, 0) + costOff);
361  myRoute.clear();
362  }
363  }
364  return std::make_pair(fromResult, toResult);
365  }
366 
367  private:
368  SUMOAbstractRouter<E, V>* myRouter;
369  SUMOAbstractRouter<ReversedEdge<E, V>, V>* myReversedRouter;
370  const V* myVehicle;
371  std::vector<const E*> myRoute;
372  std::vector<const ReversedEdge<E, V>*> myReversedRoute;
373  };
374 
375  class RoutingTask : public FXWorkerThread::Task {
376  public:
377  RoutingTask(const E* src, const E* dest, const double costOff)
378  : mySrc(src), myDest(dest), myCostOff(-costOff) {}
379  void run(FXWorkerThread* context) {
380  myCost = ((WorkerThread*)context)->compute(mySrc, myDest, myCostOff);
381  }
382  double getFromCost() {
383  return myCost.first;
384  }
385  double getToCost() {
386  return myCost.second;
387  }
388  private:
389  const E* const mySrc;
390  const E* const myDest;
391  const double myCostOff;
392  std::pair<double, double> myCost;
393  private:
395  RoutingTask& operator=(const RoutingTask&) = delete;
396  };
397 
398 private:
400 #endif
401 
402  std::string getLandmark(int i) const {
403  for (std::map<std::string, int>::const_iterator it = myLandmarks.begin(); it != myLandmarks.end(); ++it) {
404  if (it->second == i) {
405  return it->first;
406  }
407  }
408  return "";
409  }
410 };
#define UNREACHABLE
#define WRITE_WARNING(msg)
Definition: MsgHandler.h:276
T MAX2(T a, T b)
Definition: StdDefs.h:79
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 bool consistent() const =0
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 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.
void waitAll(const bool deleteFinished=true)
waits for all tasks to be finished
Abstract superclass of a task to be run with an index to keep track of pending tasks.
A thread repeatingly calculating incoming tasks.
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...
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