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