"""Library for representing road network geometry and traffic information.
A road network is represented by an instance of the :obj:`Network` class, which can
be created from a map file using :obj:`Network.fromFile`.
.. note::
This library is a prototype under active development. We will try not to make
backwards-incompatible changes, but the API may not be entirely stable.
"""
from __future__ import annotations # allow forward references for type annotations
import enum
import gzip
import hashlib
import io
import itertools
import math
import numbers
import pathlib
import pickle
import struct
import time
from typing import FrozenSet, List, Optional, Sequence, Tuple, Union
import weakref
import attr
import shapely
from shapely.geometry import MultiPolygon, Polygon
from scenic.core.distributions import (
RejectionException,
distributionFunction,
distributionMethod,
)
from scenic.core.errors import InvalidScenarioError
import scenic.core.geometry as geometry
from scenic.core.object_types import Point
from scenic.core.regions import PolygonalRegion, PolylineRegion
import scenic.core.type_support as type_support
import scenic.core.utils as utils
from scenic.core.vectors import Orientation, Vector, VectorField
import scenic.syntax.veneer as veneer
from scenic.syntax.veneer import verbosePrint
## Typing and utilities
#: Alias for types which can be interpreted as positions in Scenic.
#:
#: This includes instances of `Point` and `Object`, and pairs of numbers.
Vectorlike = Union[Vector, Point, Tuple[numbers.Real, numbers.Real]]
def _toVector(thing: Vectorlike) -> Vector:
return type_support.toVector(thing)
def _rejectSample(message):
if veneer.isActive():
raise InvalidScenarioError(message)
else:
raise RejectionException(message)
def _rejectIfNonexistent(element, name="network element"):
if element is None:
_rejectSample(f"requested {name} does not exist")
return element
class _ElementReferencer:
"""Mixin class to improve pickling of classes that reference network elements.
:meta private:
"""
def __getstate__(self):
if hasattr(super(), "__getstate__"):
state = super().__getstate__()
if state is self.__dict__:
state = state.copy()
else:
state = self.__dict__.copy()
# replace links to network elements by placeholders to prevent deep
# recursions during pickling; as a result of this, only entire `Network`
# objects can be properly unpickled
for key, value in state.items():
if isinstance(value, NetworkElement):
state[key] = _ElementPlaceholder(value.uid)
return state
class _ElementPlaceholder:
"""Placeholder for a link to a pickled `NetworkElement`.
:meta private:
"""
def __init__(self, uid):
self.uid = uid
## Metadata
[docs]@enum.unique
class VehicleType(enum.Enum):
"""A type of vehicle, including pedestrians. Used to classify lanes."""
CAR = 1
BICYCLE = 2
PEDESTRIAN = 3
[docs]@enum.unique
class ManeuverType(enum.Enum):
"""A type of `Maneuver`, e.g., going straight or turning left."""
STRAIGHT = enum.auto() #: Straight, including one lane merging into another.
LEFT_TURN = enum.auto() #: Left turn.
RIGHT_TURN = enum.auto() #: Right turn.
U_TURN = enum.auto() #: U-turn.
[docs] @staticmethod
def guessTypeFromLanes(
start: Lane,
end: Lane,
connecting: Union[Lane, None],
turnThreshold: float = math.radians(20),
):
"""For formats lacking turn information, guess it from the geometry.
Arguments:
start: starting lane of the maneuver.
end: ending lane of the maneuver.
connecting: connecting lane of the maneuver, if any.
turnThreshold: angle beyond which to consider a maneuver a turn.
"""
if connecting is None:
return ManeuverType.STRAIGHT
if end.road is start.road:
return ManeuverType.U_TURN
# Identify turns based on relative heading of start and end of connecting lane
startDir = connecting.centerline[1] - connecting.centerline[0]
endDir = connecting.centerline[-1] - connecting.centerline[-2]
turnAngle = startDir.angleWith(endDir)
if turnAngle >= turnThreshold:
return ManeuverType.LEFT_TURN
elif turnAngle <= -turnThreshold:
return ManeuverType.RIGHT_TURN
else:
return ManeuverType.STRAIGHT
[docs]@attr.s(auto_attribs=True, kw_only=True, eq=False)
class Maneuver(_ElementReferencer):
"""Maneuver()
A maneuver which can be taken upon reaching the end of a lane.
"""
type: ManeuverType = None #: type of maneuver (straight, left turn, etc.)
startLane: Lane #: starting lane of the maneuver
endLane: Lane #: ending lane of the maneuver
# the following attributes are None if startLane directly merges into endLane,
# rather than connecting via a maneuver through an intersection
#: connecting lane from the start to the end lane, if any (`None` for lane mergers)
connectingLane: Union[Lane, None] = None
#: intersection where the maneuver takes place, if any (`None` for lane mergers)
intersection: Union[Intersection, None] = None
def __attrs_post_init__(self):
assert self.type is ManeuverType.STRAIGHT or self.connectingLane is not None
if self.type is None: # unknown maneuver type; need to guess from geometry
ty = ManeuverType.guessTypeFromLanes(
self.startLane, self.endLane, self.connectingLane
)
object.__setattr__(self, "type", ty)
@property
@utils.cached
def conflictingManeuvers(self) -> Tuple[Maneuver]:
"""Maneuvers whose connecting lanes intersect this one's."""
if not self.connectingLane:
return ()
guideway = self.connectingLane
start = self.startLane
conflicts = []
for maneuver in self.intersection.maneuvers:
if (
maneuver.startLane is not start
and maneuver.connectingLane.centerline.intersects(guideway.centerline)
):
conflicts.append(maneuver)
return tuple(conflicts)
@property
@utils.cached
def reverseManeuvers(self) -> Tuple[Maneuver]:
"""Maneuvers whose start and end roads are the reverse of this one's."""
start = self.startLane.road
end = self.endLane.road
reverses = []
for maneuver in self.intersection.maneuvers:
if maneuver.startLane.road is end and maneuver.endLane.road is start:
reverses.append(maneuver)
return tuple(reverses)
## Road networks
[docs]@attr.s(auto_attribs=True, kw_only=True, repr=False, eq=False)
class NetworkElement(_ElementReferencer, PolygonalRegion):
"""NetworkElement()
Abstract class for part of a road network.
Includes roads, lane groups, lanes, sidewalks, pedestrian crossings,
and intersections.
This is a subclass of `Region`, so you can do things like ``Car in lane``
or ``Car on road`` if ``lane`` and ``road`` are elements, as well as computing
distances to an element, etc.
"""
# from PolygonalRegion
polygon: Union[Polygon, MultiPolygon]
orientation: Optional[VectorField] = None
name: str = "" #: Human-readable name, if any.
#: Unique identifier; from underlying format, if possible.
#: (In OpenDRIVE, for example, ids are not necessarily unique, so we invent our own.)
uid: str = None
id: Optional[str] = None #: Identifier from underlying format, if any.
network: Network = None #: Link to parent network.
## Traffic info
#: Which types of vehicles (car, bicycle, etc.) can be here.
vehicleTypes: FrozenSet[VehicleType] = frozenset([VehicleType.CAR])
#: Optional speed limit, which may be inherited from parent.
speedLimit: Union[float, None] = None
#: Uninterpreted semantic tags, e.g. 'roundabout'.
tags: FrozenSet[str] = frozenset()
def __attrs_post_init__(self):
assert self.uid is not None or self.id is not None
if self.uid is None:
self.uid = self.id
super().__init__(
polygon=self.polygon, orientation=self.orientation, name=self.name
)
[docs] @distributionFunction
def nominalDirectionsAt(self, point: Vectorlike) -> Tuple[Orientation]:
"""Get nominal traffic direction(s) at a point in this element.
There must be at least one such direction. If there are multiple, we
pick one arbitrarily to be the orientation of the element as a `Region`.
(So ``Object in element`` will align by default to that orientation.)
"""
assert self.orientation, self
return (self.orientation[_toVector(point)],)
def __getstate__(self):
state = super().__getstate__()
del state["network"] # do not pickle weak reference to parent network
return state
def __eq__(self, other):
if not isinstance(other, NetworkElement):
return NotImplemented
return self.network is other.network and self.uid == other.uid
def __hash__(self):
return hash((self.network.__hash__(), self.uid))
def __repr__(self):
s = f"<{type(self).__name__} at {hex(id(self))}; "
if self.name:
s += f'name="{self.name}", '
if self.id and self.id != self.uid:
s += f'id="{self.id}", '
s += f'uid="{self.uid}">'
return s
[docs]@attr.s(auto_attribs=True, kw_only=True, repr=False, eq=False)
class LinearElement(NetworkElement):
"""LinearElement()
A part of a road network with (mostly) linear 1- or 2-way flow.
Includes roads, lane groups, lanes, sidewalks, and pedestrian crossings,
but not intersections.
LinearElements have a direction, namely from the first point on their centerline
to the last point. This is called 'forward', even for 2-way roads. The 'left' and
'right' edges are interpreted with respect to this direction.
The left/right edges are oriented along the direction of traffic near them; so
for 2-way roads they will point opposite directions.
"""
# Geometric info (on top of the overall polygon from PolygonalRegion)
centerline: PolylineRegion
leftEdge: PolylineRegion
rightEdge: PolylineRegion
# Links to next/previous element
_successor: Union[NetworkElement, None] = None # going forward
_predecessor: Union[NetworkElement, None] = None # going backward
@property
def successor(self):
return _rejectIfNonexistent(self._successor, "successor")
@property
def predecessor(self):
return _rejectIfNonexistent(self._predecessor, "predecessor")
def __attrs_post_init__(self):
super().__attrs_post_init__()
# Check that left and right edges lie inside the element.
# (don't check centerline here since it can lie inside a median, for example)
# (TODO reconsider the decision to have polygon only include drivable areas?)
assert self.containsRegion(self.leftEdge, tolerance=0.5)
assert self.containsRegion(self.rightEdge, tolerance=0.5)
if self.orientation is None:
self.orientation = VectorField(self.name, self._defaultHeadingAt)
def _defaultHeadingAt(self, point):
"""Default orientation for this LinearElement.
In general, we align along the nearest segment of the centerline.
For roads, lane groups, etc., we align along the orientation of the
lane containing the point.
:meta private:
"""
point = _toVector(point)
start, end = self.centerline.nearestSegmentTo(point)
return start.angleTo(end)
[docs] @distributionFunction
def flowFrom(
self,
point: Vectorlike,
distance: float,
steps: Union[int, None] = None,
stepSize: float = 5,
) -> Vector:
"""Advance a point along this element by a given distance.
Equivalent to ``follow element.orientation from point for distance``, but
possibly more accurate. The default implementation uses the forward
Euler approximation with a step size of 5 meters; subclasses may ignore
the **steps** and **stepSize** parameters if they can compute the flow
exactly.
Arguments:
point: point to start from.
distance: distance to travel.
steps: number of steps to take, or :obj:`None` to compute the
number of steps based on the distance (default :obj:`None`).
stepSize: length used to compute how many steps to take, if **steps** is not
specified (default 5 meters).
"""
return self.orientation.followFrom(
_toVector(point), distance, steps=steps, stepSize=stepSize
)
class _ContainsCenterline:
"""Mixin which asserts that the centerline is contained in the polygon.
:meta private:
"""
def __attrs_post_init__(self):
super().__attrs_post_init__()
assert self.containsRegion(self.centerline, tolerance=0.5)
[docs]@attr.s(auto_attribs=True, kw_only=True, repr=False, eq=False)
class Road(LinearElement):
"""Road()
A road consisting of one or more lanes.
Lanes are grouped into 1 or 2 instances of `LaneGroup`:
* **forwardLanes**: the lanes going the same direction as the road
* **backwardLanes**: the lanes going the opposite direction
One of these may be None if there are no lanes in that direction.
Because of splits and mergers, the Lanes of a `Road` do not necessarily start
or end at the same point as the `Road`. Such intermediate branching points
cause the `Road` to be partitioned into multiple road sections, within which
the configuration of lanes is fixed.
"""
#: All lanes of this road, in either direction.
#:
#: The order of the lanes is arbitrary. To access lanes in order according to their
#: geometry, use `LaneGroup.lanes`.
lanes: Tuple[Lane]
#: Group of lanes aligned with the direction of the road, if any.
forwardLanes: Union[LaneGroup, None]
#: Group of lanes going in the opposite direction, if any.
backwardLanes: Union[LaneGroup, None] # lanes going the other direction
#: All LaneGroups of this road, with `forwardLanes` being first if it exists.
laneGroups: Tuple[LaneGroup] = None
#: All sections of this road, ordered from start to end.
sections: Tuple[RoadSection]
signals: Tuple[Signal]
#: All crosswalks of this road, ordered from start to end.
crossings: Tuple[PedestrianCrossing] = ()
#: All sidewalks of this road, with the one adjacent to `forwardLanes` being first.
sidewalks: Tuple[Sidewalk] = None
#: Possibly-empty region consisting of all sidewalks of this road.
sidewalkRegion: PolygonalRegion = None
def __attrs_post_init__(self):
super().__attrs_post_init__()
lgs = []
sidewalks = []
if self.forwardLanes:
lgs.append(self.forwardLanes)
if self.forwardLanes._sidewalk:
sidewalks.append(self.forwardLanes._sidewalk)
if self.backwardLanes:
lgs.append(self.backwardLanes)
if self.backwardLanes._sidewalk:
sidewalks.append(self.backwardLanes._sidewalk)
self.laneGroups = tuple(lgs)
self.sidewalks = tuple(sidewalks)
self.sidewalkRegion = PolygonalRegion.unionAll(sidewalks)
def _defaultHeadingAt(self, point):
point = _toVector(point)
group = self.laneGroupAt(point)
if group:
return group.orientation[point]
return super()._defaultHeadingAt(point)
[docs] @distributionFunction
def sectionAt(self, point: Vectorlike, reject=False) -> Union[RoadSection, None]:
"""Get the `RoadSection` passing through a given point."""
return self.network.findPointIn(point, self.sections, reject)
[docs] @distributionFunction
def laneSectionAt(self, point: Vectorlike, reject=False) -> Union[LaneSection, None]:
"""Get the `LaneSection` passing through a given point."""
point = _toVector(point)
lane = self.laneAt(point, reject=reject)
return None if lane is None else lane.sectionAt(point)
[docs] @distributionFunction
def laneAt(self, point: Vectorlike, reject=False) -> Union[Lane, None]:
"""Get the `Lane` passing through a given point."""
return self.network.findPointIn(point, self.lanes, reject)
[docs] @distributionFunction
def laneGroupAt(self, point: Vectorlike, reject=False) -> Union[LaneGroup, None]:
"""Get the `LaneGroup` passing through a given point."""
return self.network.findPointIn(point, self.laneGroups, reject)
[docs] @distributionFunction
def crossingAt(
self, point: Vectorlike, reject=False
) -> Union[PedestrianCrossing, None]:
"""Get the :obj:`.PedestrianCrossing` passing through a given point."""
return self.network.findPointIn(point, self.crossings, reject)
[docs] @distributionFunction
def shiftLanes(self, point: Vectorlike, offset: int) -> Union[Vector, None]:
"""Find the point equivalent to this one but shifted over some # of lanes."""
raise NotImplementedError # TODO implement this
@property
def is1Way(self) -> bool:
return self.forwardLanes is None or self.backwardLanes is None
[docs]@attr.s(auto_attribs=True, kw_only=True, repr=False, eq=False)
class LaneGroup(LinearElement):
"""LaneGroup()
A group of parallel lanes with the same type and direction.
"""
road: Road #: Parent road.
lanes: Tuple[Lane] #: Lanes, partially ordered with lane 0 being closest to the curb.
#: Region representing the associated curb, which is not necessarily adjacent if
#: there are parking lanes or some other kind of shoulder.
curb: PolylineRegion
# associated elements not actually part of this group
_sidewalk: Union[Sidewalk, None] = None #: Adjacent sidewalk, if any.
_bikeLane: Union[Lane, None] = None
_shoulder: Union[Shoulder, None] = None #: Adjacent shoulder, if any.
#: Opposite lane group of the same road, if any.
_opposite: Union[LaneGroup, None] = None
def __attrs_post_init__(self):
super().__attrs_post_init__()
# Ensure lanes do not overlap
for i in range(len(self.lanes) - 1):
assert not self.lanes[i].polygon.overlaps(self.lanes[i + 1].polygon)
@property
def sidewalk(self) -> Sidewalk:
"""The adjacent sidewalk; rejects if there is none."""
return _rejectIfNonexistent(self._sidewalk, "sidewalk")
@property
def bikeLane(self) -> Lane:
return _rejectIfNonexistent(self._bikeLane, "bike lane")
@property
def shoulder(self) -> Shoulder:
"""The adjacent shoulder; rejects if there is none."""
return _rejectIfNonexistent(self._shoulder, "shoulder")
@property
def opposite(self) -> LaneGroup:
"""The opposite lane group of the same road; rejects if there is none."""
return _rejectIfNonexistent(self._opposite, "opposite lane group")
def _defaultHeadingAt(self, point):
point = _toVector(point)
lane = self.laneAt(point)
if lane:
return lane.orientation[point]
return super()._defaultHeadingAt(point)
[docs] @distributionFunction
def laneAt(self, point: Vectorlike, reject=False) -> Union[Lane, None]:
"""Get the `Lane` passing through a given point."""
return self.network.findPointIn(point, self.lanes, reject)
[docs]@attr.s(auto_attribs=True, kw_only=True, repr=False, eq=False)
class Lane(_ContainsCenterline, LinearElement):
"""Lane()
A lane for cars, bicycles, or other vehicles.
"""
group: LaneGroup # parent lane group
road: Road # grandparent road
sections: Tuple[LaneSection] # sections in order from start to end
adjacentLanes: Tuple[Lane] = () # adjacent lanes of same type, if any
# possible maneuvers upon reaching the end of this lane
maneuvers: Tuple[Maneuver] = ()
[docs] @distributionFunction
def sectionAt(self, point: Vectorlike, reject=False) -> Union[LaneSection, None]:
"""Get the LaneSection passing through a given point."""
return self.network.findPointIn(point, self.sections, reject)
[docs]@attr.s(auto_attribs=True, kw_only=True, repr=False, eq=False)
class RoadSection(LinearElement):
"""RoadSection()
Part of a road with a fixed number of lanes.
A RoadSection has a fixed number of lanes: when a lane begins or ends, we
move to a new section (which will be the successor of the current one).
"""
road: Road # parent road
lanes: Tuple[LaneSection] = () # in order, with lane 0 being the rightmost
forwardLanes: Tuple[LaneSection] = () # as above
backwardLanes: Tuple[LaneSection] = () # as above
lanesByOpenDriveID: Dict[LaneSection]
def __attrs_post_init__(self):
super().__attrs_post_init__()
if not self.lanes and not self.lanesByOpenDriveID:
raise RuntimeError("RoadSection created with no lanes")
if self.lanesByOpenDriveID and not self.lanes:
forward, backward = [], []
rightmost = min(self.lanesByOpenDriveID)
assert rightmost != 0, self.lanesByOpenDriveID
leftmost = max(self.lanesByOpenDriveID)
for i in range(rightmost, leftmost + 1):
if i == 0:
continue
if i not in self.lanesByOpenDriveID:
continue
(forward if i < 0 else backward).append(self.lanesByOpenDriveID[i])
self.forwardLanes = tuple(forward)
self.backwardLanes = tuple(backward)
self.lanes = self.forwardLanes + self.backwardLanes
elif self.lanes and not self.lanesByOpenDriveID:
ids = {}
for i, lane in enumerate(self.forwardLanes, start=1):
ids[-i] = lane
for i, lane in enumerate(self.backwardLanes, start=1):
ids[i] = lane
self.lanesByOpenDriveID = ids
# Ensure lanes do not overlap
for i in range(len(self.lanes) - 1):
assert not self.lanes[i].polygon.overlaps(self.lanes[i + 1].polygon)
def _defaultHeadingAt(self, point):
point = _toVector(point)
lane = self.laneAt(point)
if lane:
return lane.orientation[point]
return super()._defaultHeadingAt(point)
[docs] @distributionFunction
def laneAt(self, point: Vectorlike, reject=False) -> Union[LaneSection, None]:
"""Get the lane section passing through a given point."""
return self.network.findPointIn(point, self.lanes, reject)
[docs]@attr.s(auto_attribs=True, kw_only=True, repr=False, eq=False)
class LaneSection(_ContainsCenterline, LinearElement):
"""LaneSection()
Part of a lane in a single `RoadSection`.
Since the lane configuration in a `RoadSection` is fixed, a `LaneSection` can have
at most one adjacent lane to left or right. These are accessible using the
`laneToLeft` and `laneToRight` properties, which for convenience reject the
simulation if the desired lane does not exist. If rejection is not desired (for
example if you want to handle the case where there is no lane to the left yourself),
you can use the `_laneToLeft` and `_laneToRight` properties instead.
"""
lane: Lane #: Parent lane.
group: LaneGroup #: Grandparent lane group.
road: Road #: Great-grandparent road.
#: ID number as in OpenDRIVE (number of lanes to left of center, with 1 being the
# first lane left of the centerline and -1 being the first lane to the right).
openDriveID: int
#: Whether this lane has the same direction as its parent road.
isForward: bool = True
#: Adjacent lanes of the same type, if any.
adjacentLanes: Tuple[LaneSection] = ()
#: Adjacent lane of same type to the left, if any.
_laneToLeft: Union[LaneSection, None] = None
#: Adjacent lane of same type to the right, if any.
_laneToRight: Union[LaneSection, None] = None
#: Faster adjacent lane of same type, if any.
#: Could be to left or right depending on the country.
_fasterLane: Union[LaneSection, None] = None
#: Slower adjacent lane of same type, if any.
_slowerLane: Union[LaneSection, None] = None
@property
def laneToLeft(self) -> LaneSection:
"""The adjacent lane of the same type to the left; rejects if there is none."""
return _rejectIfNonexistent(self._laneToLeft, "lane to left")
@property
def laneToRight(self) -> LaneSection:
"""The adjacent lane of the same type to the right; rejects if there is none."""
return _rejectIfNonexistent(self._laneToRight, "lane to right")
@property
def fasterLane(self) -> LaneSection:
"""The faster adjacent lane of the same type; rejects if there is none."""
return _rejectIfNonexistent(self._fasterLane, "faster lane")
@property
def slowerLane(self) -> LaneSection:
"""The slower adjacent lane of the same type; rejects if there is none."""
return _rejectIfNonexistent(self._slowerLane, "slower lane")
[docs] @distributionFunction
def shiftedBy(self, offset: int) -> Union[LaneSection, None]:
"""Find the lane a given number of lanes over from this lane."""
current = self
for i in range(abs(offset)):
if offset > 0:
current = current.laneToLeft
else:
current = current.laneToRight
if current is None:
return None
return current
[docs]@attr.s(auto_attribs=True, kw_only=True, repr=False, eq=False)
class Sidewalk(_ContainsCenterline, LinearElement):
"""Sidewalk()
A sidewalk.
"""
road: Road
crossings: Tuple[PedestrianCrossing]
[docs]@attr.s(auto_attribs=True, kw_only=True, repr=False, eq=False)
class PedestrianCrossing(_ContainsCenterline, LinearElement):
"""PedestrianCrossing()
A pedestrian crossing (crosswalk).
"""
parent: Union[Road, Intersection]
startSidewalk: Sidewalk
endSidewalk: Sidewalk
[docs]@attr.s(auto_attribs=True, kw_only=True, repr=False, eq=False)
class Shoulder(_ContainsCenterline, LinearElement):
"""Shoulder()
A shoulder of a road, including parking lanes by default.
"""
road: Road
[docs]@attr.s(auto_attribs=True, kw_only=True, repr=False, eq=False)
class Intersection(NetworkElement):
"""Intersection()
An intersection where multiple roads meet.
"""
roads: Tuple[Road] # in some order, preserving adjacency
incomingLanes: Tuple[Lane]
outgoingLanes: Tuple[Lane]
maneuvers: Tuple[Maneuver] # all possible maneuvers through the intersection
signals: Tuple[Signal]
crossings: Tuple[PedestrianCrossing] # also ordered to preserve adjacency
def __attrs_post_init__(self):
super().__attrs_post_init__()
for maneuver in self.maneuvers:
assert maneuver.connectingLane, maneuver
assert self.containsRegion(maneuver.connectingLane, tolerance=0.5)
if self.orientation is None:
self.orientation = VectorField(self.name, self._defaultHeadingAt)
def _defaultHeadingAt(self, point):
"""Default orientation for this Intersection.
We align along the closest connecting lane.
:meta private:
"""
point = _toVector(point)
man = min(self.maneuvers, key=lambda man: man.connectingLane.distanceTo(point))
return man.connectingLane.orientation[point]
@property
def is3Way(self) -> bool:
"""bool: Whether or not this is a 3-way intersection."""
return len(self.roads) == 3
@property
def is4Way(self) -> bool:
"""bool: Whether or not this is a 4-way intersection."""
return len(self.roads) == 4
@property
def isSignalized(self) -> bool:
"""bool: Whether or not this is a signalized intersection."""
return len(self.signals) > 0
[docs] @distributionFunction
def maneuversAt(self, point: Vectorlike) -> List[Maneuver]:
"""Get all maneuvers possible at a given point in the intersection."""
maneuvers = self.network._findPointInAll(
point, self.maneuvers, key=lambda m: m.connectingLane
)
if maneuvers:
return maneuvers
# If we filled holes in intersections when computing the geometry, there
# might be no maneuvers at some points inside the intersection. We'll pick
# the closest one.
man = min(self.maneuvers, key=lambda man: man.connectingLane.distanceTo(point))
return [man]
@distributionFunction
def nominalDirectionsAt(self, point: Vectorlike) -> Tuple[Orientation]:
point = _toVector(point)
maneuvers = self.maneuversAt(point)
assert maneuvers, self
return tuple(m.connectingLane.orientation[point] for m in maneuvers)
[docs]@attr.s(auto_attribs=True, kw_only=True, repr=False, eq=False)
class Signal:
"""Traffic lights, stop signs, etc.
.. warning::
Signal parsing is a work in progress and the API is likely to change in the future.
"""
uid: str = None
#: ID number as in OpenDRIVE (unique ID of the signal within the database)
openDriveID: int
#: Country code of the signal
country: str
#: Type identifier according to country code.
type: str
@property
def isTrafficLight(self) -> bool:
"""Whether or not this signal is a traffic light."""
return self.type == "1000001"
[docs]@attr.s(auto_attribs=True, kw_only=True, repr=False, eq=False)
class Network:
"""Network()
A road network.
Networks are composed of roads, intersections, sidewalks, etc., which are all
instances of `NetworkElement`.
Road networks can be loaded from standard formats using `Network.fromFile`.
"""
#: All network elements, indexed by unique ID.
elements: Dict[str, NetworkElement]
#: All ordinary roads in the network (i.e. those not part of an intersection).
roads: Tuple[Road]
#: All roads connecting one exit of an intersection to another.
connectingRoads: Tuple[Road]
#: All roads of either type.
allRoads: Tuple[Road] = None
#: All lane groups in the network.
laneGroups: Tuple[LaneGroup]
#: All lanes in the network.
lanes: Tuple[Lane]
#: All intersections in the network.
intersections: Tuple[Intersection]
#: All pedestrian crossings in the network.
crossings: Tuple[PedestrianCrossing]
#: All sidewalks in the network.
sidewalks: Tuple[Sidewalk]
#: All shoulders in the network (by default, includes parking lanes).
shoulders: Tuple[Shoulder]
#: All sections of ordinary roads in the network.
roadSections: Tuple[RoadSection] = None
#: All sections of lanes in the network.
laneSections: Tuple[LaneSection] = None
#: Whether or not cars drive on the left in this network.
driveOnLeft: bool = False
#: Distance tolerance for testing inclusion in network elements.
tolerance: float = 0
# convenience regions aggregated from various types of network elements
drivableRegion: PolygonalRegion = None
walkableRegion: PolygonalRegion = None
roadRegion: PolygonalRegion = None
laneRegion: PolygonalRegion = None
intersectionRegion: PolygonalRegion = None
crossingRegion: PolygonalRegion = None
sidewalkRegion: PolygonalRegion = None
curbRegion: PolylineRegion = None
shoulderRegion: PolygonalRegion = None
#: Traffic flow vector field aggregated over all roads (0 elsewhere).
roadDirection: VectorField = None
def __attrs_post_init__(self):
proxy = weakref.proxy(self)
for uid, elem in self.elements.items():
assert elem.uid == uid
elem.network = proxy
self.allRoads = self.roads + self.connectingRoads
self.roadSections = tuple(sec for road in self.roads for sec in road.sections)
self.laneSections = tuple(sec for lane in self.lanes for sec in lane.sections)
if self.roadRegion is None:
self.roadRegion = PolygonalRegion.unionAll(self.roads)
if self.laneRegion is None:
self.laneRegion = PolygonalRegion.unionAll(self.lanes)
if self.intersectionRegion is None:
self.intersectionRegion = PolygonalRegion.unionAll(self.intersections)
if self.crossingRegion is None:
self.crossingRegion = PolygonalRegion.unionAll(self.crossings)
if self.sidewalkRegion is None:
self.sidewalkRegion = PolygonalRegion.unionAll(self.sidewalks)
if self.shoulderRegion is None:
self.shoulderRegion = PolygonalRegion.unionAll(self.shoulders)
if self.drivableRegion is None:
self.drivableRegion = PolygonalRegion.unionAll(
(
self.laneRegion,
self.roadRegion, # can contain points slightly outside laneRegion
self.intersectionRegion,
)
)
assert self.drivableRegion.containsRegion(
self.laneRegion, tolerance=self.tolerance
)
assert self.drivableRegion.containsRegion(
self.roadRegion, tolerance=self.tolerance
)
assert self.drivableRegion.containsRegion(
self.intersectionRegion, tolerance=self.tolerance
)
if self.walkableRegion is None:
self.walkableRegion = self.sidewalkRegion.union(self.crossingRegion)
assert self.walkableRegion.containsRegion(
self.sidewalkRegion, tolerance=self.tolerance
)
assert self.walkableRegion.containsRegion(
self.crossingRegion, tolerance=self.tolerance
)
if self.curbRegion is None:
edges = []
for road in self.roads: # only include curbs of ordinary roads
if road.forwardLanes:
edges.append(road.forwardLanes.curb)
if road.backwardLanes:
edges.append(road.backwardLanes.curb)
self.curbRegion = PolylineRegion.unionAll(edges)
if self.roadDirection is None:
# TODO replace with a PolygonalVectorField for better pruning
self.roadDirection = VectorField("roadDirection", self._defaultRoadDirection)
# Build R-tree for faster lookup of roads, etc. at given points
self._uidForIndex = tuple(self.elements)
self._rtree = shapely.STRtree([elem.polygons for elem in self.elements.values()])
def _defaultRoadDirection(self, point):
"""Default value for the `roadDirection` vector field.
:meta private:
"""
point = _toVector(point)
road = self.roadAt(point)
return 0 if road is None else road.orientation[point]
#: File extension for cached versions of processed networks.
pickledExt = ".snet"
@classmethod
def _currentFormatVersion(cls):
"""Version number for the road network format.
Should be incremented whenever attributes of `Network`, `NetworkElement`, etc.,
attributes of the underlying Regions, or the serialization process itself are
changed, so that cached networks will be properly regenerated (rather than being
unpickled in an inconsistent state and causing errors later). Changes to the map
geometry calculations should be included, even if the format itself is unchanged.
:meta private:
"""
return 33
[docs] class DigestMismatchError(Exception):
"""Exception raised when loading a cached map not matching the original file."""
pass
[docs] @classmethod
def fromFile(cls, path, useCache: bool = True, writeCache: bool = True, **kwargs):
"""Create a `Network` from a map file.
This function calls an appropriate parsing routine based on the extension of the
given file. Supported map formats are:
* OpenDRIVE (``.xodr``): `Network.fromOpenDrive`
See the functions listed above for format-specific options to this function.
If no file extension is given in **path**, this function searches for any file
with the given name in one of the formats above (in order).
Args:
path: A string or other :term:`path-like object` giving a path to a file.
If no file extension is included, we search for any file type we know how
to parse.
useCache: Whether to use a cached version of the map, if one exists
and matches the given map file (default true; note that if the map file
changes, the cached version will still not be used).
writeCache: Whether to save a cached version of the processed map
after parsing has finished (default true).
kwargs: Additional keyword arguments specific to particular map formats.
Raises:
FileNotFoundError: no readable map was found at the given path.
ValueError: the given map is of an unknown format.
"""
path = pathlib.Path(path)
ext = path.suffix
handlers = { # in order of decreasing priority
".xodr": cls.fromOpenDrive, # OpenDRIVE
# Pickled native representation; this is the lowest priority, since original
# maps should take precedence, but if the pickled version exists and matches
# the original, we'll use it.
cls.pickledExt: cls.fromPickle,
}
if not ext: # no extension was given; search through possible formats
found = False
for ext in handlers:
newPath = path.with_suffix(ext)
if newPath.exists():
path = newPath
found = True
break
if not found:
raise FileNotFoundError(f"no readable maps found for path {path}")
elif ext not in handlers:
raise ValueError(f"unknown type of road network file {path}")
# If we don't have an underlying map file, return the pickled version directly
if ext == cls.pickledExt:
return cls.fromPickle(path)
# Otherwise, hash the underlying file to detect when the pickle is outdated
with open(path, "rb") as f:
data = f.read()
digest = hashlib.blake2b(data).digest()
# By default, use the pickled version if it exists and is not outdated
pickledPath = path.with_suffix(cls.pickledExt)
if useCache and pickledPath.exists():
try:
return cls.fromPickle(pickledPath, originalDigest=digest)
except pickle.UnpicklingError:
verbosePrint("Unable to load cached network (old format or corrupted).")
except cls.DigestMismatchError:
verbosePrint("Cached network does not match original file; ignoring it.")
# Not using the pickled version; parse the original file based on its extension
network = handlers[ext](path, **kwargs)
if writeCache:
verbosePrint(f"Caching road network in {cls.pickledExt} file.")
network.dumpPickle(path.with_suffix(cls.pickledExt), digest)
return network
[docs] @classmethod
def fromOpenDrive(
cls,
path,
ref_points: int = 20,
tolerance: float = 0.05,
fill_gaps: bool = True,
fill_intersections: bool = True,
elide_short_roads: bool = False,
):
"""Create a `Network` from an OpenDRIVE file.
Args:
path: Path to the file, as in `Network.fromFile`.
ref_points: Number of points to discretize continuous reference lines
into.
tolerance: Tolerance for merging nearby geometries.
fill_gaps: Whether to attempt to fill gaps between adjacent lanes.
fill_intersections: Whether to attempt to fill gaps inside
intersections.
elide_short_roads: Whether to attempt to fix geometry artifacts by
eliding roads with length less than **tolerance**.
"""
import scenic.formats.opendrive.xodr_parser as xodr_parser
road_map = xodr_parser.RoadMap(
tolerance=tolerance,
fill_intersections=fill_intersections,
elide_short_roads=elide_short_roads,
)
startTime = time.time()
verbosePrint("Parsing OpenDRIVE file...")
road_map.parse(path)
verbosePrint("Computing road geometry... (this may take a while)")
road_map.calculate_geometry(ref_points, calc_gap=fill_gaps, calc_intersect=True)
network = road_map.toScenicNetwork()
totalTime = time.time() - startTime
verbosePrint(f"Finished loading OpenDRIVE map in {totalTime:.2f} seconds.")
return network
@classmethod
def fromPickle(cls, path, originalDigest=None):
startTime = time.time()
verbosePrint("Loading cached version of road network...")
with open(path, "rb") as f:
versionField = f.read(4)
if len(versionField) != 4:
raise pickle.UnpicklingError(f"{cls.pickledExt} file is corrupted")
version = struct.unpack("<I", versionField)
if version[0] != cls._currentFormatVersion():
raise pickle.UnpicklingError(
f"{cls.pickledExt} file is too old; "
"regenerate it from the original map"
)
digest = f.read(64)
if len(digest) != 64:
raise pickle.UnpicklingError(f"{cls.pickledExt} file is corrupted")
if originalDigest and originalDigest != digest:
raise cls.DigestMismatchError(
f"{cls.pickledExt} file does not correspond to the original map; "
" regenerate it"
)
with gzip.open(f) as gf:
try:
network = pickle.load(gf) # invokes __setstate__ below
except pickle.UnpicklingError:
raise # propagate unpickling errors
except Exception as e:
# convert various other ways unpickling can fail into a more
# standard exception
raise pickle.UnpicklingError("unpickling failed") from e
totalTime = time.time() - startTime
verbosePrint(f"Loaded cached network in {totalTime:.2f} seconds.")
return network
def __setstate__(self, state):
# Restore our attributes (default behavior when __setstate__ isn't defined)
self.__dict__.update(state)
# Reconnect links between network elements
def reconnect(thing):
state = thing.__dict__
for key, value in state.items():
if isinstance(value, _ElementPlaceholder):
state[key] = self.elements[value.uid]
proxy = weakref.proxy(self)
for elem in self.elements.values():
reconnect(elem)
elem.network = proxy
for elem in itertools.chain(self.lanes, self.intersections):
for maneuver in elem.maneuvers:
reconnect(maneuver)
def dumpPickle(self, path, digest):
path = pathlib.Path(path)
if not path.suffix:
path = path.with_suffix(self.pickledExt)
version = struct.pack("<I", self._currentFormatVersion())
data = pickle.dumps(self)
with open(path, "wb") as f:
f.write(version) # uncompressed in case we change compression schemes later
f.write(digest) # uncompressed for quick lookup
with gzip.open(f, "wb") as gf:
gf.write(data)
[docs] @distributionMethod
def findPointIn(
self, point: Vectorlike, elems: Sequence[NetworkElement], reject: Union[bool, str]
) -> Union[NetworkElement, None]:
"""Find the first of the given elements containing the point.
Elements which *actually* contain the point have priority; if none contain the
point, then we search again allowing an error of up to **tolerance**. If there
are still no matches, we return None, unless **reject** is true, in which case we
reject the current sample.
"""
point = shapely.geometry.Point(_toVector(point))
def findElementWithin(distance):
target = point if distance == 0 else point.buffer(distance)
indices = self._rtree.query(target, predicate="intersects")
candidates = {self._uidForIndex[index] for index in indices}
if candidates:
for elem in elems:
if elem.uid in candidates:
return elem
return None
# First pass: check for elements containing the point.
if elem := findElementWithin(0):
return elem
# Second pass: check for elements within tolerance of the point.
if self.tolerance > 0 and (elem := findElementWithin(self.tolerance)):
return elem
# No matches found.
if reject:
if isinstance(reject, str):
message = reject
else:
message = "requested element does not exist"
_rejectSample(message)
return None
def _findPointInAll(self, point, things, key=lambda e: e):
point = _toVector(point)
found = []
for thing in things:
if key(thing).containsPoint(point):
found.append(thing)
if not found and self.tolerance > 0:
for thing in things:
if key(thing).distanceTo(point) <= self.tolerance:
found.append(thing)
return found
[docs] @distributionMethod
def elementAt(self, point: Vectorlike, reject=False) -> Union[NetworkElement, None]:
"""Get the highest-level `NetworkElement` at a given point, if any.
If the point lies in an `Intersection`, we return that; otherwise if the point
lies in a `Road`, we return that; otherwise we return :obj:`None`, or reject the
simulation if **reject** is true (default false).
"""
point = _toVector(point)
intersection = self.intersectionAt(point)
if intersection is not None:
return intersection
return self.roadAt(point, reject=reject)
[docs] @distributionMethod
def roadAt(self, point: Vectorlike, reject=False) -> Union[Road, None]:
"""Get the `Road` passing through a given point."""
return self.findPointIn(point, self.allRoads, reject)
[docs] @distributionMethod
def laneAt(self, point: Vectorlike, reject=False) -> Union[Lane, None]:
"""Get the `Lane` passing through a given point."""
return self.findPointIn(point, self.lanes, reject)
[docs] @distributionMethod
def laneSectionAt(self, point: Vectorlike, reject=False) -> Union[LaneSection, None]:
"""Get the `LaneSection` passing through a given point."""
point = _toVector(point)
lane = self.laneAt(point, reject=reject)
return None if lane is None else lane.sectionAt(point)
[docs] @distributionMethod
def laneGroupAt(self, point: Vectorlike, reject=False) -> Union[LaneGroup, None]:
"""Get the `LaneGroup` passing through a given point."""
point = _toVector(point)
road = self.roadAt(point, reject=reject)
return None if road is None else road.laneGroupAt(point, reject=reject)
[docs] @distributionMethod
def crossingAt(
self, point: Vectorlike, reject=False
) -> Union[PedestrianCrossing, None]:
"""Get the `PedestrianCrossing` passing through a given point."""
point = _toVector(point)
road = self.roadAt(point, reject=reject)
return None if road is None else road.crossingAt(point, reject=reject)
[docs] @distributionMethod
def intersectionAt(
self, point: Vectorlike, reject=False
) -> Union[Intersection, None]:
"""Get the `Intersection` at a given point."""
return self.findPointIn(point, self.intersections, reject)
[docs] @distributionMethod
def nominalDirectionsAt(self, point: Vectorlike, reject=False) -> Tuple[Orientation]:
"""Get the nominal traffic direction(s) at a given point, if any.
There can be more than one such direction in an intersection, for example: a car
at a given point could be going straight, turning left, etc.
"""
inter = self.intersectionAt(point)
if inter is not None:
return inter.nominalDirectionsAt(point)
road = self.roadAt(point, reject=reject)
if road is not None:
return road.nominalDirectionsAt(point)
return ()
[docs] def show(self, labelIncomingLanes=False):
"""Render a schematic of the road network for debugging.
If you call this function directly, you'll need to subsequently call
`matplotlib.pyplot.show` to actually display the diagram.
Args:
labelIncomingLanes (bool): Whether to label the incoming lanes of
intersections with their indices in ``incomingLanes``.
"""
import matplotlib.pyplot as plt
self.walkableRegion.show(plt, style="-", color="#00A0FF")
self.shoulderRegion.show(plt, style="-", color="#606060")
for road in self.roads:
road.show(plt, style="r-")
for lane in road.lanes: # will loop only over lanes of main roads
lane.leftEdge.show(plt, style="r--")
lane.rightEdge.show(plt, style="r--")
# Draw arrows indicating road direction
if lane.centerline.length >= 40:
pts = lane.centerline.pointsSeparatedBy(20)
else:
pts = [lane.centerline.pointAlongBy(0.5, normalized=True)]
hs = [lane.centerline.orientation[pt].yaw for pt in pts]
x, y, _ = zip(*pts)
u = [math.cos(h + (math.pi / 2)) for h in hs]
v = [math.sin(h + (math.pi / 2)) for h in hs]
plt.quiver(
x,
y,
u,
v,
pivot="middle",
headlength=4.5,
scale=0.06,
units="dots",
color="#A0A0A0",
)
for lane in self.lanes: # draw centerlines of all lanes (including connecting)
lane.centerline.show(plt, style=":", color="#A0A0A0")
self.intersectionRegion.show(plt, style="g")
if labelIncomingLanes:
for intersection in self.intersections:
for i, lane in enumerate(intersection.incomingLanes):
x, y, _ = lane.centerline[-1]
plt.plot([x], [y], "*b")
plt.annotate(str(i), (x, y))