sumolib.route

View Source

  1# Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
  2# Copyright (C) 2009-2026 German Aerospace Center (DLR) and others.
  3# This program and the accompanying materials are made available under the
  4# terms of the Eclipse Public License 2.0 which is available at
  5# https://www.eclipse.org/legal/epl-2.0/
  6# This Source Code may also be made available under the following Secondary
  7# Licenses when the conditions for such availability set forth in the Eclipse
  8# Public License 2.0 are satisfied: GNU General Public License, version 2
  9# or later which is available at
 10# https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
 11# SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later
 12
 13# @file    route.py
 14# @author  Michael Behrisch
 15# @author  Mirko Barthauer
 16# @date    2013-10-23
 17
 18from __future__ import print_function
 19from .miscutils import euclidean, PRACTIVAL_INFINITY
 20from .geomhelper import polygonOffsetWithMinimumDistanceToPoint, positionAtShapeOffset
 21
 22try:
 23    basestring
 24    # Allows isinstance(foo, basestring) to work in Python 3
 25except NameError:
 26    basestring = str
 27
 28
 29def getLength(net, edges):
 30    """
 31    Calculates the length of a route including internal edges.
 32    The input network has to contain internal edges (withInternal needs to be set when parsing).
 33    The list of edges can either contain edge objects or edge ids as strings.
 34    If there is no connection between two consecutive edges, length 0 is assumed (no error is thrown).
 35    If there are multiple connections of different length, the shortest is used.
 36    """
 37    if len(edges) == 0:
 38        return 0
 39    if isinstance(edges[0], basestring):
 40        edges = [net.getEdge(e) for e in edges]
 41    last = edges[0]
 42    length = last.getLength()
 43    for e in edges[1:]:
 44        if net.hasInternal:
 45            viaPath, minInternalCost = net.getInternalPath(last.getConnections(e))
 46            if viaPath is not None:
 47                length += minInternalCost
 48        length += e.getLength()
 49        last = e
 50    return length
 51
 52
 53def addInternal(net, edges):
 54    """
 55    Returns a list of edges of a route including internal edges.
 56    The input network has to contain internal edges (withInternal needs to be set when parsing).
 57    The list of input edges can either contain edge objects or edge ids as strings.
 58    The return value will always contain edge objects.
 59    If there is no connection between two consecutive edges no internal edge is added.
 60    If there are multiple connections between two edges, the shortest one is used.
 61    """
 62    if len(edges) == 0:
 63        return []
 64    if isinstance(edges[0], basestring):
 65        edges = [net.getEdge(e) for e in edges]
 66    last = edges[0]
 67    result = [last]
 68    for e in edges[1:]:
 69        if net.hasInternal:
 70            viaPath, _ = net.getInternalPath(last.getConnections(e))
 71            if viaPath is not None:
 72                result += viaPath
 73        result.append(e)
 74        last = e
 75    return result
 76
 77
 78def _getMinPath(paths, detoursOut=None):
 79    minDist = 1e400
 80    minPath = None
 81    minDetours = None
 82    for path, (dist, _, detours) in paths.items():
 83        if dist < minDist:
 84            minPath = path
 85            minDist = dist
 86            minDetours = detours
 87    if detoursOut is not None:
 88        detoursOut += minDetours
 89    return minPath
 90
 91
 92def mapTrace(trace, net, delta, verbose=False, airDistFactor=2, fillGaps=0, gapPenalty=-1,
 93             debug=False, direction=False, vClass=None, vias=None, reversalPenalty=0.,
 94             fastest=False, resultDetours=None, preferences={}, distPenalty=2):
 95    """
 96    matching a list of 2D positions to consecutive edges in a network.
 97    The positions are assumed to be dense (i.e. covering each edge of the route) and in the correct order.
 98    """
 99    result = ()
100    if resultDetours is None:
101        resultDetours = []
102    paths = {}  # maps a path stub to a tuple (currentCost, posOnLastEdge, detours)
103    lastPos = None
104    nPathCalls = 0
105    nNoCandidates = 0
106    if verbose:
107        print("mapping trace with %s points ..." % len(trace), end="", flush=True)
108    for idx, pos in enumerate(trace):
109        x, y = pos
110        newPaths = {}
111        if vias and idx in vias:
112            candidates = []
113            for edgeID in vias[idx]:
114                if net.hasEdge(edgeID):
115                    edge = net.getEdge(edgeID)
116                    offset = polygonOffsetWithMinimumDistanceToPoint(pos, edge.getShape())
117                    offsetPos = positionAtShapeOffset(edge.getShape(), offset)
118                    candidates.append((net.getEdge(edgeID), euclidean(pos, offsetPos)))
119                else:
120                    print("Unknown via edge %s for %s,%s" % (edgeID, x, y))
121            if candidates:
122                # normalize distances depending on the minimum value in the candidate set
123                minLocError = min([d for e, d in candidates])
124                candidates = [(e, d - minLocError) for e, d in candidates]
125
126            # print("idx %s: vias=%s, candidates=%s (%s)" % (idx, len(vias[idx]),
127            #    len(candidates), [ed[0].getID() for ed in candidates]))
128        else:
129            candidates = net.getNeighboringEdges(x, y, delta, False)
130        if debug:
131            print("\n\nindex: %s pos:%s, %s" % (idx, x, y))
132            print("candidates:%s\n" % [(e.getID(), c) for e, c in candidates])
133        if verbose and not candidates:
134            if nNoCandidates == 0:
135                print()
136            print("   Found no candidate edges for %.2f,%.2f (index %s)" % (x, y, idx))
137            nNoCandidates += 1
138
139        for edge, d in candidates:
140            if vClass is not None and not edge.allows(vClass):
141                continue
142            base = polygonOffsetWithMinimumDistanceToPoint(pos, edge.getShape())
143            base *= edge.getLengthGeometryFactor()
144            if paths:
145                advance = euclidean(lastPos, pos)  # should become a vector
146                bestLength = 1e400  # length of the best path (not necessarily the shortest)
147                minDist = 1e400
148                minPath = None
149                minDetours = None
150                for path, (dist, lastBase, detours) in paths.items():
151                    pathLength = None
152                    if debug:
153                        print("*** extending prev '%s' path: %s" % (path[-1].getID(), " ".join([e.getID() for e in path])))
154                        print("           by edge '%s' (d=%s) lastBase: %.2f, base: %.2f, advance: %.2f, old dist: %.2f, minDist: %.2f" %
155                              (edge.getID(), d, lastBase, base, advance, dist, minDist))
156                    if dist < minDist:
157                        if edge == path[-1] and base > lastBase:
158                            pathLength = base - lastBase
159                            pathCost = pathLength
160                            if fastest:
161                                pathCost /= edge.getSpeed()
162                            baseDiff = advance - pathCost
163                            extension = ()
164                            if debug:
165                                print("---------- same edge")
166                        else:
167                            penalty = airDistFactor * advance if gapPenalty < 0 else gapPenalty
168                            maxGap = min(penalty + edge.getLength() + path[-1].getLength(), fillGaps)
169                            extension, cost = net.getOptimalPath(path[-1], edge, maxCost=maxGap,
170                                                                 fastest=fastest,
171                                                                 reversalPenalty=reversalPenalty,
172                                                                 fromPos=lastBase, toPos=base, vClass=vClass,
173                                                                 preferences=preferences)
174                            nPathCalls += 1
175                            if extension is None:
176                                airLineDist = euclidean(
177                                    path[-1].getToNode().getCoord(),
178                                    edge.getFromNode().getCoord())
179                                pathCost = path[-1].getLength() - lastBase + base + airLineDist + penalty
180                                pathLength = pathCost
181                                baseDiff = abs(lastBase + advance -
182                                               path[-1].getLength() - base - airLineDist) + penalty
183                                if cost > maxGap and maxGap > 0:
184                                    pathCost = PRACTIVAL_INFINITY
185                                    extension = ()
186                                else:
187                                    extension = (edge,)
188                            else:
189                                pathCost = cost
190                                baseDiff = advance - pathCost
191                                extension = extension[1:]
192                                pathLength = sum([e.getLength() for e in extension[:-1]]) - lastBase + base
193                            if debug:
194                                print("---------- extension cost: %.2f, pathCost: %.2f, pathLength: %.2f n: %s edges: %s" %
195                                      (cost, pathCost, pathLength, len(extension),
196                                       " ".join([e.getID() for e in extension])))
197                        dist += d ** distPenalty + pathCost
198                        if direction:
199                            dist += baseDiff * baseDiff
200                        if dist < minDist:
201                            minDist = dist
202                            minPath = path + extension
203                            minDetours = detours
204                            bestLength = pathLength
205                            if debug:
206                                print("*** new dist: %.2f baseDiff: %.2f minDist: %.2f advance: %.2f pathLength: %.2f detour: %.2f" % (
207                                    dist, baseDiff, minDist, advance, bestLength, bestLength / advance))
208                if minPath:
209                    newPaths[minPath] = (minDist, base, minDetours + [bestLength / advance if advance > 0 else 0])
210            else:
211                #  the penality for picking a departure edge that is further away from pos
212                #  must outweigh the distance that is saved by picking an edge
213                #  that is closer to the subsequent pos
214                if debug:
215                    print("*** origin %s d=%s base=%s" % (edge.getID(), d, base))
216                newPaths[(edge,)] = (d * 2, base, [0])
217        if not newPaths:
218            if debug:
219                print("*** no newPaths ***")
220            # no mapping for the current pos, the route may be disconnected or the radius is too small
221            if paths:
222                minPath = _getMinPath(paths, resultDetours)
223                if len(result) > 0 and minPath[0] in result:
224                    cropIndex = max([i for i in range(len(minPath)) if minPath[i] in result])
225                    minPath = minPath[cropIndex+1:]
226                result += minPath
227                # signal disconnect
228                resultDetours.append(PRACTIVAL_INFINITY)
229            else:
230                resultDetours.append(-1)
231        paths = newPaths
232        lastPos = pos
233    if verbose:
234        if nNoCandidates > 0:
235            print("%s Points had no candidates." % nNoCandidates, end="")
236        print(" (%s router calls)" % nPathCalls)
237    if paths:
238        result += _getMinPath(paths, resultDetours)
239        if debug:
240            print("**************** paths:")
241            for edges, (cost, base, _) in paths.items():
242                print(cost, base, " ".join([e.getID() for e in edges]))
243            print("**************** result:")
244            for i in result:
245                print("path:%s" % i.getID())
246    # remove disconnect info if no positions where mapped after the first unmapped
247    if PRACTIVAL_INFINITY in resultDetours:
248        for i, d in enumerate(resultDetours):
249            if d == PRACTIVAL_INFINITY:
250                if all([d2 == -1 for d2 in resultDetours[i + 1:]]):
251                    resultDetours[i] = -1
252    return result

def getLength(net, edges): View Source

30def getLength(net, edges):
31    """
32    Calculates the length of a route including internal edges.
33    The input network has to contain internal edges (withInternal needs to be set when parsing).
34    The list of edges can either contain edge objects or edge ids as strings.
35    If there is no connection between two consecutive edges, length 0 is assumed (no error is thrown).
36    If there are multiple connections of different length, the shortest is used.
37    """
38    if len(edges) == 0:
39        return 0
40    if isinstance(edges[0], basestring):
41        edges = [net.getEdge(e) for e in edges]
42    last = edges[0]
43    length = last.getLength()
44    for e in edges[1:]:
45        if net.hasInternal:
46            viaPath, minInternalCost = net.getInternalPath(last.getConnections(e))
47            if viaPath is not None:
48                length += minInternalCost
49        length += e.getLength()
50        last = e
51    return length

Calculates the length of a route including internal edges. The input network has to contain internal edges (withInternal needs to be set when parsing). The list of edges can either contain edge objects or edge ids as strings. If there is no connection between two consecutive edges, length 0 is assumed (no error is thrown). If there are multiple connections of different length, the shortest is used.

def addInternal(net, edges): View Source

54def addInternal(net, edges):
55    """
56    Returns a list of edges of a route including internal edges.
57    The input network has to contain internal edges (withInternal needs to be set when parsing).
58    The list of input edges can either contain edge objects or edge ids as strings.
59    The return value will always contain edge objects.
60    If there is no connection between two consecutive edges no internal edge is added.
61    If there are multiple connections between two edges, the shortest one is used.
62    """
63    if len(edges) == 0:
64        return []
65    if isinstance(edges[0], basestring):
66        edges = [net.getEdge(e) for e in edges]
67    last = edges[0]
68    result = [last]
69    for e in edges[1:]:
70        if net.hasInternal:
71            viaPath, _ = net.getInternalPath(last.getConnections(e))
72            if viaPath is not None:
73                result += viaPath
74        result.append(e)
75        last = e
76    return result

Returns a list of edges of a route including internal edges. The input network has to contain internal edges (withInternal needs to be set when parsing). The list of input edges can either contain edge objects or edge ids as strings. The return value will always contain edge objects. If there is no connection between two consecutive edges no internal edge is added. If there are multiple connections between two edges, the shortest one is used.

def mapTrace( trace, net, delta, verbose=False, airDistFactor=2, fillGaps=0, gapPenalty=-1, debug=False, direction=False, vClass=None, vias=None, reversalPenalty=0.0, fastest=False, resultDetours=None, preferences={}, distPenalty=2): View Source

 93def mapTrace(trace, net, delta, verbose=False, airDistFactor=2, fillGaps=0, gapPenalty=-1,
 94             debug=False, direction=False, vClass=None, vias=None, reversalPenalty=0.,
 95             fastest=False, resultDetours=None, preferences={}, distPenalty=2):
 96    """
 97    matching a list of 2D positions to consecutive edges in a network.
 98    The positions are assumed to be dense (i.e. covering each edge of the route) and in the correct order.
 99    """
100    result = ()
101    if resultDetours is None:
102        resultDetours = []
103    paths = {}  # maps a path stub to a tuple (currentCost, posOnLastEdge, detours)
104    lastPos = None
105    nPathCalls = 0
106    nNoCandidates = 0
107    if verbose:
108        print("mapping trace with %s points ..." % len(trace), end="", flush=True)
109    for idx, pos in enumerate(trace):
110        x, y = pos
111        newPaths = {}
112        if vias and idx in vias:
113            candidates = []
114            for edgeID in vias[idx]:
115                if net.hasEdge(edgeID):
116                    edge = net.getEdge(edgeID)
117                    offset = polygonOffsetWithMinimumDistanceToPoint(pos, edge.getShape())
118                    offsetPos = positionAtShapeOffset(edge.getShape(), offset)
119                    candidates.append((net.getEdge(edgeID), euclidean(pos, offsetPos)))
120                else:
121                    print("Unknown via edge %s for %s,%s" % (edgeID, x, y))
122            if candidates:
123                # normalize distances depending on the minimum value in the candidate set
124                minLocError = min([d for e, d in candidates])
125                candidates = [(e, d - minLocError) for e, d in candidates]
126
127            # print("idx %s: vias=%s, candidates=%s (%s)" % (idx, len(vias[idx]),
128            #    len(candidates), [ed[0].getID() for ed in candidates]))
129        else:
130            candidates = net.getNeighboringEdges(x, y, delta, False)
131        if debug:
132            print("\n\nindex: %s pos:%s, %s" % (idx, x, y))
133            print("candidates:%s\n" % [(e.getID(), c) for e, c in candidates])
134        if verbose and not candidates:
135            if nNoCandidates == 0:
136                print()
137            print("   Found no candidate edges for %.2f,%.2f (index %s)" % (x, y, idx))
138            nNoCandidates += 1
139
140        for edge, d in candidates:
141            if vClass is not None and not edge.allows(vClass):
142                continue
143            base = polygonOffsetWithMinimumDistanceToPoint(pos, edge.getShape())
144            base *= edge.getLengthGeometryFactor()
145            if paths:
146                advance = euclidean(lastPos, pos)  # should become a vector
147                bestLength = 1e400  # length of the best path (not necessarily the shortest)
148                minDist = 1e400
149                minPath = None
150                minDetours = None
151                for path, (dist, lastBase, detours) in paths.items():
152                    pathLength = None
153                    if debug:
154                        print("*** extending prev '%s' path: %s" % (path[-1].getID(), " ".join([e.getID() for e in path])))
155                        print("           by edge '%s' (d=%s) lastBase: %.2f, base: %.2f, advance: %.2f, old dist: %.2f, minDist: %.2f" %
156                              (edge.getID(), d, lastBase, base, advance, dist, minDist))
157                    if dist < minDist:
158                        if edge == path[-1] and base > lastBase:
159                            pathLength = base - lastBase
160                            pathCost = pathLength
161                            if fastest:
162                                pathCost /= edge.getSpeed()
163                            baseDiff = advance - pathCost
164                            extension = ()
165                            if debug:
166                                print("---------- same edge")
167                        else:
168                            penalty = airDistFactor * advance if gapPenalty < 0 else gapPenalty
169                            maxGap = min(penalty + edge.getLength() + path[-1].getLength(), fillGaps)
170                            extension, cost = net.getOptimalPath(path[-1], edge, maxCost=maxGap,
171                                                                 fastest=fastest,
172                                                                 reversalPenalty=reversalPenalty,
173                                                                 fromPos=lastBase, toPos=base, vClass=vClass,
174                                                                 preferences=preferences)
175                            nPathCalls += 1
176                            if extension is None:
177                                airLineDist = euclidean(
178                                    path[-1].getToNode().getCoord(),
179                                    edge.getFromNode().getCoord())
180                                pathCost = path[-1].getLength() - lastBase + base + airLineDist + penalty
181                                pathLength = pathCost
182                                baseDiff = abs(lastBase + advance -
183                                               path[-1].getLength() - base - airLineDist) + penalty
184                                if cost > maxGap and maxGap > 0:
185                                    pathCost = PRACTIVAL_INFINITY
186                                    extension = ()
187                                else:
188                                    extension = (edge,)
189                            else:
190                                pathCost = cost
191                                baseDiff = advance - pathCost
192                                extension = extension[1:]
193                                pathLength = sum([e.getLength() for e in extension[:-1]]) - lastBase + base
194                            if debug:
195                                print("---------- extension cost: %.2f, pathCost: %.2f, pathLength: %.2f n: %s edges: %s" %
196                                      (cost, pathCost, pathLength, len(extension),
197                                       " ".join([e.getID() for e in extension])))
198                        dist += d ** distPenalty + pathCost
199                        if direction:
200                            dist += baseDiff * baseDiff
201                        if dist < minDist:
202                            minDist = dist
203                            minPath = path + extension
204                            minDetours = detours
205                            bestLength = pathLength
206                            if debug:
207                                print("*** new dist: %.2f baseDiff: %.2f minDist: %.2f advance: %.2f pathLength: %.2f detour: %.2f" % (
208                                    dist, baseDiff, minDist, advance, bestLength, bestLength / advance))
209                if minPath:
210                    newPaths[minPath] = (minDist, base, minDetours + [bestLength / advance if advance > 0 else 0])
211            else:
212                #  the penality for picking a departure edge that is further away from pos
213                #  must outweigh the distance that is saved by picking an edge
214                #  that is closer to the subsequent pos
215                if debug:
216                    print("*** origin %s d=%s base=%s" % (edge.getID(), d, base))
217                newPaths[(edge,)] = (d * 2, base, [0])
218        if not newPaths:
219            if debug:
220                print("*** no newPaths ***")
221            # no mapping for the current pos, the route may be disconnected or the radius is too small
222            if paths:
223                minPath = _getMinPath(paths, resultDetours)
224                if len(result) > 0 and minPath[0] in result:
225                    cropIndex = max([i for i in range(len(minPath)) if minPath[i] in result])
226                    minPath = minPath[cropIndex+1:]
227                result += minPath
228                # signal disconnect
229                resultDetours.append(PRACTIVAL_INFINITY)
230            else:
231                resultDetours.append(-1)
232        paths = newPaths
233        lastPos = pos
234    if verbose:
235        if nNoCandidates > 0:
236            print("%s Points had no candidates." % nNoCandidates, end="")
237        print(" (%s router calls)" % nPathCalls)
238    if paths:
239        result += _getMinPath(paths, resultDetours)
240        if debug:
241            print("**************** paths:")
242            for edges, (cost, base, _) in paths.items():
243                print(cost, base, " ".join([e.getID() for e in edges]))
244            print("**************** result:")
245            for i in result:
246                print("path:%s" % i.getID())
247    # remove disconnect info if no positions where mapped after the first unmapped
248    if PRACTIVAL_INFINITY in resultDetours:
249        for i, d in enumerate(resultDetours):
250            if d == PRACTIVAL_INFINITY:
251                if all([d2 == -1 for d2 in resultDetours[i + 1:]]):
252                    resultDetours[i] = -1
253    return result

matching a list of 2D positions to consecutive edges in a network. The positions are assumed to be dense (i.e. covering each edge of the route) and in the correct order.