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-2023 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(TLF("Could not load landmark-lookup-table from '%'.", filename));
125 }
126 std::string line;
127 std::vector<const E*> landmarks;
128 int numLandMarks = 0;
129 bool haveData = false;
130 while (std::getline(strm, line)) {
131 if (line == "") {
132 break;
133 }
134 StringTokenizer st(line);
135 if (st.size() == 1) {
136 const std::string lm = st.get(0);
137 if (myLandmarks.count(lm) != 0) {
138 throw ProcessError(TLF("Duplicate edge '%' in landmark file.", lm));
139 }
140 // retrieve landmark edge
141 const auto& it = numericID.find(lm);
142 if (it == numericID.end()) {
143 throw ProcessError(TLF("Landmark edge '%' does not exist in the network.", lm));
144 }
145 myLandmarks[lm] = numLandMarks++;
146 myFromLandmarkDists.push_back(std::vector<double>(0));
147 myToLandmarkDists.push_back(std::vector<double>(0));
148 landmarks.push_back(edges[it->second + myFirstNonInternal]);
149 } else if (st.size() == 4) {
150 // legacy style landmark table
151 const std::string lm = st.get(0);
152 const std::string edge = st.get(1);
153 if (numericID[edge] != (int)myFromLandmarkDists[myLandmarks[lm]].size()) {
154 throw ProcessError(TLF("Unknown or unordered edge '%' in landmark file.", edge));
155 }
156 const double distFrom = StringUtils::toDouble(st.get(2));
157 const double distTo = StringUtils::toDouble(st.get(3));
158 myFromLandmarkDists[myLandmarks[lm]].push_back(distFrom);
159 myToLandmarkDists[myLandmarks[lm]].push_back(distTo);
160 haveData = true;
161 } else {
162 const std::string edge = st.get(0);
163 if ((int)st.size() != 2 * numLandMarks + 1) {
164 throw ProcessError(TLF("Broken landmark file, unexpected number of entries (%) for edge '%'.", st.size() - 1, edge));
165 }
166 if (numericID[edge] != (int)myFromLandmarkDists[0].size()) {
167 throw ProcessError(TLF("Unknown or unordered edge '%' in landmark file.", edge));
168 }
169 for (int i = 0; i < numLandMarks; i++) {
170 const double distFrom = StringUtils::toDouble(st.get(2 * i + 1));
171 const double distTo = StringUtils::toDouble(st.get(2 * i + 2));
172 myFromLandmarkDists[i].push_back(distFrom);
173 myToLandmarkDists[i].push_back(distTo);
174 }
175 haveData = true;
176 }
177 }
178 if (myLandmarks.empty()) {
179 WRITE_WARNINGF("No landmarks in '%', falling back to standard A*.", filename);
180 return;
181 }
182#ifdef HAVE_FOX
183 MFXWorkerThread::Pool threadPool;
184 std::vector<RoutingTask*> currentTasks;
185#endif
186 if (!haveData) {
187 WRITE_MESSAGE(TL("Calculating new lookup table."));
188 }
189 for (int i = 0; i < numLandMarks; ++i) {
190 if ((int)myFromLandmarkDists[i].size() != (int)edges.size() - myFirstNonInternal) {
191 const E* const landmark = landmarks[i];
192 if (haveData) {
193 if (myFromLandmarkDists[i].empty()) {
194 WRITE_WARNINGF(TL("No lookup table for landmark edge '%', recalculating."), landmark->getID());
195 } else {
196 throw ProcessError(TLF("Not all network edges were found in the lookup table '%' for landmark edge '%'.", filename, landmark->getID()));
197 }
198 }
199#ifdef HAVE_FOX
200 if (maxNumThreads > 0) {
201 const double lmCost = router->recomputeCosts({landmark}, defaultVehicle, 0);
202 router->setAutoBulkMode(true);
203 if (threadPool.size() == 0) {
204 if (reverseRouter == nullptr) {
205 // The CHRouter needs initialization
206 // before it gets cloned, so we do a dummy routing which is not in parallel
207 std::vector<const E*> route;
208 router->compute(landmark, landmark, defaultVehicle, 0, route);
209 } else {
210 reverseRouter->setAutoBulkMode(true);
211 }
212 while ((int)threadPool.size() < maxNumThreads) {
213 auto revClone = reverseRouter == nullptr ? nullptr : reverseRouter->clone();
214 new WorkerThread(threadPool, router->clone(), revClone, defaultVehicle);
215 }
216 }
217 for (int j = (int)myFromLandmarkDists[i].size() + myFirstNonInternal; j < (int)edges.size(); ++j) {
218 const E* const edge = edges[j];
219 if (landmark != edge) {
220 const double sourceDestCost = lmCost + router->recomputeCosts({edge}, defaultVehicle, 0);
221 currentTasks.push_back(new RoutingTask(landmark, edge, sourceDestCost));
222 threadPool.add(currentTasks.back(), i % maxNumThreads);
223 }
224 }
225 }
226#else
227 UNUSED_PARAMETER(reverseRouter);
228#endif
229 }
230 }
231#ifdef HAVE_FOX
232 threadPool.waitAll(false);
233 int taskIndex = 0;
234#endif
235 for (int i = 0; i < numLandMarks; ++i) {
236 if ((int)myFromLandmarkDists[i].size() != (int)edges.size() - myFirstNonInternal) {
237 const E* landmark = landmarks[i];
238 const double lmCost = router->recomputeCosts({landmark}, defaultVehicle, 0);
239 for (int j = (int)myFromLandmarkDists[i].size() + myFirstNonInternal; j < (int)edges.size(); ++j) {
240 const E* edge = edges[j];
241 double distFrom = -1;
242 double distTo = -1;
243 if (landmark == edge) {
244 distFrom = 0;
245 distTo = 0;
246 } else {
247 if (maxNumThreads > 0) {
248#ifdef HAVE_FOX
249 distFrom = currentTasks[taskIndex]->getFromCost();
250 distTo = currentTasks[taskIndex]->getToCost();
251 delete currentTasks[taskIndex++];
252#endif
253 } else {
254 const double sourceDestCost = lmCost + router->recomputeCosts({edge}, defaultVehicle, 0);
255 std::vector<const E*> route;
256 std::vector<const ReversedEdge<E, V>*> reversedRoute;
257 // compute from-distance (skip taz-sources and other unreachable edges)
258 if (edge->getPredecessors().size() > 0 && landmark->getSuccessors().size() > 0) {
259 if (router->compute(landmark, edge, defaultVehicle, 0, route)) {
260 distFrom = MAX2(0.0, router->recomputeCosts(route, defaultVehicle, 0) - sourceDestCost);
261 route.clear();
262 }
263 }
264 // compute to-distance (skip unreachable landmarks)
265 if (landmark->getPredecessors().size() > 0 && edge->getSuccessors().size() > 0) {
266 if (router->compute(edge, landmark, defaultVehicle, 0, route)) {
267 distTo = MAX2(0.0, router->recomputeCosts(route, defaultVehicle, 0) - sourceDestCost);
268 route.clear();
269 }
270 }
271 }
272 }
273 myFromLandmarkDists[i].push_back(distFrom);
274 myToLandmarkDists[i].push_back(distTo);
275 }
276 }
277 }
278 if (!outfile.empty()) {
279 std::ostream* ostrm = nullptr;
280#ifdef HAVE_ZLIB
281 if (StringUtils::endsWith(outfile, ".gz")) {
282 ostrm = new zstr::ofstream(outfile.c_str(), std::ios_base::out);
283 } else {
284#endif
285 ostrm = new std::ofstream(outfile.c_str());
286#ifdef HAVE_ZLIB
287 }
288#endif
289 if (!ostrm->good()) {
290 delete ostrm;
291 throw ProcessError(TLF("Could not open file '%' for writing.", outfile));
292 }
293 WRITE_MESSAGEF(TL("Saving new matrix to '%'."), outfile);
294 for (int i = 0; i < numLandMarks; ++i) {
295 (*ostrm) << getLandmark(i) << "\n";
296 }
297 for (int j = 0; j < (int)myFromLandmarkDists[0].size(); ++j) {
298 (*ostrm) << edges[j + myFirstNonInternal]->getID();
299 for (int i = 0; i < numLandMarks; ++i) {
300 (*ostrm) << " " << myFromLandmarkDists[i][j] << " " << myToLandmarkDists[i][j];
301 }
302 (*ostrm) << "\n";
303 }
304 delete ostrm;
305 }
306 }
307
309 }
310
311 double lowerBound(const E* from, const E* to, double speed, double speedFactor, double fromEffort, double toEffort) const {
312 double result = from->getDistanceTo(to) / speed;
313#ifdef ASTAR_DEBUG_LOOKUPTABLE
314 if (from->getID() == ASTAR_DEBUG_LOOKUPTABLE_FROM) {
315 std::cout << " lowerBound to=" << to->getID() << " result1=" << result << "\n";
316 }
317#endif
318 for (int i = 0; i < (int)myLandmarks.size(); ++i) {
319 // a cost of -1 is used to encode unreachability.
320 const double fl = myToLandmarkDists[i][from->getNumericalID() - myFirstNonInternal];
321 const double tl = myToLandmarkDists[i][to->getNumericalID() - myFirstNonInternal];
322 if (fl >= 0 && tl >= 0) {
323 const double bound = (fl - tl - toEffort) / speedFactor;
324#ifdef ASTAR_DEBUG_LOOKUPTABLE
325 if (from->getID() == ASTAR_DEBUG_LOOKUPTABLE_FROM && result < bound) {
326 std::cout << " landmarkTo=" << getLandmark(i) << " result2=" << bound
327 << " fl=" << fl << " tl=" << tl << "\n";
328 }
329#endif
330 result = MAX2(result, bound);
331 }
332 const double lt = myFromLandmarkDists[i][to->getNumericalID() - myFirstNonInternal];
333 const double lf = myFromLandmarkDists[i][from->getNumericalID() - myFirstNonInternal];
334 if (lt >= 0 && lf >= 0) {
335 const double bound = (lt - lf - fromEffort) / speedFactor;
336#ifdef ASTAR_DEBUG_LOOKUPTABLE
337 if (from->getID() == ASTAR_DEBUG_LOOKUPTABLE_FROM && result < bound) {
338 std::cout << " landmarkFrom=" << getLandmark(i) << " result3=" << bound
339 << " lt=" << lt << " lf=" << lf << "\n";
340 }
341#endif
342 result = MAX2(result, bound);
343 }
344 if ((tl >= 0 && fl < 0)
345 || (lf >= 0 && lt < 0)) {
346 // target unreachable.
347#ifdef ASTAR_DEBUG_UNREACHABLE
348 std::cout << " unreachable: from=" << from->getID() << " to=" << to->getID() << " landmark=" << getLandmark(i) << " "
349 << ((tl >= 0 && fl < 0) ? " (toLandmark)" : " (fromLandmark)")
350 << " fl=" << fl << " tl=" << tl << " lt=" << lt << " lf=" << lf
351 << "\n";
352#endif
353 return UNREACHABLE;
354 }
355 }
356 return result;
357 }
358
359 bool consistent() const {
360 return false;
361 }
362
363private:
364 std::map<std::string, int> myLandmarks;
365 std::vector<std::vector<double> > myFromLandmarkDists;
366 std::vector<std::vector<double> > myToLandmarkDists;
368
369#ifdef HAVE_FOX
370private:
371 class WorkerThread : public MFXWorkerThread {
372 public:
373 WorkerThread(MFXWorkerThread::Pool& pool,
375 SUMOAbstractRouter<ReversedEdge<E, V>, V>* reverseRouter, const V* vehicle)
376 : MFXWorkerThread(pool), myRouter(router), myReversedRouter(reverseRouter), myVehicle(vehicle) {}
377
378 virtual ~WorkerThread() {
379 delete myRouter;
380 delete myReversedRouter;
381 }
382
383 const std::pair<double, double> compute(const E* src, const E* dest, const double costOff) {
384 double fromResult = -1.;
385 if (myRouter->compute(src, dest, myVehicle, 0, myRoute)) {
386 fromResult = MAX2(0.0, myRouter->recomputeCosts(myRoute, myVehicle, 0) + costOff);
387 myRoute.clear();
388 }
389 double toResult = -1.;
390 if (myReversedRouter != nullptr) {
391 if (myReversedRouter->compute(src->getReversedRoutingEdge(), dest->getReversedRoutingEdge(), myVehicle, 0, myReversedRoute)) {
392 toResult = MAX2(0.0, myReversedRouter->recomputeCosts(myReversedRoute, myVehicle, 0) + costOff);
393 myReversedRoute.clear();
394 }
395 } else {
396 if (myRouter->compute(dest, src, myVehicle, 0, myRoute)) {
397 toResult = MAX2(0.0, myRouter->recomputeCosts(myRoute, myVehicle, 0) + costOff);
398 myRoute.clear();
399 }
400 }
401 return std::make_pair(fromResult, toResult);
402 }
403
404 private:
405 SUMOAbstractRouter<E, V>* myRouter;
406 SUMOAbstractRouter<ReversedEdge<E, V>, V>* myReversedRouter;
407 const V* myVehicle;
408 std::vector<const E*> myRoute;
409 std::vector<const ReversedEdge<E, V>*> myReversedRoute;
410 };
411
412 class RoutingTask : public MFXWorkerThread::Task {
413 public:
414 RoutingTask(const E* src, const E* dest, const double costOff)
415 : mySrc(src), myDest(dest), myCostOff(-costOff) {}
416 void run(MFXWorkerThread* context) {
417 myCost = ((WorkerThread*)context)->compute(mySrc, myDest, myCostOff);
418 }
419 double getFromCost() {
420 return myCost.first;
421 }
422 double getToCost() {
423 return myCost.second;
424 }
425 private:
426 const E* const mySrc;
427 const E* const myDest;
428 const double myCostOff;
429 std::pair<double, double> myCost;
430 private:
432 RoutingTask& operator=(const RoutingTask&) = delete;
433 };
434
435private:
437#endif
438
439 std::string getLandmark(int i) const {
440 for (std::map<std::string, int>::const_iterator it = myLandmarks.begin(); it != myLandmarks.end(); ++it) {
441 if (it->second == i) {
442 return it->first;
443 }
444 }
445 return "";
446 }
447};
#define UNREACHABLE
#define WRITE_WARNINGF(...)
Definition MsgHandler.h:271
#define WRITE_MESSAGEF(...)
Definition MsgHandler.h:273
#define WRITE_MESSAGE(msg)
Definition MsgHandler.h:272
#define TL(string)
Definition MsgHandler.h:287
#define TLF(string,...)
Definition MsgHandler.h:288
#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::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
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
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.