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

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