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-2022 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_ZLIB
26#include <foreign/zstr/zstr.hpp>
27#endif
28#ifdef HAVE_FOX
30#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// ===========================================================================
58template<class E, class V>
60public:
62
64 virtual double lowerBound(const E* from, const E* to, double speed, double speedFactor, double fromEffort, double toEffort) const = 0;
65
67 virtual bool consistent() const = 0;
68};
69
70
71template<class E, class V>
73public:
74 FullLookupTable(const std::string& filename, const int size) :
75 myTable(size) {
76 std::ifstream strm(filename.c_str());
77 for (int i = 0; i < size; i++) {
78 for (int j = 0; j < size; j++) {
79 double val;
80 strm >> val;
81 myTable[i].push_back(val);
82 }
83 }
84 }
85
86 virtual ~FullLookupTable() {
87 }
88
89 double lowerBound(const E* from, const E* to, double /*speed*/, double speedFactor, double /*fromEffort*/, double /*toEffort*/) const {
90 return myTable[from->getNumericalID()][to->getNumericalID()] / speedFactor;
91 }
92
93 bool consistent() const {
94 return true;
95 }
96
97private:
98 std::vector<std::vector<double> > myTable;
99};
100
101
102template<class E, class V>
104public:
105 LandmarkLookupTable(const std::string& filename, const std::vector<E*>& edges, SUMOAbstractRouter<E, V>* router,
106 SUMOAbstractRouter<ReversedEdge<E, V>, V>* reverseRouter,
107 const V* defaultVehicle, const std::string& outfile, const int maxNumThreads) {
109 std::map<std::string, int> numericID;
110 for (E* e : edges) {
111 if (!e->isInternal()) {
112 if (myFirstNonInternal == -1) {
113 myFirstNonInternal = e->getNumericalID();
114 }
115 numericID[e->getID()] = e->getNumericalID() - myFirstNonInternal;
116 }
117 }
118#ifdef HAVE_ZLIB
119 zstr::ifstream strm(filename.c_str(), std::fstream::in | std::fstream::binary);
120#else
121 std::ifstream strm(filename.c_str());
122#endif
123 if (!strm.good()) {
124 throw ProcessError("Could not load landmark-lookup-table from '" + filename + "'.");
125 }
126 std::string line;
127 int numLandMarks = 0;
128 while (std::getline(strm, line)) {
129 if (line == "") {
130 break;
131 }
132 //std::cout << "'" << line << "'" << "\n";
133 StringTokenizer st(line);
134 if (st.size() == 1) {
135 const std::string lm = st.get(0);
136 myLandmarks[lm] = numLandMarks++;
137 myFromLandmarkDists.push_back(std::vector<double>(0));
138 myToLandmarkDists.push_back(std::vector<double>(0));
139 } else if (st.size() == 4) {
140 // legacy style landmark table
141 const std::string lm = st.get(0);
142 const std::string edge = st.get(1);
143 if (numericID[edge] != (int)myFromLandmarkDists[myLandmarks[lm]].size()) {
144 WRITE_WARNING("Unknown or unordered edge '" + edge + "' in landmark file.");
145 }
146 const double distFrom = StringUtils::toDouble(st.get(2));
147 const double distTo = StringUtils::toDouble(st.get(3));
148 myFromLandmarkDists[myLandmarks[lm]].push_back(distFrom);
149 myToLandmarkDists[myLandmarks[lm]].push_back(distTo);
150 } else {
151 assert((int)st.size() == 2 * numLandMarks + 1);
152 const std::string edge = st.get(0);
153 if (numericID[edge] != (int)myFromLandmarkDists[0].size()) {
154 WRITE_WARNING("Unknown or unordered edge '" + edge + "' in landmark file.");
155 }
156 for (int i = 0; i < numLandMarks; i++) {
157 const double distFrom = StringUtils::toDouble(st.get(2 * i + 1));
158 const double distTo = StringUtils::toDouble(st.get(2 * i + 2));
159 myFromLandmarkDists[i].push_back(distFrom);
160 myToLandmarkDists[i].push_back(distTo);
161 }
162 }
163 }
164 if (myLandmarks.empty()) {
165 WRITE_WARNING("No landmarks in '" + filename + "', falling back to standard A*.");
166 return;
167 }
168#ifdef HAVE_FOX
169 MFXWorkerThread::Pool threadPool;
170 std::vector<RoutingTask*> currentTasks;
171#endif
172 std::vector<const E*> landmarks;
173 for (int i = 0; i < numLandMarks; ++i) {
174 if ((int)myFromLandmarkDists[i].size() != (int)edges.size() - myFirstNonInternal) {
175 const std::string landmarkID = getLandmark(i);
176 const E* landmark = nullptr;
177 // retrieve landmark edge
178 for (const E* const edge : edges) {
179 if (edge->getID() == landmarkID) {
180 landmark = edge;
181 landmarks.push_back(edge);
182 break;
183 }
184 }
185 if (landmark == nullptr) {
186 WRITE_WARNING("Landmark edge '" + landmarkID + "' does not exist in the network.");
187 continue;
188 }
189 if (!outfile.empty()) {
190 WRITE_WARNING("Not all network edges were found in the lookup table '" + filename + "' for landmark edge '" + landmarkID + "'. Saving new matrix to '" + outfile + "'.");
191 } else {
192 if (myFromLandmarkDists[i].empty()) {
193 WRITE_WARNING("No lookup table for landmark edge '" + landmarkID + "', recalculating.");
194 } else {
195 throw ProcessError("Not all network edges were found in the lookup table '" + filename + "' for landmark edge '" + landmarkID + "'.");
196 }
197 }
198#ifdef HAVE_FOX
199 if (maxNumThreads > 0) {
200 const double lmCost = router->recomputeCosts({landmark}, defaultVehicle, 0);
201 router->setAutoBulkMode(true);
202 if (threadPool.size() == 0) {
203 if (reverseRouter == nullptr) {
204 // The CHRouter needs initialization
205 // before it gets cloned, so we do a dummy routing which is not in parallel
206 std::vector<const E*> route;
207 router->compute(landmark, landmark, defaultVehicle, 0, route);
208 } else {
209 reverseRouter->setAutoBulkMode(true);
210 }
211 while ((int)threadPool.size() < maxNumThreads) {
212 auto revClone = reverseRouter == nullptr ? nullptr : reverseRouter->clone();
213 new WorkerThread(threadPool, router->clone(), revClone, defaultVehicle);
214 }
215 }
216 for (int j = (int)myFromLandmarkDists[i].size() + myFirstNonInternal; j < (int)edges.size(); ++j) {
217 const E* const edge = edges[j];
218 if (landmark != edge) {
219 const double sourceDestCost = lmCost + router->recomputeCosts({edge}, defaultVehicle, 0);
220 currentTasks.push_back(new RoutingTask(landmark, edge, sourceDestCost));
221 threadPool.add(currentTasks.back(), i % maxNumThreads);
222 }
223 }
224 }
225#else
226 UNUSED_PARAMETER(reverseRouter);
227#endif
228 }
229 }
230#ifdef HAVE_FOX
231 threadPool.waitAll(false);
232 int taskIndex = 0;
233#endif
234 for (int i = 0; i < numLandMarks; ++i) {
235 if ((int)myFromLandmarkDists[i].size() != (int)edges.size() - myFirstNonInternal) {
236 const E* landmark = landmarks[i];
237 const double lmCost = router->recomputeCosts({landmark}, defaultVehicle, 0);
238 for (int j = (int)myFromLandmarkDists[i].size() + myFirstNonInternal; j < (int)edges.size(); ++j) {
239 const E* edge = edges[j];
240 double distFrom = -1;
241 double distTo = -1;
242 if (landmark == edge) {
243 distFrom = 0;
244 distTo = 0;
245 } else {
246 if (maxNumThreads > 0) {
247#ifdef HAVE_FOX
248 distFrom = currentTasks[taskIndex]->getFromCost();
249 distTo = currentTasks[taskIndex]->getToCost();
250 delete currentTasks[taskIndex++];
251#endif
252 } else {
253 const double sourceDestCost = lmCost + router->recomputeCosts({edge}, defaultVehicle, 0);
254 std::vector<const E*> route;
255 std::vector<const ReversedEdge<E, V>*> reversedRoute;
256 // compute from-distance (skip taz-sources and other unreachable edges)
257 if (edge->getPredecessors().size() > 0 && landmark->getSuccessors().size() > 0) {
258 if (router->compute(landmark, edge, defaultVehicle, 0, route)) {
259 distFrom = MAX2(0.0, router->recomputeCosts(route, defaultVehicle, 0) - sourceDestCost);
260 route.clear();
261 }
262 }
263 // compute to-distance (skip unreachable landmarks)
264 if (landmark->getPredecessors().size() > 0 && edge->getSuccessors().size() > 0) {
265 if (router->compute(edge, landmark, defaultVehicle, 0, route)) {
266 distTo = MAX2(0.0, router->recomputeCosts(route, defaultVehicle, 0) - sourceDestCost);
267 route.clear();
268 }
269 }
270 }
271 }
272 myFromLandmarkDists[i].push_back(distFrom);
273 myToLandmarkDists[i].push_back(distTo);
274 }
275 }
276 }
277 if (!outfile.empty()) {
278 std::ostream* ostrm = nullptr;
279#ifdef HAVE_ZLIB
280 if (StringUtils::endsWith(outfile, ".gz")) {
281 ostrm = new zstr::ofstream(outfile.c_str(), std::ios_base::out);
282 } else {
283#endif
284 ostrm = new std::ofstream(outfile.c_str());
285#ifdef HAVE_ZLIB
286 }
287#endif
288 if (!ostrm->good()) {
289 delete ostrm;
290 throw ProcessError("Could not open file '" + outfile + "' for writing.");
291 }
292 for (int i = 0; i < numLandMarks; ++i) {
293 (*ostrm) << getLandmark(i) << "\n";
294 }
295 for (int j = 0; j < (int)myFromLandmarkDists[0].size(); ++j) {
296 (*ostrm) << edges[j + myFirstNonInternal]->getID();
297 for (int i = 0; i < numLandMarks; ++i) {
298 (*ostrm) << " " << myFromLandmarkDists[i][j] << " " << myToLandmarkDists[i][j];
299 }
300 (*ostrm) << "\n";
301 }
302 delete ostrm;
303 }
304 }
305
307 }
308
309 double lowerBound(const E* from, const E* to, double speed, double speedFactor, double fromEffort, double toEffort) const {
310 double result = from->getDistanceTo(to) / speed;
311#ifdef ASTAR_DEBUG_LOOKUPTABLE
312 if (from->getID() == ASTAR_DEBUG_LOOKUPTABLE_FROM) {
313 std::cout << " lowerBound to=" << to->getID() << " result1=" << result << "\n";
314 }
315#endif
316 for (int i = 0; i < (int)myLandmarks.size(); ++i) {
317 // a cost of -1 is used to encode unreachability.
318 const double fl = myToLandmarkDists[i][from->getNumericalID() - myFirstNonInternal];
319 const double tl = myToLandmarkDists[i][to->getNumericalID() - myFirstNonInternal];
320 if (fl >= 0 && tl >= 0) {
321 const double bound = (fl - tl - toEffort) / speedFactor;
322#ifdef ASTAR_DEBUG_LOOKUPTABLE
323 if (from->getID() == ASTAR_DEBUG_LOOKUPTABLE_FROM && result < bound) {
324 std::cout << " landmarkTo=" << getLandmark(i) << " result2=" << bound
325 << " fl=" << fl << " tl=" << tl << "\n";
326 }
327#endif
328 result = MAX2(result, bound);
329 }
330 const double lt = myFromLandmarkDists[i][to->getNumericalID() - myFirstNonInternal];
331 const double lf = myFromLandmarkDists[i][from->getNumericalID() - myFirstNonInternal];
332 if (lt >= 0 && lf >= 0) {
333 const double bound = (lt - lf - fromEffort) / speedFactor;
334#ifdef ASTAR_DEBUG_LOOKUPTABLE
335 if (from->getID() == ASTAR_DEBUG_LOOKUPTABLE_FROM && result < bound) {
336 std::cout << " landmarkFrom=" << getLandmark(i) << " result3=" << bound
337 << " lt=" << lt << " lf=" << lf << "\n";
338 }
339#endif
340 result = MAX2(result, bound);
341 }
342 if ((tl >= 0 && fl < 0)
343 || (lf >= 0 && lt < 0)) {
344 // target unreachable.
345#ifdef ASTAR_DEBUG_UNREACHABLE
346 std::cout << " unreachable: from=" << from->getID() << " to=" << to->getID() << " landmark=" << getLandmark(i) << " "
347 << ((tl >= 0 && fl < 0) ? " (toLandmark)" : " (fromLandmark)")
348 << " fl=" << fl << " tl=" << tl << " lt=" << lt << " lf=" << lf
349 << "\n";
350#endif
351 return UNREACHABLE;
352 }
353 }
354 return result;
355 }
356
357 bool consistent() const {
358 return false;
359 }
360
361private:
362 std::map<std::string, int> myLandmarks;
363 std::vector<std::vector<double> > myFromLandmarkDists;
364 std::vector<std::vector<double> > myToLandmarkDists;
366
367#ifdef HAVE_FOX
368private:
369 class WorkerThread : public MFXWorkerThread {
370 public:
371 WorkerThread(MFXWorkerThread::Pool& pool,
373 SUMOAbstractRouter<ReversedEdge<E, V>, V>* reverseRouter, const V* vehicle)
374 : MFXWorkerThread(pool), myRouter(router), myReversedRouter(reverseRouter), myVehicle(vehicle) {}
375
376 virtual ~WorkerThread() {
377 delete myRouter;
378 }
379
380 const std::pair<double, double> compute(const E* src, const E* dest, const double costOff) {
381 double fromResult = -1.;
382 if (myRouter->compute(src, dest, myVehicle, 0, myRoute)) {
383 fromResult = MAX2(0.0, myRouter->recomputeCosts(myRoute, myVehicle, 0) + costOff);
384 myRoute.clear();
385 }
386 double toResult = -1.;
387 if (myReversedRouter != nullptr) {
388 if (myReversedRouter->compute(src->getReversedRoutingEdge(), dest->getReversedRoutingEdge(), myVehicle, 0, myReversedRoute)) {
389 toResult = MAX2(0.0, myReversedRouter->recomputeCosts(myReversedRoute, myVehicle, 0) + costOff);
390 myReversedRoute.clear();
391 }
392 } else {
393 if (myRouter->compute(dest, src, myVehicle, 0, myRoute)) {
394 toResult = MAX2(0.0, myRouter->recomputeCosts(myRoute, myVehicle, 0) + costOff);
395 myRoute.clear();
396 }
397 }
398 return std::make_pair(fromResult, toResult);
399 }
400
401 private:
402 SUMOAbstractRouter<E, V>* myRouter;
403 SUMOAbstractRouter<ReversedEdge<E, V>, V>* myReversedRouter;
404 const V* myVehicle;
405 std::vector<const E*> myRoute;
406 std::vector<const ReversedEdge<E, V>*> myReversedRoute;
407 };
408
409 class RoutingTask : public MFXWorkerThread::Task {
410 public:
411 RoutingTask(const E* src, const E* dest, const double costOff)
412 : mySrc(src), myDest(dest), myCostOff(-costOff) {}
413 void run(MFXWorkerThread* context) {
414 myCost = ((WorkerThread*)context)->compute(mySrc, myDest, myCostOff);
415 }
416 double getFromCost() {
417 return myCost.first;
418 }
419 double getToCost() {
420 return myCost.second;
421 }
422 private:
423 const E* const mySrc;
424 const E* const myDest;
425 const double myCostOff;
426 std::pair<double, double> myCost;
427 private:
429 RoutingTask& operator=(const RoutingTask&) = delete;
430 };
431
432private:
434#endif
435
436 std::string getLandmark(int i) const {
437 for (std::map<std::string, int>::const_iterator it = myLandmarks.begin(); it != myLandmarks.end(); ++it) {
438 if (it->second == i) {
439 return it->first;
440 }
441 }
442 return "";
443 }
444};
#define UNREACHABLE
#define WRITE_WARNING(msg)
Definition: MsgHandler.h:265
#define UNUSED_PARAMETER(x)
Definition: StdDefs.h:30
T MAX2(T a, T b)
Definition: StdDefs.h:77
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 ~AbstractLookupTable()
virtual bool consistent() const =0
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, 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...
A pool of worker threads which distributes the tasks and collects the results.
void waitAll(const bool deleteFinished=true)
waits for all tasks to be finished
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.
Abstract superclass of a task to be run with an index to keep track of pending tasks.
A thread repeatingly calculating incoming tasks.
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
static bool endsWith(const std::string &str, const std::string suffix)
Checks whether a given string ends with the suffix.