Line data Source code
1 : /****************************************************************************/
2 : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3 : // Copyright (C) 2012-2026 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 AStarRouter.h
15 : /// @author Jakob Erdmann
16 : /// @author Daniel Krajzewicz
17 : /// @author Michael Behrisch
18 : /// @date January 2012
19 : ///
20 : // A* Algorithm using euclidean distance heuristic.
21 : // Based on DijkstraRouter. For routing by effort a novel heuristic would be needed.
22 : /****************************************************************************/
23 : #pragma once
24 : #include <config.h>
25 :
26 : #include <cassert>
27 : #include <string>
28 : #include <functional>
29 : #include <vector>
30 : #include <set>
31 : #include <limits>
32 : #include <algorithm>
33 : #include <iterator>
34 : #include <map>
35 : #include <iostream>
36 : #include <memory>
37 : #include <utils/common/MsgHandler.h>
38 : #include <utils/common/StringTokenizer.h>
39 : #include <utils/common/StringUtils.h>
40 : #include <utils/common/StdDefs.h>
41 : #include <utils/common/ToString.h>
42 : #include <utils/iodevices/OutputDevice.h>
43 : #include "AStarLookupTable.h"
44 : #include "SUMOAbstractRouter.h"
45 :
46 : #define UNREACHABLE (std::numeric_limits<double>::max() / 1000.0)
47 :
48 : //#define ASTAR_DEBUG_QUERY
49 : //#define ASTAR_DEBUG_QUERY_FOLLOWERS
50 : //#define ASTAR_DEBUG_QUERY_PERF
51 : //#define ASTAR_DEBUG_VISITED
52 : //#define ASTAR_DEBUG_COND (vehicle->getID() == "")
53 : //#define ASTAR_DEBUG_COND (true)
54 : //#define ASTAR_DEBUG_LOOKUPTABLE
55 : //#define ASTAR_DEBUG_LOOKUPTABLE_FROM "disabled"
56 : //#define ASTAR_DEBUG_UNREACHABLE
57 :
58 : // ===========================================================================
59 : // class definitions
60 : // ===========================================================================
61 : /**
62 : * @class AStarRouter
63 : * @brief Computes the shortest path through a network using the A* algorithm.
64 : *
65 : * The template parameters are:
66 : * @param E The edge class to use (MSEdge/ROEdge)
67 : * @param V The vehicle class to use (MSVehicle/ROVehicle)
68 : * @param BASE The base class to use (SUMOAbstractRouterPermissions/SUMOAbstractRouter)
69 : *
70 : * The router is edge-based. It must know the number of edges for internal reasons
71 : * and whether a missing connection between two given edges (unbuild route) shall
72 : * be reported as an error or as a warning.
73 : *
74 : */
75 : template<class E, class V, class M>
76 : class AStarRouter : public SUMOAbstractRouter<E, V> {
77 : public:
78 : typedef AbstractLookupTable<E, V> LookupTable;
79 : typedef FullLookupTable<E, V> FLT;
80 : typedef LandmarkLookupTable<E, V, M> LMLT;
81 :
82 : /**
83 : * @class EdgeInfoComparator
84 : * Class to compare (and so sort) nodes by their effort
85 : */
86 : class EdgeInfoComparator {
87 : public:
88 : /// Comparing method
89 : bool operator()(const typename SUMOAbstractRouter<E, V>::EdgeInfo* nod1, const typename SUMOAbstractRouter<E, V>::EdgeInfo* nod2) const {
90 14878726 : if (nod1->heuristicEffort == nod2->heuristicEffort) {
91 3085259 : return nod1->edge->getNumericalID() > nod2->edge->getNumericalID();
92 : }
93 11793467 : return nod1->heuristicEffort > nod2->heuristicEffort;
94 : }
95 : };
96 :
97 : /// Constructor
98 1223 : AStarRouter(const std::vector<E*>& edges, bool unbuildIsWarning, typename SUMOAbstractRouter<E, V>::Operation operation, const std::shared_ptr<const LookupTable> lookup = nullptr,
99 : const bool havePermissions = false, const bool haveRestrictions = false) :
100 : SUMOAbstractRouter<E, V>("AStarRouter", unbuildIsWarning, operation, nullptr, havePermissions, haveRestrictions),
101 : myLookupTable(lookup),
102 2446 : myMaxSpeed(NUMERICAL_EPS) {
103 696758 : for (const E* const edge : edges) {
104 695535 : this->myEdgeInfos.push_back(typename SUMOAbstractRouter<E, V>::EdgeInfo(edge));
105 877414 : myMaxSpeed = MAX2(myMaxSpeed, edge->getSpeedLimit() * MAX2(1.0, edge->getLengthGeometryFactor()));
106 : }
107 1223 : }
108 :
109 68 : AStarRouter(const std::vector<typename SUMOAbstractRouter<E, V>::EdgeInfo>& edgeInfos, bool unbuildIsWarning, typename SUMOAbstractRouter<E, V>::Operation operation, const std::shared_ptr<const LookupTable> lookup = nullptr,
110 : const bool havePermissions = false, const bool haveRestrictions = false) :
111 : SUMOAbstractRouter<E, V>("AStarRouter", unbuildIsWarning, operation, nullptr, havePermissions, haveRestrictions),
112 : myLookupTable(lookup),
113 136 : myMaxSpeed(NUMERICAL_EPS) {
114 54436 : for (const auto& edgeInfo : edgeInfos) {
115 54368 : this->myEdgeInfos.push_back(typename SUMOAbstractRouter<E, V>::EdgeInfo(edgeInfo.edge));
116 54728 : myMaxSpeed = MAX2(myMaxSpeed, edgeInfo.edge->getSpeedLimit() * edgeInfo.edge->getLengthGeometryFactor());
117 : }
118 68 : }
119 :
120 : /// Destructor
121 2656 : virtual ~AStarRouter() {}
122 :
123 68 : virtual SUMOAbstractRouter<E, V>* clone() {
124 136 : return new AStarRouter<E, V, M>(this->myEdgeInfos, this->myErrorMsgHandler == MsgHandler::getWarningInstance(), this->myOperation, myLookupTable,
125 136 : this->myHavePermissions, this->myHaveRestrictions);
126 : }
127 :
128 : /** @brief Builds the route between the given edges using the minimum travel time */
129 117958 : bool compute(const E* from, const E* to, const V* const vehicle,
130 : SUMOTime msTime, std::vector<const E*>& into, bool silent = false) {
131 : assert(from != nullptr && to != nullptr);
132 : // check whether from and to can be used
133 117958 : if (this->isProhibited(from, vehicle, STEPS2TIME(msTime))) {
134 16 : if (!silent) {
135 48 : this->myErrorMsgHandler->inform("Vehicle '" + Named::getIDSecure(vehicle) + "' is not allowed on source edge '" + from->getID() + "'.");
136 : }
137 16 : return false;
138 : }
139 : // technically, a temporary permission might be lifted by the time of arrival
140 117942 : if (this->isProhibited(to, vehicle, STEPS2TIME(msTime))) {
141 21 : if (!silent) {
142 24 : this->myErrorMsgHandler->inform("Vehicle '" + Named::getIDSecure(vehicle) + "' is not allowed on destination edge '" + to->getID() + "'.");
143 : }
144 21 : return false;
145 : }
146 117921 : double length = 0.; // dummy for the via edge cost update
147 : this->startQuery();
148 : #ifdef ASTAR_DEBUG_QUERY
149 : if (ASTAR_DEBUG_COND) {
150 : std::cout << "DEBUG: starting search for '" << Named::getIDSecure(vehicle) << "' speed: " << MIN2(vehicle->getMaxSpeed(), myMaxSpeed * vehicle->getChosenSpeedFactor()) << " time: " << STEPS2TIME(msTime) << "\n";
151 : }
152 : #endif
153 :
154 117921 : const SUMOVehicleClass vClass = vehicle == 0 ? SVC_IGNORING : vehicle->getVClass();
155 117921 : if (this->myBulkMode && !this->myAmClean) {
156 118 : const auto& toInfo = this->myEdgeInfos[to->getNumericalID()];
157 118 : if (toInfo.visited) {
158 0 : this->buildPathFrom(&toInfo, into);
159 : this->endQuery(1);
160 0 : return true;
161 : }
162 : } else {
163 117803 : this->init(from->getNumericalID(), msTime);
164 117803 : this->myAmClean = false;
165 : }
166 : // loop
167 : int num_visited = 0;
168 117921 : const bool mayRevisit = myLookupTable != nullptr && !myLookupTable->consistent();
169 122494 : const double speed = vehicle == nullptr ? myMaxSpeed : MIN2(vehicle->getMaxSpeed(), myMaxSpeed * vehicle->getChosenSpeedFactor());
170 2302848 : while (!this->myFrontierList.empty()) {
171 2302660 : num_visited += 1;
172 : // use the node with the minimal length
173 2302660 : auto* const minimumInfo = this->myFrontierList.front();
174 2302660 : const E* const minEdge = minimumInfo->edge;
175 : #ifdef ASTAR_DEBUG_QUERY
176 : if (ASTAR_DEBUG_COND) {
177 : std::cout << "DEBUG: hit=" << minEdge->getID()
178 : << " TT=" << minimumInfo->effort
179 : << " EF=" << this->getEffort(minEdge, vehicle, minimumInfo->leaveTime)
180 : << " HT=" << minimumInfo->heuristicEffort
181 : << " Q(TT,HT,Edge)=";
182 : for (const auto& edgeInfo : this->myFrontierList) {
183 : std::cout << "\n " << edgeInfo->effort << "," << edgeInfo->heuristicEffort << "," << edgeInfo->edge->getID();
184 : }
185 : std::cout << "\n";
186 : }
187 : #endif
188 : // check whether the destination node was already reached
189 2302660 : if (minEdge == to) {
190 117733 : this->buildPathFrom(minimumInfo, into);
191 : this->endQuery(num_visited);
192 : #ifdef ASTAR_DEBUG_QUERY_PERF
193 : if (ASTAR_DEBUG_COND) {
194 : std::cout << "visited " + toString(num_visited) + " edges (final path length=" + toString(into.size())
195 : + " time=" + toString(this->recomputeCosts(into, vehicle, msTime))
196 : + " edges=" + toString(into) + ")\n";
197 : }
198 : #endif
199 : #ifdef ASTAR_DEBUG_VISITED
200 : if (ASTAR_DEBUG_COND) {
201 : OutputDevice& dev = OutputDevice::getDevice(StringUtils::replace(Named::getIDSecure(vehicle), ":", "_") + "_" + toString(STEPS2TIME(msTime)));
202 : for (const auto& i : this->myEdgeInfos) {
203 : if (i.visited) {
204 : dev << "edge:" << i.edge->getID() << "\n";
205 : }
206 : }
207 : dev.close();
208 : }
209 : #endif
210 117733 : return true;
211 : }
212 2184927 : std::pop_heap(this->myFrontierList.begin(), this->myFrontierList.end(), myComparator);
213 : this->myFrontierList.pop_back();
214 2184927 : this->myFound.push_back(minimumInfo);
215 2184927 : minimumInfo->visited = true;
216 2184927 : const double effortDelta = this->getEffort(minEdge, vehicle, minimumInfo->leaveTime);
217 2184927 : const double leaveTime = minimumInfo->leaveTime + this->getTravelTime(minEdge, vehicle, minimumInfo->leaveTime, effortDelta);
218 :
219 : // admissible A* heuristic: straight line distance at maximum speed
220 : // this is calculated from the end of minEdge so it possibly includes via efforts to the followers
221 2184927 : const double heuristic_remaining = (myLookupTable == nullptr ? minEdge->getDistanceTo(to) / speed :
222 11714 : myLookupTable->lowerBound(minEdge, to, speed, vehicle->getChosenSpeedFactor(),
223 : minEdge->getMinimumTravelTime(nullptr), to->getMinimumTravelTime(nullptr)));
224 2184927 : if (heuristic_remaining == UNREACHABLE) {
225 4 : continue;
226 : }
227 2184923 : const double heuristicEffort = minimumInfo->effort + effortDelta + heuristic_remaining;
228 : // check all ways from the node with the minimal length
229 9070379 : for (const std::pair<const E*, const E*>& follower : minEdge->getViaSuccessors(vClass)) {
230 6885456 : auto& followerInfo = this->myEdgeInfos[follower.first->getNumericalID()];
231 : // check whether it can be used
232 6885456 : if (this->isProhibited(follower.first, vehicle, leaveTime)) {
233 6958 : continue;
234 : }
235 6878498 : double effort = minimumInfo->effort + effortDelta;
236 6878498 : double time = leaveTime;
237 6878498 : this->updateViaEdgeCost(follower.second, vehicle, time, effort, length);
238 6878498 : const double oldEffort = followerInfo.effort;
239 6878498 : if ((!followerInfo.visited || mayRevisit) && effort < oldEffort) {
240 3832572 : followerInfo.effort = effort;
241 : // if we use the effort including the via effort below we would count the via twice as shown by the ticket676 test
242 : // but we should never get below the real effort, see #12463
243 3832572 : followerInfo.heuristicEffort = MAX2(MIN2(heuristicEffort, followerInfo.heuristicEffort), effort);
244 3832572 : followerInfo.leaveTime = time;
245 3832572 : followerInfo.prev = minimumInfo;
246 : #ifdef ASTAR_DEBUG_QUERY_FOLLOWERS
247 : if (ASTAR_DEBUG_COND) {
248 : std::cout << " follower=" << followerInfo.edge->getID()
249 : << " OEF=" << (oldEffort == std::numeric_limits<double>::max() ? "inf" : toString(oldEffort))
250 : << " TT=" << effort << " HR=" << heuristic_remaining << " HT=" << followerInfo.heuristicEffort << "\n";
251 : }
252 : #endif
253 3832572 : if (oldEffort == std::numeric_limits<double>::max()) {
254 3478969 : this->myFrontierList.push_back(&followerInfo);
255 : std::push_heap(this->myFrontierList.begin(), this->myFrontierList.end(), myComparator);
256 : } else {
257 353603 : auto fi = std::find(this->myFrontierList.begin(), this->myFrontierList.end(), &followerInfo);
258 353603 : if (fi == this->myFrontierList.end()) {
259 : assert(mayRevisit);
260 201 : this->myFrontierList.push_back(&followerInfo);
261 : std::push_heap(this->myFrontierList.begin(), this->myFrontierList.end(), myComparator);
262 : } else {
263 : std::push_heap(this->myFrontierList.begin(), fi + 1, myComparator);
264 : }
265 : }
266 : }
267 : }
268 : }
269 : this->endQuery(num_visited);
270 : #ifdef ASTAR_DEBUG_QUERY_PERF
271 : if (ASTAR_DEBUG_COND) {
272 : std::cout << "visited " + toString(num_visited) + " edges (unsuccessful path length: " + toString(into.size()) + ")\n";
273 : }
274 : #endif
275 188 : if (!silent) {
276 462 : this->myErrorMsgHandler->informf(TL("No connection between edge '%' and edge '%' found."), from->getID(), to->getID());
277 : }
278 : return false;
279 : }
280 :
281 :
282 : protected:
283 : EdgeInfoComparator myComparator;
284 :
285 : /// @brief the lookup table for travel time heuristics
286 : const std::shared_ptr<const LookupTable> myLookupTable;
287 :
288 : /// @brief maximum speed in the network
289 : double myMaxSpeed;
290 : };
|
