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
 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\npos:%s, %s" % (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 path %s by edge '%s' (d=%s)" %
154                              ([e.getID() for e in path], edge.getID(), d))
155                        print("              lastBase: %.2f, base: %.2f, advance: %.2f, old dist: %.2f, minDist: %.2f" %
156                              (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                                extension = (edge,)
185                            else:
186                                pathCost = cost
187                                baseDiff = advance - pathCost
188                                extension = extension[1:]
189                                pathLength = sum([e.getLength() for e in extension[:-1]]) - lastBase + base
190                            if debug:
191                                print("---------- extension path: %s, cost: %.2f, pathCost: %.2f" %
192                                      (" ".join([e.getID() for e in extension]),
193                                          cost, pathCost))
194                        dist += d ** distPenalty + pathCost
195                        if direction:
196                            dist += baseDiff * baseDiff
197                        if dist < minDist:
198                            minDist = dist
199                            minPath = path + extension
200                            minDetours = detours
201                            bestLength = pathLength
202                        if debug:
203                            print("*** new dist: %.2f baseDiff: %.2f minDist: %.2f" % (dist, baseDiff, minDist))
204                if minPath:
205                    newPaths[minPath] = (minDist, base, minDetours + [bestLength / advance if advance > 0 else 0])
206            else:
207                #  the penality for picking a departure edge that is further away from pos
208                #  must outweigh the distance that is saved by picking an edge
209                #  that is closer to the subsequent pos
210                if debug:
211                    print("*** origin %s d=%s base=%s" % (edge.getID(), d, base))
212                newPaths[(edge,)] = (d * 2, base, [0])
213        if not newPaths:
214            # no mapping for the current pos, the route may be disconnected or the radius is too small
215            if paths:
216                minPath = _getMinPath(paths, resultDetours)
217                if len(result) > 0 and minPath[0] in result:
218                    cropIndex = max([i for i in range(len(minPath)) if minPath[i] in result])
219                    minPath = minPath[cropIndex+1:]
220                result += minPath
221            resultDetours.append(0)
222        paths = newPaths
223        lastPos = pos
224    if verbose:
225        if nNoCandidates > 0:
226            print("%s Points had no candidates." % nNoCandidates, end="")
227        print(" (%s router calls)" % nPathCalls)
228    if paths:
229        result += _getMinPath(paths, resultDetours)
230        if debug:
231            print("**************** paths:")
232            for edges, (cost, base, _) in paths.items():
233                print(cost, base, " ".join([e.getID() for e in edges]))
234            print("**************** result:")
235            for i in result:
236                print("path:%s" % i.getID())
237    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\npos:%s, %s" % (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 path %s by edge '%s' (d=%s)" %
155                              ([e.getID() for e in path], edge.getID(), d))
156                        print("              lastBase: %.2f, base: %.2f, advance: %.2f, old dist: %.2f, minDist: %.2f" %
157                              (lastBase, base, advance, dist, minDist))
158                    if dist < minDist:
159                        if edge == path[-1] and base > lastBase:
160                            pathLength = base - lastBase
161                            pathCost = pathLength
162                            if fastest:
163                                pathCost /= edge.getSpeed()
164                            baseDiff = advance - pathCost
165                            extension = ()
166                            if debug:
167                                print("---------- same edge")
168                        else:
169                            penalty = airDistFactor * advance if gapPenalty < 0 else gapPenalty
170                            maxGap = min(penalty + edge.getLength() + path[-1].getLength(), fillGaps)
171                            extension, cost = net.getOptimalPath(path[-1], edge, maxCost=maxGap,
172                                                                 fastest=fastest,
173                                                                 reversalPenalty=reversalPenalty,
174                                                                 fromPos=lastBase, toPos=base, vClass=vClass,
175                                                                 preferences=preferences)
176                            nPathCalls += 1
177                            if extension is None:
178                                airLineDist = euclidean(
179                                    path[-1].getToNode().getCoord(),
180                                    edge.getFromNode().getCoord())
181                                pathCost = path[-1].getLength() - lastBase + base + airLineDist + penalty
182                                pathLength = pathCost
183                                baseDiff = abs(lastBase + advance -
184                                               path[-1].getLength() - base - airLineDist) + penalty
185                                extension = (edge,)
186                            else:
187                                pathCost = cost
188                                baseDiff = advance - pathCost
189                                extension = extension[1:]
190                                pathLength = sum([e.getLength() for e in extension[:-1]]) - lastBase + base
191                            if debug:
192                                print("---------- extension path: %s, cost: %.2f, pathCost: %.2f" %
193                                      (" ".join([e.getID() for e in extension]),
194                                          cost, pathCost))
195                        dist += d ** distPenalty + pathCost
196                        if direction:
197                            dist += baseDiff * baseDiff
198                        if dist < minDist:
199                            minDist = dist
200                            minPath = path + extension
201                            minDetours = detours
202                            bestLength = pathLength
203                        if debug:
204                            print("*** new dist: %.2f baseDiff: %.2f minDist: %.2f" % (dist, baseDiff, minDist))
205                if minPath:
206                    newPaths[minPath] = (minDist, base, minDetours + [bestLength / advance if advance > 0 else 0])
207            else:
208                #  the penality for picking a departure edge that is further away from pos
209                #  must outweigh the distance that is saved by picking an edge
210                #  that is closer to the subsequent pos
211                if debug:
212                    print("*** origin %s d=%s base=%s" % (edge.getID(), d, base))
213                newPaths[(edge,)] = (d * 2, base, [0])
214        if not newPaths:
215            # no mapping for the current pos, the route may be disconnected or the radius is too small
216            if paths:
217                minPath = _getMinPath(paths, resultDetours)
218                if len(result) > 0 and minPath[0] in result:
219                    cropIndex = max([i for i in range(len(minPath)) if minPath[i] in result])
220                    minPath = minPath[cropIndex+1:]
221                result += minPath
222            resultDetours.append(0)
223        paths = newPaths
224        lastPos = pos
225    if verbose:
226        if nNoCandidates > 0:
227            print("%s Points had no candidates." % nNoCandidates, end="")
228        print(" (%s router calls)" % nPathCalls)
229    if paths:
230        result += _getMinPath(paths, resultDetours)
231        if debug:
232            print("**************** paths:")
233            for edges, (cost, base, _) in paths.items():
234                print(cost, base, " ".join([e.getID() for e in edges]))
235            print("**************** result:")
236            for i in result:
237                print("path:%s" % i.getID())
238    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.