Line data Source code
1 : /****************************************************************************/
2 : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3 : // Copyright (C) 2012-2025 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 Node2EdgeRouter.h
15 : /// @author Ruediger Ebendt
16 : /// @date 01.01.2024
17 : ///
18 : // Simple extension of edge-based AStarRouter, adding a) a new method computeNode2Edge,
19 : // which computes a shortest path starting at a given node and ending at a given edge,
20 : // and b) a new method computeNode2Edges, which computes shortest paths, each starting
21 : // at the given node and ending at one of the given edges
22 : /****************************************************************************/
23 : #pragma once
24 : #include <config.h>
25 : #include <unordered_set>
26 : #include <utils/common/StdDefs.h>
27 : #include "AStarRouter.h"
28 :
29 : // ===========================================================================
30 : // class definitions
31 : // ===========================================================================
32 : /**
33 : * @class Node2EdgeRouter
34 : * Computes shortest paths from a node to one edge (using A*) or to all edges
35 : * (using Dijkstra's algorithm)
36 : *
37 : * The template parameters are:
38 : * @param E The edge class to use (MSEdge/ROEdge)
39 : * @param V The vehicle class to use (MSVehicle/ROVehicle)
40 : *
41 : * The router is edge-based. It must know the number of edges for internal reasons
42 : * and whether a missing connection between two given edges (unbuild route) shall
43 : * be reported as an error or as a warning
44 : *
45 : */
46 : template<class E, class N, class V, class M>
47 : class Node2EdgeRouter : public AStarRouter<E, V, M> {
48 : public:
49 : typedef AbstractLookupTable<E, V> LookupTable;
50 :
51 : /** @brief Returns the edge information for the passed edge
52 : * @param[in] edge The edge
53 : * @note Non-const version
54 : */
55 : typename SUMOAbstractRouter<E, V>::EdgeInfo* edgeInfo(const E* const edge) {
56 0 : return &(this->myEdgeInfos[edge->getNumericalID()]);
57 : }
58 : /** @brief Returns the edge information for the passed edge
59 : * @param[in] edge The edge
60 : * @note Const version
61 : */
62 : const typename SUMOAbstractRouter<E, V>::EdgeInfo* edgeInfo(const E* const edge) const {
63 : return &(this->myEdgeInfos[edge->getNumericalID()]);
64 : }
65 :
66 : /** @brief Constructor
67 : * @param[in] edges The edges
68 : * @param[in] unbuildIsWarning The flag indicating whether network unbuilds should issue warnings or errors
69 : * @param[in] operation The operation
70 : * @param[in] lookup The lookup table
71 : * @param[in] havePermissions The flag indicating whether to respect edge permissions
72 : * @param[in] haveRestrictions The flag indicating whether to respect edge restrictions
73 : */
74 0 : Node2EdgeRouter(const std::vector<E*>& edges, bool unbuildIsWarning, typename SUMOAbstractRouter<E, V>::Operation operation,
75 : const std::shared_ptr<const LookupTable> lookup = nullptr,
76 : const bool havePermissions = false, const bool haveRestrictions = false) :
77 0 : AStarRouter<E, V, M>(edges, unbuildIsWarning, operation, lookup, havePermissions, haveRestrictions) {
78 0 : }
79 :
80 : /** @brief Cloning constructor
81 : * @param[in] edgeInfos The vector of edge information
82 : * @param[in] unbuildIsWarning The flag indicating whether network unbuilds should issue warnings or errors
83 : * @param[in] operation The operation
84 : * @param[in] lookup The lookup table
85 : * @param[in] havePermissions The flag indicating whether to respect edge permissions
86 : * @param[in] haveRestrictions The flag indicating whether to respect edge restrictions
87 : */
88 0 : Node2EdgeRouter(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,
89 : const bool havePermissions = false, const bool haveRestrictions = false) :
90 0 : AStarRouter<E, V, M>(edgeInfos, unbuildIsWarning, operation, lookup, havePermissions, haveRestrictions) {
91 0 : }
92 :
93 : /// @brief Destructor
94 0 : virtual ~Node2EdgeRouter() {
95 0 : WRITE_MESSAGE("The following stats for AStarRouter (which might also be empty) belong to Node2EdgeRouter, a derived class:");
96 0 : }
97 :
98 : /// @brief Cloning method
99 0 : virtual SUMOAbstractRouter<E, V>* clone() {
100 0 : return new Node2EdgeRouter<E, N, V, M>(this->myEdgeInfos, this->myErrorMsgHandler == MsgHandler::getWarningInstance(), this->myOperation, this->myLookupTable,
101 0 : this->myHavePermissions, this->myHaveRestrictions);
102 : }
103 :
104 : /** @brief Reset method
105 : * @param[in] vehicle The vehicle
106 : */
107 0 : virtual void reset(const V* const vehicle) {
108 : UNUSED_PARAMETER(vehicle);
109 0 : for (auto& edgeInfo : this->myFrontierList) {
110 0 : edgeInfo->reset();
111 : }
112 : this->myFrontierList.clear();
113 0 : for (auto& edgeInfo : this->myFound) {
114 0 : edgeInfo->reset();
115 : }
116 : this->myFound.clear();
117 0 : }
118 :
119 : /** @brief Builds the routes between the given node and all given edges using the minimum travel time
120 : * @param[in] fromNode The node which the routes start with
121 : * @param[in] toEdges The edges at which the routes end
122 : * @param[in] vehicle The vehicle
123 : * @param[in] msTime The start time of the routes in milliseconds
124 : * @param[in] silent The boolean flag indicating whether the method stays silent or puts out messages
125 : */
126 0 : bool computeNode2Edges(const N* fromNode, const std::unordered_set<const E*>& toEdges, const V* const vehicle,
127 : SUMOTime msTime, bool silent = false) {
128 : assert(fromNode != nullptr);
129 : // check whether fromNode and to can be used
130 : bool found = false;
131 : bool allVisited = true; // we try to disprove this
132 0 : for (const E* from : fromNode->getOutgoing()) {
133 0 : if (from->isInternal()) {
134 0 : continue;
135 : }
136 0 : if (this->myEdgeInfos[from->getNumericalID()].prohibited || this->isProhibited(from, vehicle)) {
137 0 : if (!silent) {
138 0 : this->myErrorMsgHandler->inform("Vehicle '" + Named::getIDSecure(vehicle) + "' is not allowed on source edge '" + from->getID() + "'.");
139 : }
140 : } else {
141 : found = true;
142 : break;
143 : }
144 : }
145 0 : if (!found) {
146 : return false;
147 : }
148 0 : const std::vector<const E*>& fromEdges = fromNode->getOutgoing();
149 :
150 0 : if (fromEdges.empty() || toEdges.empty()) { // nothing to do here
151 : return false;
152 : }
153 :
154 0 : double length = 0.; // dummy for the via edge cost update
155 : this->startQuery();
156 : #ifdef ASTAR_DEBUG_QUERY
157 : if (ASTAR_DEBUG_COND) {
158 : std::cout << "DEBUG: starting search for '" << Named::getIDSecure(vehicle) << "' speed: " << MIN2(vehicle->getMaxSpeed(), myMaxSpeed * vehicle->getChosenSpeedFactor()) << " time: " << STEPS2TIME(msTime) << "\n";
159 : }
160 : #endif
161 :
162 0 : const SUMOVehicleClass vClass = vehicle == 0 ? SVC_IGNORING : vehicle->getVClass();
163 0 : init(fromEdges, vehicle, msTime);
164 0 : this->myAmClean = false;
165 : // loop
166 : int num_visited = 0;
167 0 : while (!this->myFrontierList.empty()) {
168 0 : num_visited += 1;
169 : // use the edge with the minimal length
170 0 : auto* const minimumInfo = this->myFrontierList.front();
171 0 : const E* const minEdge = minimumInfo->edge;
172 : // check whether the destination edge was already reached
173 : if (toEdges.count(minEdge)) {
174 0 : for (const E* to : toEdges) {
175 0 : const auto& toInfo = this->myEdgeInfos[to->getNumericalID()];
176 0 : if (!toInfo.visited) {
177 : allVisited = false;
178 : break;
179 : }
180 : }
181 0 : if (allVisited) {
182 : this->endQuery(num_visited);
183 0 : return true;
184 : }
185 : }
186 0 : std::pop_heap(this->myFrontierList.begin(), this->myFrontierList.end(), this->myComparator);
187 : this->myFrontierList.pop_back();
188 0 : this->myFound.push_back(minimumInfo);
189 0 : minimumInfo->visited = true;
190 0 : const double effortDelta = this->getEffort(minEdge, vehicle, minimumInfo->leaveTime);
191 0 : const double leaveTime = minimumInfo->leaveTime + this->getTravelTime(minEdge, vehicle, minimumInfo->leaveTime, effortDelta);
192 :
193 : const double heuristic_remaining = 0;
194 : if (heuristic_remaining == UNREACHABLE) {
195 : continue;
196 : }
197 0 : const double heuristicEffort = minimumInfo->effort + effortDelta + heuristic_remaining;
198 : // check all ways from the edge with the minimal length
199 0 : for (const std::pair<const E*, const E*>& follower : minEdge->getViaSuccessors(vClass)) {
200 0 : auto& followerInfo = this->myEdgeInfos[follower.first->getNumericalID()];
201 : // check whether it can be used
202 0 : if (followerInfo.prohibited || this->isProhibited(follower.first, vehicle)) {
203 0 : continue;
204 : }
205 0 : double effort = minimumInfo->effort + effortDelta;
206 0 : double time = leaveTime;
207 0 : this->updateViaEdgeCost(follower.second, vehicle, time, effort, length);
208 0 : const double oldEffort = followerInfo.effort;
209 0 : if ((!followerInfo.visited) && effort < oldEffort) {
210 0 : followerInfo.effort = effort;
211 : // if we use the effort including the via effort below we would count the via twice as shown by the ticket676 test
212 : // but we should never get below the real effort, see #12463
213 0 : followerInfo.heuristicEffort = MAX2(MIN2(heuristicEffort, followerInfo.heuristicEffort), effort);
214 0 : followerInfo.leaveTime = time;
215 0 : followerInfo.prev = minimumInfo;
216 0 : if (oldEffort == std::numeric_limits<double>::max()) {
217 0 : this->myFrontierList.push_back(&followerInfo);
218 : std::push_heap(this->myFrontierList.begin(), this->myFrontierList.end(), this->myComparator);
219 : } else {
220 0 : auto fi = std::find(this->myFrontierList.begin(), this->myFrontierList.end(), &followerInfo);
221 : assert(fi != this->myFrontierList.end());
222 : std::push_heap(this->myFrontierList.begin(), fi + 1, this->myComparator);
223 : }
224 : }
225 : }
226 : }
227 : this->endQuery(num_visited);
228 0 : for (const E* to : toEdges) {
229 0 : const auto& toInfo = this->myEdgeInfos[to->getNumericalID()];
230 0 : if (toInfo.visited) {
231 0 : if (!silent) {
232 0 : this->myErrorMsgHandler->informf("Only some connections between node '%' and the given edges were found.", fromNode->getID());
233 : }
234 : return true; // at least one connection could be found
235 : }
236 : }
237 0 : if (!silent) {
238 0 : this->myErrorMsgHandler->informf("No connections between node '%' and the given edges were found.", fromNode->getID());
239 : }
240 : return false;
241 : }
242 :
243 : /** @brief Builds the route between the given node and edge using the minimum travel time
244 : * @param[in] fromNode The node the route starts with
245 : * @param[in] to The edge at which the route ends
246 : * @param[in] vehicle The vehicle
247 : * @param[in] msTime The start time of the route in milliseconds
248 : * @param[out] into The vector of edges, into which the solution route is written
249 : * @param[in] silent The boolean flag indicating whether the method stays silent or puts out messages
250 : */
251 : bool computeNode2Edge(const N* fromNode, const E* to, const V* const vehicle,
252 : SUMOTime msTime, std::vector<const E*>& into, bool silent = false) {
253 : assert(fromNode != nullptr && to != nullptr);
254 : // check whether fromNode and to can be used
255 : bool found = false;
256 : for (const E* from : fromNode->getOutgoing()) {
257 : if (from->isInternal()) {
258 : continue;
259 : }
260 : if (this->myEdgeInfos[from->getNumericalID()].prohibited || this->isProhibited(from, vehicle)) {
261 : if (!silent) {
262 : this->myErrorMsgHandler->inform("Vehicle '" + Named::getIDSecure(vehicle) + "' is not allowed on source edge '" + from->getID() + "'.");
263 : }
264 : } else {
265 : found = true;
266 : break;
267 : }
268 : }
269 : if (!found) {
270 : return false;
271 : }
272 : const std::vector<const E*>& fromEdges = fromNode->getOutgoing();
273 :
274 : if (fromEdges.empty()) { // nothing to do here
275 : return false;
276 : }
277 : if (this->myEdgeInfos[to->getNumericalID()].prohibited || this->isProhibited(to, vehicle)) {
278 : if (!silent) {
279 : this->myErrorMsgHandler->inform("Vehicle '" + Named::getIDSecure(vehicle) + "' is not allowed on destination edge '" + to->getID() + "'.");
280 : }
281 : return false;
282 : }
283 : double length = 0.; // dummy for the via edge cost update
284 : this->startQuery();
285 :
286 : const SUMOVehicleClass vClass = vehicle == 0 ? SVC_IGNORING : vehicle->getVClass();
287 : init(fromEdges, vehicle, msTime);
288 : this->myAmClean = false;
289 : // loop
290 : int num_visited = 0;
291 : const bool mayRevisit = this->myLookupTable != nullptr && !this->myLookupTable->consistent();
292 : const double speed = vehicle == nullptr ? this->myMaxSpeed : MIN2(vehicle->getMaxSpeed(), this->myMaxSpeed * vehicle->getChosenSpeedFactor());
293 : while (!this->myFrontierList.empty()) {
294 : num_visited += 1;
295 : // use the edge with the minimal length
296 : auto* const minimumInfo = this->myFrontierList.front();
297 : const E* const minEdge = minimumInfo->edge;
298 : // check whether the destination edge was already reached
299 : if (minEdge == to) {
300 : this->buildPathFrom(minimumInfo, into);
301 : this->endQuery(num_visited);
302 : return true;
303 : }
304 : std::pop_heap(this->myFrontierList.begin(), this->myFrontierList.end(), this->myComparator);
305 : this->myFrontierList.pop_back();
306 : this->myFound.push_back(minimumInfo);
307 : minimumInfo->visited = true;
308 : const double effortDelta = this->getEffort(minEdge, vehicle, minimumInfo->leaveTime);
309 : const double leaveTime = minimumInfo->leaveTime + this->getTravelTime(minEdge, vehicle, minimumInfo->leaveTime, effortDelta);
310 :
311 : // admissible A* heuristic: straight line distance at maximum speed
312 : // this is calculated from the end of minEdge so it possibly includes via efforts to the followers
313 : const double heuristic_remaining = (this->myLookupTable == nullptr ? minEdge->getDistanceTo(to) / speed :
314 : this->myLookupTable->lowerBound(minEdge, to, speed, vehicle->getChosenSpeedFactor(),
315 : minEdge->getMinimumTravelTime(nullptr), to->getMinimumTravelTime(nullptr)));
316 : //const double heuristic_remaining = 0.;
317 : if (heuristic_remaining == UNREACHABLE) {
318 : continue;
319 : }
320 : const double heuristicEffort = minimumInfo->effort + effortDelta + heuristic_remaining;
321 : // check all ways from the edge with the minimal length
322 : for (const std::pair<const E*, const E*>& follower : minEdge->getViaSuccessors(vClass)) {
323 : auto& followerInfo = this->myEdgeInfos[follower.first->getNumericalID()];
324 : // check whether it can be used
325 : if (followerInfo.prohibited || this->isProhibited(follower.first, vehicle)) {
326 : continue;
327 : }
328 : double effort = minimumInfo->effort + effortDelta;
329 : double time = leaveTime;
330 : this->updateViaEdgeCost(follower.second, vehicle, time, effort, length);
331 : const double oldEffort = followerInfo.effort;
332 : if ((!followerInfo.visited || mayRevisit) && effort < oldEffort) {
333 : followerInfo.effort = effort;
334 : // if we use the effort including the via effort below we would count the via twice as shown by the ticket676 test
335 : // but we should never get below the real effort, see #12463
336 : followerInfo.heuristicEffort = MAX2(MIN2(heuristicEffort, followerInfo.heuristicEffort), effort);
337 : followerInfo.leaveTime = time;
338 : followerInfo.prev = minimumInfo;
339 : if (oldEffort == std::numeric_limits<double>::max()) {
340 : this->myFrontierList.push_back(&followerInfo);
341 : std::push_heap(this->myFrontierList.begin(), this->myFrontierList.end(), this->myComparator);
342 : } else {
343 : auto fi = std::find(this->myFrontierList.begin(), this->myFrontierList.end(), &followerInfo);
344 : if (fi == this->myFrontierList.end()) {
345 : assert(mayRevisit);
346 : this->myFrontierList.push_back(&followerInfo);
347 : std::push_heap(this->myFrontierList.begin(), this->myFrontierList.end(), this->myComparator);
348 : } else {
349 : std::push_heap(this->myFrontierList.begin(), fi + 1, this->myComparator);
350 : }
351 : }
352 : }
353 : }
354 : }
355 : this->endQuery(num_visited);
356 : if (!silent) {
357 : this->myErrorMsgHandler->informf("No connection between node '%' and edge '%' found.", fromNode->getID(), to->getID());
358 : }
359 : return false;
360 : }
361 :
362 : /** @brief Updates the via cost up to a given edge
363 : * @param[in] prev The previous edge
364 : * @param[in] e The given edge
365 : * @param[in] v The vehicle
366 : * @param[in,out] time The passed and updated start time in seconds
367 : * @param[in,out] effort The passed and updated effort
368 : * @param[in,out] length The passed and updated length
369 : */
370 : void updateViaCostUpToEdge(const E* const prev, const E* const e, const V* const v, double& time, double& effort, double& length) const;
371 :
372 : /** @brief Returns the recomputed cost for traversing the given edges, excluding the last one
373 : * @param[in] edges The vector of edges
374 : * @param[in] v The vehicle
375 : * @param[in] msTime The start time in milliseconds
376 : * @param[in,out] lengthp The passed and updated length
377 : */
378 : double recomputeCostsNoLastEdge(const std::vector<const E*>& edges, const V* const v, SUMOTime msTime, double* lengthp = nullptr) const {
379 : const E* lastEdge = edges.back();
380 : double time = STEPS2TIME(msTime);
381 : double effort = 0.;
382 : double length = 0.;
383 : if (lengthp == nullptr) {
384 : lengthp = &length;
385 : } else {
386 : *lengthp = 0.;
387 : }
388 : const E* prev = nullptr;
389 : for (const E* const e : edges) {
390 : if (e == lastEdge) {
391 : break;
392 : }
393 : if (isProhibited(e, v)) {
394 : return -1;
395 : }
396 : updateViaCost(prev, e, v, time, effort, *lengthp);
397 : prev = e;
398 : }
399 : updateViaCostUpToEdge(prev, lastEdge, v, time, effort, *lengthp);
400 : return effort;
401 : }
402 :
403 : /// @brief Bulk mode is not supported
404 0 : virtual void setBulkMode(const bool mode) {
405 : UNUSED_PARAMETER(mode);
406 0 : throw std::runtime_error("Bulk mode is not supported by the node-to-edge router.");
407 : }
408 :
409 : protected:
410 : /** @brief Initialize the node-to-edge router
411 : * @param[in] fromEdges The vector of start edges
412 : * @param[in] vehicle The vehicle
413 : * @param[in] msTime The start time in milliseconds
414 : */
415 : void init(std::vector<const E*> fromEdges, const V* const vehicle, const SUMOTime msTime);
416 : };
417 :
418 : // ===========================================================================
419 : // method definitions
420 : // ===========================================================================
421 :
422 : template<class E, class N, class V, class M>
423 : void Node2EdgeRouter<E, N, V, M>::updateViaCostUpToEdge(const E* const prev, const E* const e, const V* const v, double& time, double& effort, double& length) const {
424 : assert(prev && e);
425 : for (const std::pair<const E*, const E*>& follower : prev->getViaSuccessors()) {
426 : if (follower.first == e) {
427 : updateViaEdgeCost(follower.second, v, time, effort, length);
428 : break;
429 : }
430 : }
431 : }
432 :
433 : template<class E, class N, class V, class M>
434 0 : void Node2EdgeRouter<E, N, V, M>::init(std::vector<const E*> fromEdges, const V* const vehicle, const SUMOTime msTime) {
435 : // all EdgeInfos touched in the previous query are either in myFrontierList or myFound: clean those up
436 0 : for (auto& edgeInfo : this->myFrontierList) {
437 0 : edgeInfo->reset();
438 : }
439 : this->myFrontierList.clear();
440 0 : for (auto& edgeInfo : this->myFound) {
441 0 : edgeInfo->reset();
442 : }
443 : this->myFound.clear();
444 0 : for (const E* from : fromEdges) {
445 0 : if (from->isInternal()) {
446 0 : continue;
447 : }
448 : int edgeID = from->getNumericalID();
449 0 : auto& fromInfo = this->myEdgeInfos[edgeID];
450 0 : if (fromInfo.prohibited || this->isProhibited(from, vehicle)) {
451 0 : continue;
452 : }
453 0 : fromInfo.effort = 0.;
454 0 : fromInfo.heuristicEffort = 0.;
455 0 : fromInfo.prev = nullptr;
456 0 : fromInfo.leaveTime = STEPS2TIME(msTime);
457 0 : this->myFrontierList.push_back(&fromInfo);
458 : }
459 0 : }
|
