Line data Source code
1 : /****************************************************************************/
2 : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3 : // Copyright (C) 2012-2025 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 : /****************************************************************************/
14 : /// @file AStarLookupTable.h
15 : /// @author Jakob Erdmann
16 : /// @date July 2017
17 : ///
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
29 : #include <utils/foxtools/MFXWorkerThread.h>
30 : #endif
31 : #include <utils/common/MapMatcher.h>
32 : #include <utils/geom/PositionVector.h>
33 : #include <utils/router/ReversedEdge.h>
34 : #include <utils/router/SUMOAbstractRouter.h>
35 :
36 : #define UNREACHABLE (std::numeric_limits<double>::max() / 1000.0)
37 :
38 : //#define ASTAR_DEBUG_LOOKUPTABLE
39 : //#define ASTAR_DEBUG_LOOKUPTABLE_FROM "disabled"
40 : //#define ASTAR_DEBUG_UNREACHABLE
41 :
42 : // ===========================================================================
43 : // class definitions
44 : // ===========================================================================
45 : /**
46 : * @class LandmarkLookupTable
47 : * @brief Computes the shortest path through a network using the A* algorithm.
48 : *
49 : * The template parameters are:
50 : * @param E The edge class to use (MSEdge/ROEdge)
51 : * @param V The vehicle class to use (MSVehicle/ROVehicle)
52 : * @param PF The prohibition function to use (prohibited_withPermissions/noProhibitions)
53 : * @param EC The class to retrieve the effort for an edge from
54 : *
55 : * The router is edge-based. It must know the number of edges for internal reasons
56 : * and whether a missing connection between two given edges (unbuild route) shall
57 : * be reported as an error or as a warning.
58 : *
59 : */
60 :
61 : template<class E, class V>
62 : class AbstractLookupTable {
63 : public:
64 : virtual ~AbstractLookupTable() {}
65 :
66 : /// @brief provide a lower bound on the distance between from and to (excluding traveltime of both edges)
67 : virtual double lowerBound(const E* from, const E* to, double speed, double speedFactor, double fromEffort, double toEffort) const = 0;
68 :
69 : /// @brief whether the heuristic ist consistent (found nodes are always visited on the shortest path the first time)
70 : virtual bool consistent() const = 0;
71 : };
72 :
73 :
74 : template<class E, class V>
75 : class FullLookupTable : public AbstractLookupTable<E, V> {
76 : public:
77 0 : FullLookupTable(const std::string& filename, const int size) :
78 0 : myTable(size) {
79 0 : std::ifstream strm(filename.c_str());
80 0 : for (int i = 0; i < size; i++) {
81 0 : for (int j = 0; j < size; j++) {
82 : double val;
83 : strm >> val;
84 0 : myTable[i].push_back(val);
85 : }
86 : }
87 0 : }
88 :
89 0 : virtual ~FullLookupTable() {
90 0 : }
91 :
92 0 : double lowerBound(const E* from, const E* to, double /*speed*/, double speedFactor, double /*fromEffort*/, double /*toEffort*/) const {
93 0 : return myTable[from->getNumericalID()][to->getNumericalID()] / speedFactor;
94 : }
95 :
96 0 : bool consistent() const {
97 0 : return true;
98 : }
99 :
100 : private:
101 : std::vector<std::vector<double> > myTable;
102 : };
103 :
104 :
105 : template<class E, class V, class M>
106 : class LandmarkLookupTable : public AbstractLookupTable<E, V> {
107 : public:
108 55 : LandmarkLookupTable(const std::string& filename, const std::vector<E*>& edges, SUMOAbstractRouter<E, V>* router,
109 : SUMOAbstractRouter<ReversedEdge<E, V>, V>* reverseRouter,
110 55 : const V* defaultVehicle, const std::string& outfile, const int maxNumThreads, M* mapMatcher) {
111 55 : myFirstNonInternal = -1;
112 : std::map<std::string, int> numericID;
113 : bool haveTaz = false;
114 105391 : for (E* e : edges) {
115 105336 : if (!e->isInternal()) {
116 19248 : if (myFirstNonInternal == -1) {
117 55 : myFirstNonInternal = e->getNumericalID();
118 : }
119 19248 : if (e->isTazConnector()) {
120 : haveTaz = true;
121 : }
122 19248 : numericID[e->getID()] = e->getNumericalID() - myFirstNonInternal;
123 : }
124 : }
125 : #ifdef HAVE_ZLIB
126 116 : zstr::ifstream strm(filename.c_str(), std::fstream::in | std::fstream::binary);
127 : #else
128 : std::ifstream strm(filename.c_str());
129 : #endif
130 55 : if (!strm.good()) {
131 0 : throw ProcessError(TLF("Could not load landmark-lookup-table from '%'.", filename));
132 : }
133 : std::string line;
134 : std::vector<const E*> landmarks;
135 : int numLandMarks = 0;
136 : bool haveData = false;
137 21965 : while (std::getline(strm, line)) {
138 10956 : if (line == "") {
139 : break;
140 : }
141 10956 : StringTokenizer st(line);
142 10956 : if (st.size() == 1) {
143 72 : const std::string lm = st.get(0);
144 : if (myLandmarks.count(lm) != 0) {
145 0 : throw ProcessError(TLF("Duplicate edge '%' in landmark file.", lm));
146 : }
147 : // retrieve landmark edge
148 : const auto& it = numericID.find(lm);
149 72 : if (it == numericID.end()) {
150 6 : throw ProcessError(TLF("Landmark edge '%' does not exist in the network.", lm));
151 : }
152 70 : myLandmarks[lm] = numLandMarks++;
153 140 : myFromLandmarkDists.push_back(std::vector<double>(0));
154 142 : myToLandmarkDists.push_back(std::vector<double>(0));
155 70 : landmarks.push_back(edges[it->second + myFirstNonInternal]);
156 10884 : } else if (st.size() == 2) {
157 : // geo landmark
158 : try {
159 8 : std::string lonStr = st.get(0);
160 8 : if (!lonStr.empty() && lonStr.back() == ',') {
161 : // remove trailing comma
162 8 : lonStr = lonStr.substr(0, lonStr.size() - 1);
163 : }
164 8 : double lon = StringUtils::toDouble(lonStr);
165 16 : double lat = StringUtils::toDouble(st.get(1));
166 : std::vector<const E*> mapped;
167 : bool ok;
168 8 : mapMatcher->parseGeoEdges(PositionVector({Position(lon, lat)}), true, SVC_IGNORING, mapped, "LMLT", false, ok, true);
169 8 : if (mapped.size() != 1) {
170 0 : throw ProcessError(TLF("Invalid coordinate in landmark file, could not find edge at '%'", line));
171 : }
172 8 : std::string lm = mapped.front()->getID();
173 : const auto& it = numericID.find(lm);
174 8 : if (it == numericID.end()) {
175 0 : throw ProcessError(TLF("Landmark edge '%' does not exist in the network.", lm));
176 : }
177 8 : myLandmarks[lm] = numLandMarks++;
178 16 : myFromLandmarkDists.push_back(std::vector<double>(0));
179 16 : myToLandmarkDists.push_back(std::vector<double>(0));
180 8 : landmarks.push_back(edges[it->second + myFirstNonInternal]);
181 8 : } catch (const NumberFormatException&) {
182 0 : throw ProcessError(TLF("Broken landmark file, could not parse '%' as coordinates.", line));
183 : }
184 10876 : } else if (st.size() == 4) {
185 : // legacy style landmark table
186 9436 : const std::string lm = st.get(0);
187 9436 : const std::string edge = st.get(1);
188 9436 : if (numericID[edge] != (int)myFromLandmarkDists[myLandmarks[lm]].size()) {
189 0 : throw ProcessError(TLF("Unknown or unordered edge '%' in landmark file.", edge));
190 : }
191 9436 : const double distFrom = StringUtils::toDouble(st.get(2));
192 9436 : const double distTo = StringUtils::toDouble(st.get(3));
193 9436 : myFromLandmarkDists[myLandmarks[lm]].push_back(distFrom);
194 9436 : myToLandmarkDists[myLandmarks[lm]].push_back(distTo);
195 : haveData = true;
196 : } else {
197 1440 : const std::string edge = st.get(0);
198 1440 : if ((int)st.size() != 2 * numLandMarks + 1) {
199 0 : throw ProcessError(TLF("Broken landmark file, unexpected number of entries (%) for edge '%'.", st.size() - 1, edge));
200 : }
201 1440 : if (numericID[edge] != (int)myFromLandmarkDists[0].size()) {
202 0 : throw ProcessError(TLF("Unknown or unordered edge '%' in landmark file.", edge));
203 : }
204 4320 : for (int i = 0; i < numLandMarks; i++) {
205 2880 : const double distFrom = StringUtils::toDouble(st.get(2 * i + 1));
206 2880 : const double distTo = StringUtils::toDouble(st.get(2 * i + 2));
207 2880 : myFromLandmarkDists[i].push_back(distFrom);
208 2880 : myToLandmarkDists[i].push_back(distTo);
209 : }
210 : haveData = true;
211 : }
212 : }
213 53 : if (myLandmarks.empty()) {
214 42 : WRITE_WARNINGF("No landmarks in '%', falling back to standard A*.", filename);
215 : return;
216 : }
217 : #ifdef HAVE_FOX
218 39 : MFXWorkerThread::Pool threadPool;
219 : std::vector<RoutingTask*> currentTasks;
220 : #endif
221 39 : if (!haveData) {
222 44 : WRITE_MESSAGE(TL("Calculating new lookup table."));
223 : }
224 111 : for (int i = 0; i < numLandMarks; ++i) {
225 76 : if ((int)myFromLandmarkDists[i].size() != (int)edges.size() - myFirstNonInternal) {
226 48 : const E* const landmark = landmarks[i];
227 48 : if (haveData) {
228 4 : if (myFromLandmarkDists[i].empty()) {
229 0 : WRITE_WARNINGF(TL("No lookup table for landmark edge '%', recalculating."), landmark->getID());
230 : } else {
231 6 : const std::string tazWarning = haveTaz ? " Make sure that any used taz or junction-taz definitions are loaded when generating the table" : "";
232 12 : throw ProcessError(TLF("Not all network edges were found in the lookup table '%' for landmark edge '%'.%", filename, landmark->getID(), tazWarning));
233 : }
234 : }
235 : #ifdef HAVE_FOX
236 44 : if (maxNumThreads > 0) {
237 8 : const double lmCost = router->recomputeCosts({landmark}, defaultVehicle, 0);
238 : router->setAutoBulkMode(true);
239 8 : if (threadPool.size() == 0) {
240 4 : if (reverseRouter == nullptr) {
241 : // The CHRouter needs initialization
242 : // before it gets cloned, so we do a dummy routing which is not in parallel
243 : std::vector<const E*> route;
244 0 : router->compute(landmark, landmark, defaultVehicle, 0, route, true);
245 0 : } else {
246 : reverseRouter->setAutoBulkMode(true);
247 : }
248 12 : while ((int)threadPool.size() < maxNumThreads) {
249 8 : auto revClone = reverseRouter == nullptr ? nullptr : reverseRouter->clone();
250 8 : new WorkerThread(threadPool, router->clone(), revClone, defaultVehicle);
251 : }
252 : }
253 2208 : for (int j = (int)myFromLandmarkDists[i].size() + myFirstNonInternal; j < (int)edges.size(); ++j) {
254 2200 : const E* const edge = edges[j];
255 2200 : if (landmark != edge) {
256 2192 : const double sourceDestCost = lmCost + router->recomputeCosts({edge}, defaultVehicle, 0);
257 2192 : currentTasks.push_back(new RoutingTask(landmark, edge, sourceDestCost));
258 2192 : threadPool.add(currentTasks.back(), i % maxNumThreads);
259 : }
260 : }
261 : }
262 : #else
263 : UNUSED_PARAMETER(reverseRouter);
264 : #endif
265 : }
266 : }
267 : #ifdef HAVE_FOX
268 35 : threadPool.waitAll(false);
269 : int taskIndex = 0;
270 : #endif
271 105 : for (int i = 0; i < numLandMarks; ++i) {
272 70 : if ((int)myFromLandmarkDists[i].size() != (int)edges.size() - myFirstNonInternal) {
273 44 : const E* landmark = landmarks[i];
274 44 : const double lmCost = router->recomputeCosts({landmark}, defaultVehicle, 0);
275 : int unreachableFrom = 0;
276 : int unreachableTo = 0;
277 14580 : for (int j = (int)myFromLandmarkDists[i].size() + myFirstNonInternal; j < (int)edges.size(); ++j) {
278 14536 : const E* const edge = edges[j];
279 14536 : double distFrom = -1;
280 14536 : double distTo = -1;
281 14536 : if (landmark == edge) {
282 44 : distFrom = 0;
283 44 : distTo = 0;
284 : } else {
285 14492 : if (maxNumThreads > 0) {
286 : #ifdef HAVE_FOX
287 2192 : distFrom = currentTasks[taskIndex]->getFromCost();
288 2192 : distTo = currentTasks[taskIndex]->getToCost();
289 2192 : delete currentTasks[taskIndex++];
290 : #endif
291 : } else {
292 12300 : const double sourceDestCost = lmCost + router->recomputeCosts({edge}, defaultVehicle, 0);
293 : std::vector<const E*> route;
294 : std::vector<const ReversedEdge<E, V>*> reversedRoute;
295 : // compute from-distance (skip taz-sources and other unreachable edges)
296 12300 : if (edge->getPredecessors().size() > 0 && landmark->getSuccessors().size() > 0) {
297 11844 : if (router->compute(landmark, edge, defaultVehicle, 0, route, true)) {
298 23564 : distFrom = MAX2(0.0, router->recomputeCosts(route, defaultVehicle, 0) - sourceDestCost);
299 : route.clear();
300 : }
301 : }
302 : // compute to-distance (skip unreachable landmarks)
303 12300 : if (landmark->getPredecessors().size() > 0 && edge->getSuccessors().size() > 0) {
304 12174 : if (router->compute(edge, landmark, defaultVehicle, 0, route, true)) {
305 24100 : distTo = MAX2(0.0, router->recomputeCosts(route, defaultVehicle, 0) - sourceDestCost);
306 : route.clear();
307 : }
308 : }
309 12300 : }
310 : }
311 14536 : myFromLandmarkDists[i].push_back(distFrom);
312 14536 : myToLandmarkDists[i].push_back(distTo);
313 14536 : if (!edge->isTazConnector()) {
314 14480 : if (distFrom == -1) {
315 980 : unreachableFrom++;
316 : }
317 14480 : if (distTo == -1) {
318 444 : unreachableTo++;
319 : }
320 : }
321 : }
322 44 : if (unreachableFrom > 0 || unreachableTo > 0) {
323 24 : WRITE_WARNINGF(TL("Landmark % is not reachable from % edges and is unable to reach % out of % total edges."),
324 : landmark->getID(), unreachableFrom, unreachableTo, numericID.size());
325 : }
326 : }
327 : }
328 35 : if (!outfile.empty()) {
329 : std::ostream* ostrm = nullptr;
330 : #ifdef HAVE_ZLIB
331 16 : if (StringUtils::endsWith(outfile, ".gz")) {
332 0 : ostrm = new zstr::ofstream(outfile.c_str(), std::ios_base::out);
333 : } else {
334 : #endif
335 8 : ostrm = new std::ofstream(outfile.c_str());
336 : #ifdef HAVE_ZLIB
337 : }
338 : #endif
339 8 : if (!ostrm->good()) {
340 0 : delete ostrm;
341 0 : throw ProcessError(TLF("Could not open file '%' for writing.", outfile));
342 : }
343 24 : WRITE_MESSAGEF(TL("Saving new matrix to '%'."), outfile);
344 24 : for (int i = 0; i < numLandMarks; ++i) {
345 48 : (*ostrm) << getLandmark(i) << "\n";
346 : }
347 2208 : for (int j = 0; j < (int)myFromLandmarkDists[0].size(); ++j) {
348 2200 : (*ostrm) << edges[j + myFirstNonInternal]->getID();
349 6600 : for (int i = 0; i < numLandMarks; ++i) {
350 8800 : (*ostrm) << " " << myFromLandmarkDists[i][j] << " " << myToLandmarkDists[i][j];
351 : }
352 2200 : (*ostrm) << "\n";
353 : }
354 8 : delete ostrm;
355 : }
356 130 : }
357 :
358 49 : virtual ~LandmarkLookupTable() {
359 49 : }
360 :
361 5938 : double lowerBound(const E* from, const E* to, double speed, double speedFactor, double fromEffort, double toEffort) const {
362 5938 : double result = from->getDistanceTo(to) / speed;
363 : #ifdef ASTAR_DEBUG_LOOKUPTABLE
364 : if (from->getID() == ASTAR_DEBUG_LOOKUPTABLE_FROM) {
365 : std::cout << " lowerBound to=" << to->getID() << " result1=" << result << "\n";
366 : }
367 : #endif
368 14114 : for (int i = 0; i < (int)myLandmarks.size(); ++i) {
369 : // a cost of -1 is used to encode unreachability.
370 8180 : const double fl = myToLandmarkDists[i][from->getNumericalID() - myFirstNonInternal];
371 8180 : const double tl = myToLandmarkDists[i][to->getNumericalID() - myFirstNonInternal];
372 8180 : if (fl >= 0 && tl >= 0) {
373 4286 : const double bound = (fl - tl - toEffort) / speedFactor;
374 : #ifdef ASTAR_DEBUG_LOOKUPTABLE
375 : if (from->getID() == ASTAR_DEBUG_LOOKUPTABLE_FROM && result < bound) {
376 : std::cout << " landmarkTo=" << getLandmark(i) << " result2=" << bound
377 : << " fl=" << fl << " tl=" << tl << "\n";
378 : }
379 : #endif
380 : result = MAX2(result, bound);
381 : }
382 8180 : const double lt = myFromLandmarkDists[i][to->getNumericalID() - myFirstNonInternal];
383 8180 : const double lf = myFromLandmarkDists[i][from->getNumericalID() - myFirstNonInternal];
384 8180 : if (lt >= 0 && lf >= 0) {
385 7744 : const double bound = (lt - lf - fromEffort) / speedFactor;
386 : #ifdef ASTAR_DEBUG_LOOKUPTABLE
387 : if (from->getID() == ASTAR_DEBUG_LOOKUPTABLE_FROM && result < bound) {
388 : std::cout << " landmarkFrom=" << getLandmark(i) << " result3=" << bound
389 : << " lt=" << lt << " lf=" << lf << "\n";
390 : }
391 : #endif
392 : result = MAX2(result, bound);
393 : }
394 8180 : if ((tl >= 0 && fl < 0)
395 8180 : || (lf >= 0 && lt < 0)) {
396 : // target unreachable.
397 : #ifdef ASTAR_DEBUG_UNREACHABLE
398 : std::cout << " unreachable: from=" << from->getID() << " to=" << to->getID() << " landmark=" << getLandmark(i) << " "
399 : << ((tl >= 0 && fl < 0) ? " (toLandmark)" : " (fromLandmark)")
400 : << " fl=" << fl << " tl=" << tl << " lt=" << lt << " lf=" << lf
401 : << "\n";
402 : #endif
403 : return UNREACHABLE;
404 : }
405 : }
406 : return result;
407 : }
408 :
409 49 : bool consistent() const {
410 49 : return false;
411 : }
412 :
413 : private:
414 : std::map<std::string, int> myLandmarks;
415 : std::vector<std::vector<double> > myFromLandmarkDists;
416 : std::vector<std::vector<double> > myToLandmarkDists;
417 : int myFirstNonInternal;
418 :
419 : #ifdef HAVE_FOX
420 : private:
421 : class WorkerThread : public MFXWorkerThread {
422 : public:
423 8 : WorkerThread(MFXWorkerThread::Pool& pool,
424 : SUMOAbstractRouter<E, V>* router,
425 : SUMOAbstractRouter<ReversedEdge<E, V>, V>* reverseRouter, const V* vehicle)
426 8 : : MFXWorkerThread(pool), myRouter(router), myReversedRouter(reverseRouter), myVehicle(vehicle) {}
427 :
428 16 : virtual ~WorkerThread() {
429 8 : delete myRouter;
430 8 : delete myReversedRouter;
431 24 : }
432 :
433 2192 : const std::pair<double, double> compute(const E* src, const E* dest, const double costOff) {
434 : double fromResult = -1.;
435 2192 : if (myRouter->compute(src, dest, myVehicle, 0, myRoute, true)) {
436 1702 : fromResult = MAX2(0.0, myRouter->recomputeCosts(myRoute, myVehicle, 0) + costOff);
437 : myRoute.clear();
438 : }
439 : double toResult = -1.;
440 2192 : if (myReversedRouter != nullptr) {
441 2192 : if (myReversedRouter->compute(src->getReversedRoutingEdge(), dest->getReversedRoutingEdge(), myVehicle, 0, myReversedRoute, true)) {
442 1970 : toResult = MAX2(0.0, myReversedRouter->recomputeCosts(myReversedRoute, myVehicle, 0) + costOff);
443 : myReversedRoute.clear();
444 : }
445 : } else {
446 0 : if (myRouter->compute(dest, src, myVehicle, 0, myRoute, true)) {
447 0 : toResult = MAX2(0.0, myRouter->recomputeCosts(myRoute, myVehicle, 0) + costOff);
448 : myRoute.clear();
449 : }
450 : }
451 2192 : return std::make_pair(fromResult, toResult);
452 : }
453 :
454 : private:
455 : SUMOAbstractRouter<E, V>* myRouter;
456 : SUMOAbstractRouter<ReversedEdge<E, V>, V>* myReversedRouter;
457 : const V* myVehicle;
458 : std::vector<const E*> myRoute;
459 : std::vector<const ReversedEdge<E, V>*> myReversedRoute;
460 : };
461 :
462 : class RoutingTask : public MFXWorkerThread::Task {
463 : public:
464 2192 : RoutingTask(const E* src, const E* dest, const double costOff)
465 2192 : : mySrc(src), myDest(dest), myCostOff(-costOff) {}
466 2192 : void run(MFXWorkerThread* context) {
467 2192 : myCost = ((WorkerThread*)context)->compute(mySrc, myDest, myCostOff);
468 2192 : }
469 : double getFromCost() {
470 2192 : return myCost.first;
471 : }
472 : double getToCost() {
473 2192 : return myCost.second;
474 : }
475 : private:
476 : const E* const mySrc;
477 : const E* const myDest;
478 : const double myCostOff;
479 : std::pair<double, double> myCost;
480 : private:
481 : /// @brief Invalidated assignment operator.
482 : RoutingTask& operator=(const RoutingTask&) = delete;
483 : };
484 :
485 : private:
486 : /// @brief for multi threaded routing
487 : #endif
488 :
489 16 : std::string getLandmark(int i) const {
490 24 : for (std::map<std::string, int>::const_iterator it = myLandmarks.begin(); it != myLandmarks.end(); ++it) {
491 24 : if (it->second == i) {
492 : return it->first;
493 : }
494 : }
495 0 : return "";
496 : }
497 : };
|
