Eclipse SUMO - Simulation of Urban MObility
Loading...
Searching...
No Matches
AStarLookupTable.h
Go to the documentation of this file.
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/****************************************************************************/
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
34
35#define UNREACHABLE (std::numeric_limits<double>::max() / 1000.0)
36
37//#define ASTAR_DEBUG_LOOKUPTABLE
38//#define ASTAR_DEBUG_LOOKUPTABLE_FROM "disabled"
39//#define ASTAR_DEBUG_UNREACHABLE
40
41// ===========================================================================
42// class definitions
43// ===========================================================================
60template<class E, class V>
62public:
64
66 virtual double lowerBound(const E* from, const E* to, double speed, double speedFactor, double fromEffort, double toEffort) const = 0;
67
69 virtual bool consistent() const = 0;
70};
71
72
73template<class E, class V>
75public:
76 FullLookupTable(const std::string& filename, const int size) :
77 myTable(size) {
78 std::ifstream strm(filename.c_str());
79 for (int i = 0; i < size; i++) {
80 for (int j = 0; j < size; j++) {
81 double val;
82 strm >> val;
83 myTable[i].push_back(val);
84 }
85 }
86 }
87
88 virtual ~FullLookupTable() {
89 }
90
91 double lowerBound(const E* from, const E* to, double /*speed*/, double speedFactor, double /*fromEffort*/, double /*toEffort*/) const {
92 return myTable[from->getNumericalID()][to->getNumericalID()] / speedFactor;
93 }
94
95 bool consistent() const {
96 return true;
97 }
98
99private:
100 std::vector<std::vector<double> > myTable;
101};
102
103
104template<class E, class V, class M>
106public:
107 LandmarkLookupTable(const std::string& filename, const std::vector<E*>& edges, SUMOAbstractRouter<E, V>* router,
108 SUMOAbstractRouter<ReversedEdge<E, V>, V>* reverseRouter,
109 const V* defaultVehicle, const std::string& outfile, const int maxNumThreads, M* mapMatcher) {
111 std::map<std::string, int> numericID;
112 for (E* e : edges) {
113 if (!e->isInternal()) {
114 if (myFirstNonInternal == -1) {
115 myFirstNonInternal = e->getNumericalID();
116 }
117 numericID[e->getID()] = e->getNumericalID() - myFirstNonInternal;
118 }
119 }
120#ifdef HAVE_ZLIB
121 zstr::ifstream strm(filename.c_str(), std::fstream::in | std::fstream::binary);
122#else
123 std::ifstream strm(filename.c_str());
124#endif
125 if (!strm.good()) {
126 throw ProcessError(TLF("Could not load landmark-lookup-table from '%'.", filename));
127 }
128 std::string line;
129 std::vector<const E*> landmarks;
130 int numLandMarks = 0;
131 bool haveData = false;
132 while (std::getline(strm, line)) {
133 if (line == "") {
134 break;
135 }
136 StringTokenizer st(line);
137 if (st.size() == 1) {
138 const std::string lm = st.get(0);
139 if (myLandmarks.count(lm) != 0) {
140 throw ProcessError(TLF("Duplicate edge '%' in landmark file.", lm));
141 }
142 // retrieve landmark edge
143 const auto& it = numericID.find(lm);
144 if (it == numericID.end()) {
145 throw ProcessError(TLF("Landmark edge '%' does not exist in the network.", lm));
146 }
147 myLandmarks[lm] = numLandMarks++;
148 myFromLandmarkDists.push_back(std::vector<double>(0));
149 myToLandmarkDists.push_back(std::vector<double>(0));
150 landmarks.push_back(edges[it->second + myFirstNonInternal]);
151 } else if (st.size() == 2) {
152 // geo landmark
153 try {
154 std::string lonStr = st.get(0);
155 if (!lonStr.empty() && lonStr.back() == ',') {
156 // remove trailing comma
157 lonStr = lonStr.substr(0, lonStr.size() - 1);
158 }
159 double lon = StringUtils::toDouble(lonStr);
160 double lat = StringUtils::toDouble(st.get(1));
161 std::vector<const E*> mapped;
162 bool ok;
163 mapMatcher->parseGeoEdges(PositionVector({Position(lon, lat)}), true, SVC_IGNORING, mapped, "LMLT", false, ok, true);
164 if (mapped.size() != 1) {
165 throw ProcessError(TLF("Invalid coordinate in landmark file, could not find edge at '%'", line));
166 }
167 std::string lm = mapped.front()->getID();
168 const auto& it = numericID.find(lm);
169 if (it == numericID.end()) {
170 throw ProcessError(TLF("Landmark edge '%' does not exist in the network.", lm));
171 }
172 myLandmarks[lm] = numLandMarks++;
173 myFromLandmarkDists.push_back(std::vector<double>(0));
174 myToLandmarkDists.push_back(std::vector<double>(0));
175 landmarks.push_back(edges[it->second + myFirstNonInternal]);
176 } catch (const NumberFormatException&) {
177 throw ProcessError(TLF("Broken landmark file, could not parse '%' as coordinates.", line));
178 }
179 } else if (st.size() == 4) {
180 // legacy style landmark table
181 const std::string lm = st.get(0);
182 const std::string edge = st.get(1);
183 if (numericID[edge] != (int)myFromLandmarkDists[myLandmarks[lm]].size()) {
184 throw ProcessError(TLF("Unknown or unordered edge '%' in landmark file.", edge));
185 }
186 const double distFrom = StringUtils::toDouble(st.get(2));
187 const double distTo = StringUtils::toDouble(st.get(3));
188 myFromLandmarkDists[myLandmarks[lm]].push_back(distFrom);
189 myToLandmarkDists[myLandmarks[lm]].push_back(distTo);
190 haveData = true;
191 } else {
192 const std::string edge = st.get(0);
193 if ((int)st.size() != 2 * numLandMarks + 1) {
194 throw ProcessError(TLF("Broken landmark file, unexpected number of entries (%) for edge '%'.", st.size() - 1, edge));
195 }
196 if (numericID[edge] != (int)myFromLandmarkDists[0].size()) {
197 throw ProcessError(TLF("Unknown or unordered edge '%' in landmark file.", edge));
198 }
199 for (int i = 0; i < numLandMarks; i++) {
200 const double distFrom = StringUtils::toDouble(st.get(2 * i + 1));
201 const double distTo = StringUtils::toDouble(st.get(2 * i + 2));
202 myFromLandmarkDists[i].push_back(distFrom);
203 myToLandmarkDists[i].push_back(distTo);
204 }
205 haveData = true;
206 }
207 }
208 if (myLandmarks.empty()) {
209 WRITE_WARNINGF("No landmarks in '%', falling back to standard A*.", filename);
210 return;
211 }
212#ifdef HAVE_FOX
213 MFXWorkerThread::Pool threadPool;
214 std::vector<RoutingTask*> currentTasks;
215#endif
216 if (!haveData) {
217 WRITE_MESSAGE(TL("Calculating new lookup table."));
218 }
219 for (int i = 0; i < numLandMarks; ++i) {
220 if ((int)myFromLandmarkDists[i].size() != (int)edges.size() - myFirstNonInternal) {
221 const E* const landmark = landmarks[i];
222 if (haveData) {
223 if (myFromLandmarkDists[i].empty()) {
224 WRITE_WARNINGF(TL("No lookup table for landmark edge '%', recalculating."), landmark->getID());
225 } else {
226 throw ProcessError(TLF("Not all network edges were found in the lookup table '%' for landmark edge '%'.", filename, landmark->getID()));
227 }
228 }
229#ifdef HAVE_FOX
230 if (maxNumThreads > 0) {
231 const double lmCost = router->recomputeCosts({landmark}, defaultVehicle, 0);
232 router->setAutoBulkMode(true);
233 if (threadPool.size() == 0) {
234 if (reverseRouter == nullptr) {
235 // The CHRouter needs initialization
236 // before it gets cloned, so we do a dummy routing which is not in parallel
237 std::vector<const E*> route;
238 router->compute(landmark, landmark, defaultVehicle, 0, route, true);
239 } else {
240 reverseRouter->setAutoBulkMode(true);
241 }
242 while ((int)threadPool.size() < maxNumThreads) {
243 auto revClone = reverseRouter == nullptr ? nullptr : reverseRouter->clone();
244 new WorkerThread(threadPool, router->clone(), revClone, defaultVehicle);
245 }
246 }
247 for (int j = (int)myFromLandmarkDists[i].size() + myFirstNonInternal; j < (int)edges.size(); ++j) {
248 const E* const edge = edges[j];
249 if (landmark != edge) {
250 const double sourceDestCost = lmCost + router->recomputeCosts({edge}, defaultVehicle, 0);
251 currentTasks.push_back(new RoutingTask(landmark, edge, sourceDestCost));
252 threadPool.add(currentTasks.back(), i % maxNumThreads);
253 }
254 }
255 }
256#else
257 UNUSED_PARAMETER(reverseRouter);
258#endif
259 }
260 }
261#ifdef HAVE_FOX
262 threadPool.waitAll(false);
263 int taskIndex = 0;
264#endif
265 for (int i = 0; i < numLandMarks; ++i) {
266 if ((int)myFromLandmarkDists[i].size() != (int)edges.size() - myFirstNonInternal) {
267 const E* landmark = landmarks[i];
268 const double lmCost = router->recomputeCosts({landmark}, defaultVehicle, 0);
269 int unreachableFrom = 0;
270 int unreachableTo = 0;
271 for (int j = (int)myFromLandmarkDists[i].size() + myFirstNonInternal; j < (int)edges.size(); ++j) {
272 const E* const edge = edges[j];
273 double distFrom = -1;
274 double distTo = -1;
275 if (landmark == edge) {
276 distFrom = 0;
277 distTo = 0;
278 } else {
279 if (maxNumThreads > 0) {
280#ifdef HAVE_FOX
281 distFrom = currentTasks[taskIndex]->getFromCost();
282 distTo = currentTasks[taskIndex]->getToCost();
283 delete currentTasks[taskIndex++];
284#endif
285 } else {
286 const double sourceDestCost = lmCost + router->recomputeCosts({edge}, defaultVehicle, 0);
287 std::vector<const E*> route;
288 std::vector<const ReversedEdge<E, V>*> reversedRoute;
289 // compute from-distance (skip taz-sources and other unreachable edges)
290 if (edge->getPredecessors().size() > 0 && landmark->getSuccessors().size() > 0) {
291 if (router->compute(landmark, edge, defaultVehicle, 0, route, true)) {
292 distFrom = MAX2(0.0, router->recomputeCosts(route, defaultVehicle, 0) - sourceDestCost);
293 route.clear();
294 }
295 }
296 // compute to-distance (skip unreachable landmarks)
297 if (landmark->getPredecessors().size() > 0 && edge->getSuccessors().size() > 0) {
298 if (router->compute(edge, landmark, defaultVehicle, 0, route, true)) {
299 distTo = MAX2(0.0, router->recomputeCosts(route, defaultVehicle, 0) - sourceDestCost);
300 route.clear();
301 }
302 }
303 }
304 }
305 myFromLandmarkDists[i].push_back(distFrom);
306 myToLandmarkDists[i].push_back(distTo);
307 if (!edge->isTazConnector()) {
308 if (distFrom == -1) {
309 unreachableFrom++;
310 }
311 if (distTo == -1) {
312 unreachableTo++;
313 }
314 }
315 }
316 if (unreachableFrom > 0 || unreachableTo > 0) {
317 WRITE_WARNINGF(TL("Landmark % is not reachable from % edges and is unable to reach % out of % total edges."),
318 landmark->getID(), unreachableFrom, unreachableTo, numericID.size());
319 }
320 }
321 }
322 if (!outfile.empty()) {
323 std::ostream* ostrm = nullptr;
324#ifdef HAVE_ZLIB
325 if (StringUtils::endsWith(outfile, ".gz")) {
326 ostrm = new zstr::ofstream(outfile.c_str(), std::ios_base::out);
327 } else {
328#endif
329 ostrm = new std::ofstream(outfile.c_str());
330#ifdef HAVE_ZLIB
331 }
332#endif
333 if (!ostrm->good()) {
334 delete ostrm;
335 throw ProcessError(TLF("Could not open file '%' for writing.", outfile));
336 }
337 WRITE_MESSAGEF(TL("Saving new matrix to '%'."), outfile);
338 for (int i = 0; i < numLandMarks; ++i) {
339 (*ostrm) << getLandmark(i) << "\n";
340 }
341 for (int j = 0; j < (int)myFromLandmarkDists[0].size(); ++j) {
342 (*ostrm) << edges[j + myFirstNonInternal]->getID();
343 for (int i = 0; i < numLandMarks; ++i) {
344 (*ostrm) << " " << myFromLandmarkDists[i][j] << " " << myToLandmarkDists[i][j];
345 }
346 (*ostrm) << "\n";
347 }
348 delete ostrm;
349 }
350 }
351
353 }
354
355 double lowerBound(const E* from, const E* to, double speed, double speedFactor, double fromEffort, double toEffort) const {
356 double result = from->getDistanceTo(to) / speed;
357#ifdef ASTAR_DEBUG_LOOKUPTABLE
358 if (from->getID() == ASTAR_DEBUG_LOOKUPTABLE_FROM) {
359 std::cout << " lowerBound to=" << to->getID() << " result1=" << result << "\n";
360 }
361#endif
362 for (int i = 0; i < (int)myLandmarks.size(); ++i) {
363 // a cost of -1 is used to encode unreachability.
364 const double fl = myToLandmarkDists[i][from->getNumericalID() - myFirstNonInternal];
365 const double tl = myToLandmarkDists[i][to->getNumericalID() - myFirstNonInternal];
366 if (fl >= 0 && tl >= 0) {
367 const double bound = (fl - tl - toEffort) / speedFactor;
368#ifdef ASTAR_DEBUG_LOOKUPTABLE
369 if (from->getID() == ASTAR_DEBUG_LOOKUPTABLE_FROM && result < bound) {
370 std::cout << " landmarkTo=" << getLandmark(i) << " result2=" << bound
371 << " fl=" << fl << " tl=" << tl << "\n";
372 }
373#endif
374 result = MAX2(result, bound);
375 }
376 const double lt = myFromLandmarkDists[i][to->getNumericalID() - myFirstNonInternal];
377 const double lf = myFromLandmarkDists[i][from->getNumericalID() - myFirstNonInternal];
378 if (lt >= 0 && lf >= 0) {
379 const double bound = (lt - lf - fromEffort) / speedFactor;
380#ifdef ASTAR_DEBUG_LOOKUPTABLE
381 if (from->getID() == ASTAR_DEBUG_LOOKUPTABLE_FROM && result < bound) {
382 std::cout << " landmarkFrom=" << getLandmark(i) << " result3=" << bound
383 << " lt=" << lt << " lf=" << lf << "\n";
384 }
385#endif
386 result = MAX2(result, bound);
387 }
388 if ((tl >= 0 && fl < 0)
389 || (lf >= 0 && lt < 0)) {
390 // target unreachable.
391#ifdef ASTAR_DEBUG_UNREACHABLE
392 std::cout << " unreachable: from=" << from->getID() << " to=" << to->getID() << " landmark=" << getLandmark(i) << " "
393 << ((tl >= 0 && fl < 0) ? " (toLandmark)" : " (fromLandmark)")
394 << " fl=" << fl << " tl=" << tl << " lt=" << lt << " lf=" << lf
395 << "\n";
396#endif
397 return UNREACHABLE;
398 }
399 }
400 return result;
401 }
402
403 bool consistent() const {
404 return false;
405 }
406
407private:
408 std::map<std::string, int> myLandmarks;
409 std::vector<std::vector<double> > myFromLandmarkDists;
410 std::vector<std::vector<double> > myToLandmarkDists;
412
413#ifdef HAVE_FOX
414private:
415 class WorkerThread : public MFXWorkerThread {
416 public:
417 WorkerThread(MFXWorkerThread::Pool& pool,
419 SUMOAbstractRouter<ReversedEdge<E, V>, V>* reverseRouter, const V* vehicle)
420 : MFXWorkerThread(pool), myRouter(router), myReversedRouter(reverseRouter), myVehicle(vehicle) {}
421
422 virtual ~WorkerThread() {
423 delete myRouter;
424 delete myReversedRouter;
425 }
426
427 const std::pair<double, double> compute(const E* src, const E* dest, const double costOff) {
428 double fromResult = -1.;
429 if (myRouter->compute(src, dest, myVehicle, 0, myRoute, true)) {
430 fromResult = MAX2(0.0, myRouter->recomputeCosts(myRoute, myVehicle, 0) + costOff);
431 myRoute.clear();
432 }
433 double toResult = -1.;
434 if (myReversedRouter != nullptr) {
435 if (myReversedRouter->compute(src->getReversedRoutingEdge(), dest->getReversedRoutingEdge(), myVehicle, 0, myReversedRoute, true)) {
436 toResult = MAX2(0.0, myReversedRouter->recomputeCosts(myReversedRoute, myVehicle, 0) + costOff);
437 myReversedRoute.clear();
438 }
439 } else {
440 if (myRouter->compute(dest, src, myVehicle, 0, myRoute, true)) {
441 toResult = MAX2(0.0, myRouter->recomputeCosts(myRoute, myVehicle, 0) + costOff);
442 myRoute.clear();
443 }
444 }
445 return std::make_pair(fromResult, toResult);
446 }
447
448 private:
449 SUMOAbstractRouter<E, V>* myRouter;
450 SUMOAbstractRouter<ReversedEdge<E, V>, V>* myReversedRouter;
451 const V* myVehicle;
452 std::vector<const E*> myRoute;
453 std::vector<const ReversedEdge<E, V>*> myReversedRoute;
454 };
455
456 class RoutingTask : public MFXWorkerThread::Task {
457 public:
458 RoutingTask(const E* src, const E* dest, const double costOff)
459 : mySrc(src), myDest(dest), myCostOff(-costOff) {}
460 void run(MFXWorkerThread* context) {
461 myCost = ((WorkerThread*)context)->compute(mySrc, myDest, myCostOff);
462 }
463 double getFromCost() {
464 return myCost.first;
465 }
466 double getToCost() {
467 return myCost.second;
468 }
469 private:
470 const E* const mySrc;
471 const E* const myDest;
472 const double myCostOff;
473 std::pair<double, double> myCost;
474 private:
476 RoutingTask& operator=(const RoutingTask&) = delete;
477 };
478
479private:
481#endif
482
483 std::string getLandmark(int i) const {
484 for (std::map<std::string, int>::const_iterator it = myLandmarks.begin(); it != myLandmarks.end(); ++it) {
485 if (it->second == i) {
486 return it->first;
487 }
488 }
489 return "";
490 }
491};
#define UNREACHABLE
#define WRITE_WARNINGF(...)
Definition MsgHandler.h:296
#define WRITE_MESSAGEF(...)
Definition MsgHandler.h:298
#define WRITE_MESSAGE(msg)
Definition MsgHandler.h:297
#define TL(string)
Definition MsgHandler.h:315
#define TLF(string,...)
Definition MsgHandler.h:317
@ SVC_IGNORING
vehicles ignoring classes
#define UNUSED_PARAMETER(x)
Definition StdDefs.h:30
T MAX2(T a, T b)
Definition StdDefs.h:82
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...
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::string getLandmark(int i) const
std::vector< std::vector< double > > myToLandmarkDists
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, M *mapMatcher)
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 > > myFromLandmarkDists
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
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.
A point in 2D or 3D with translation and scaling methods.
Definition Position.h:37
A list of positions.
the edge type representing backward edges
virtual SUMOAbstractRouter * clone()=0
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...
virtual double recomputeCosts(const std::vector< const E * > &edges, const V *const v, SUMOTime msTime, double *lengthp=nullptr) const
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.