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 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>
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> 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 8581465 : if (nod1->heuristicEffort == nod2->heuristicEffort) {
91 1677137 : return nod1->edge->getNumericalID() > nod2->edge->getNumericalID();
92 : }
93 6904328 : return nod1->heuristicEffort > nod2->heuristicEffort;
94 : }
95 : };
96 :
97 : /// Constructor
98 1231 : 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 2462 : myMaxSpeed(NUMERICAL_EPS) {
103 635243 : for (const E* const edge : edges) {
104 634012 : this->myEdgeInfos.push_back(typename SUMOAbstractRouter<E, V>::EdgeInfo(edge));
105 822379 : myMaxSpeed = MAX2(myMaxSpeed, edge->getSpeedLimit() * MAX2(1.0, edge->getLengthGeometryFactor()));
106 : }
107 1231 : }
108 :
109 66 : 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 132 : myMaxSpeed(NUMERICAL_EPS) {
114 48078 : for (const auto& edgeInfo : edgeInfos) {
115 48012 : this->myEdgeInfos.push_back(typename SUMOAbstractRouter<E, V>::EdgeInfo(edgeInfo.edge));
116 48364 : myMaxSpeed = MAX2(myMaxSpeed, edgeInfo.edge->getSpeedLimit() * edgeInfo.edge->getLengthGeometryFactor());
117 : }
118 66 : }
119 :
120 : /// Destructor
121 2650 : virtual ~AStarRouter() {}
122 :
123 66 : virtual SUMOAbstractRouter<E, V>* clone() {
124 132 : return new AStarRouter<E, V>(this->myEdgeInfos, this->myErrorMsgHandler == MsgHandler::getWarningInstance(), this->myOperation, myLookupTable,
125 132 : this->myHavePermissions, this->myHaveRestrictions);
126 : }
127 :
128 : /** @brief Builds the route between the given edges using the minimum travel time */
129 83359 : 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 83359 : if (this->myEdgeInfos[from->getNumericalID()].prohibited || this->isProhibited(from, vehicle)) {
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 83343 : if (this->myEdgeInfos[to->getNumericalID()].prohibited || this->isProhibited(to, vehicle)) {
140 8 : if (!silent) {
141 24 : this->myErrorMsgHandler->inform("Vehicle '" + Named::getIDSecure(vehicle) + "' is not allowed on destination edge '" + to->getID() + "'.");
142 : }
143 8 : return false;
144 : }
145 83335 : double length = 0.; // dummy for the via edge cost update
146 : this->startQuery();
147 : #ifdef ASTAR_DEBUG_QUERY
148 : if (ASTAR_DEBUG_COND) {
149 : std::cout << "DEBUG: starting search for '" << Named::getIDSecure(vehicle) << "' speed: " << MIN2(vehicle->getMaxSpeed(), myMaxSpeed * vehicle->getChosenSpeedFactor()) << " time: " << STEPS2TIME(msTime) << "\n";
150 : }
151 : #endif
152 :
153 83335 : const SUMOVehicleClass vClass = vehicle == 0 ? SVC_IGNORING : vehicle->getVClass();
154 83335 : if (this->myBulkMode && !this->myAmClean) {
155 117 : const auto& toInfo = this->myEdgeInfos[to->getNumericalID()];
156 117 : if (toInfo.visited) {
157 0 : this->buildPathFrom(&toInfo, into);
158 : this->endQuery(1);
159 0 : return true;
160 : }
161 : } else {
162 83218 : this->init(from->getNumericalID(), msTime);
163 83218 : this->myAmClean = false;
164 : }
165 : // loop
166 : int num_visited = 0;
167 83335 : const bool mayRevisit = myLookupTable != nullptr && !myLookupTable->consistent();
168 83435 : const double speed = vehicle == nullptr ? myMaxSpeed : MIN2(vehicle->getMaxSpeed(), myMaxSpeed * vehicle->getChosenSpeedFactor());
169 1437714 : while (!this->myFrontierList.empty()) {
170 1437532 : num_visited += 1;
171 : // use the node with the minimal length
172 1437532 : auto* const minimumInfo = this->myFrontierList.front();
173 1437532 : const E* const minEdge = minimumInfo->edge;
174 : #ifdef ASTAR_DEBUG_QUERY
175 : if (ASTAR_DEBUG_COND) {
176 : std::cout << "DEBUG: hit=" << minEdge->getID()
177 : << " TT=" << minimumInfo->effort
178 : << " EF=" << this->getEffort(minEdge, vehicle, minimumInfo->leaveTime)
179 : << " HT=" << minimumInfo->heuristicEffort
180 : << " Q(TT,HT,Edge)=";
181 : for (const auto& edgeInfo : this->myFrontierList) {
182 : std::cout << "\n " << edgeInfo->effort << "," << edgeInfo->heuristicEffort << "," << edgeInfo->edge->getID();
183 : }
184 : std::cout << "\n";
185 : }
186 : #endif
187 : // check whether the destination node was already reached
188 1437532 : if (minEdge == to) {
189 83153 : this->buildPathFrom(minimumInfo, into);
190 : this->endQuery(num_visited);
191 : #ifdef ASTAR_DEBUG_QUERY_PERF
192 : if (ASTAR_DEBUG_COND) {
193 : std::cout << "visited " + toString(num_visited) + " edges (final path length=" + toString(into.size())
194 : + " time=" + toString(this->recomputeCosts(into, vehicle, msTime))
195 : + " edges=" + toString(into) + ")\n";
196 : }
197 : #endif
198 : #ifdef ASTAR_DEBUG_VISITED
199 : if (ASTAR_DEBUG_COND) {
200 : OutputDevice& dev = OutputDevice::getDevice(StringUtils::replace(Named::getIDSecure(vehicle), ":", "_") + "_" + toString(STEPS2TIME(msTime)));
201 : for (const auto& i : this->myEdgeInfos) {
202 : if (i.visited) {
203 : dev << "edge:" << i.edge->getID() << "\n";
204 : }
205 : }
206 : dev.close();
207 : }
208 : #endif
209 83153 : return true;
210 : }
211 1354379 : std::pop_heap(this->myFrontierList.begin(), this->myFrontierList.end(), myComparator);
212 : this->myFrontierList.pop_back();
213 1354379 : this->myFound.push_back(minimumInfo);
214 1354379 : minimumInfo->visited = true;
215 1354379 : const double effortDelta = this->getEffort(minEdge, vehicle, minimumInfo->leaveTime);
216 1354379 : const double leaveTime = minimumInfo->leaveTime + this->getTravelTime(minEdge, vehicle, minimumInfo->leaveTime, effortDelta);
217 :
218 : // admissible A* heuristic: straight line distance at maximum speed
219 : // this is calculated from the end of minEdge so it possibly includes via efforts to the followers
220 1354379 : const double heuristic_remaining = (myLookupTable == nullptr ? minEdge->getDistanceTo(to) / speed :
221 7586 : myLookupTable->lowerBound(minEdge, to, speed, vehicle->getChosenSpeedFactor(),
222 : minEdge->getMinimumTravelTime(nullptr), to->getMinimumTravelTime(nullptr)));
223 1354379 : if (heuristic_remaining == UNREACHABLE) {
224 0 : continue;
225 : }
226 1354379 : const double heuristicEffort = minimumInfo->effort + effortDelta + heuristic_remaining;
227 : // check all ways from the node with the minimal length
228 5423661 : for (const std::pair<const E*, const E*>& follower : minEdge->getViaSuccessors(vClass)) {
229 4069282 : auto& followerInfo = this->myEdgeInfos[follower.first->getNumericalID()];
230 : // check whether it can be used
231 4069282 : if (followerInfo.prohibited || this->isProhibited(follower.first, vehicle)) {
232 2248 : continue;
233 : }
234 4067034 : double effort = minimumInfo->effort + effortDelta;
235 4067034 : double time = leaveTime;
236 4067034 : this->updateViaEdgeCost(follower.second, vehicle, time, effort, length);
237 4067034 : const double oldEffort = followerInfo.effort;
238 4067034 : if ((!followerInfo.visited || mayRevisit) && effort < oldEffort) {
239 2237254 : followerInfo.effort = effort;
240 : // if we use the effort including the via effort below we would count the via twice as shown by the ticket676 test
241 : // but we should never get below the real effort, see #12463
242 2237254 : followerInfo.heuristicEffort = MAX2(MIN2(heuristicEffort, followerInfo.heuristicEffort), effort);
243 2237254 : followerInfo.leaveTime = time;
244 2237254 : followerInfo.prev = minimumInfo;
245 : #ifdef ASTAR_DEBUG_QUERY_FOLLOWERS
246 : if (ASTAR_DEBUG_COND) {
247 : std::cout << " follower=" << followerInfo.edge->getID()
248 : << " OEF=" << (oldEffort == std::numeric_limits<double>::max() ? "inf" : toString(oldEffort))
249 : << " TT=" << effort << " HR=" << heuristic_remaining << " HT=" << followerInfo.heuristicEffort << "\n";
250 : }
251 : #endif
252 2237254 : if (oldEffort == std::numeric_limits<double>::max()) {
253 1981290 : this->myFrontierList.push_back(&followerInfo);
254 : std::push_heap(this->myFrontierList.begin(), this->myFrontierList.end(), myComparator);
255 : } else {
256 255964 : auto fi = std::find(this->myFrontierList.begin(), this->myFrontierList.end(), &followerInfo);
257 255964 : if (fi == this->myFrontierList.end()) {
258 : assert(mayRevisit);
259 175 : this->myFrontierList.push_back(&followerInfo);
260 : std::push_heap(this->myFrontierList.begin(), this->myFrontierList.end(), myComparator);
261 : } else {
262 : std::push_heap(this->myFrontierList.begin(), fi + 1, myComparator);
263 : }
264 : }
265 : }
266 : }
267 : }
268 : this->endQuery(num_visited);
269 : #ifdef ASTAR_DEBUG_QUERY_PERF
270 : if (ASTAR_DEBUG_COND) {
271 : std::cout << "visited " + toString(num_visited) + " edges (unsuccessful path length: " + toString(into.size()) + ")\n";
272 : }
273 : #endif
274 182 : if (!silent) {
275 462 : this->myErrorMsgHandler->informf(TL("No connection between edge '%' and edge '%' found."), from->getID(), to->getID());
276 : }
277 : return false;
278 : }
279 :
280 :
281 : protected:
282 : EdgeInfoComparator myComparator;
283 :
284 : /// @brief the lookup table for travel time heuristics
285 : const std::shared_ptr<const LookupTable> myLookupTable;
286 :
287 : /// @brief maximum speed in the network
288 : double myMaxSpeed;
289 : };
|
