diff --git a/src/scenic/domains/driving/roads.py b/src/scenic/domains/driving/roads.py index f06377c5a..147ed178c 100644 --- a/src/scenic/domains/driving/roads.py +++ b/src/scenic/domains/driving/roads.py @@ -147,6 +147,42 @@ def guessTypeFromLanes( return ManeuverType.STRAIGHT +@enum.unique +class SignalPriorityType(enum.Enum): + """OpenDRIVE ``e_signals_semantics_priority`` literals (Annex A.7.3). + + Values match the ``type`` attribute of ````. + Introduced in OpenDRIVE 1.8.0 as a country-agnostic behavior tag. + """ + + FOUR_WAY = "4way" + KEEP_CLEAR_LINE = "keepClearLine" + NO_PARKING_LINE = "noParkingLine" + NO_TURN_ON_RED = "noTurnOnRed" + PRIORITY_ROAD_END = "priorityRoadEnd" + PRIORITY_ROAD = "priorityRoad" + PRIORITY_TO_THE_RIGHT_RULE = "priorityToTheRightRule" + STOP_LINE = "stopLine" + STOP = "stop" + TRAFFIC_LIGHT = "trafficLight" + TURN_ON_RED_ALLOWED = "turnOnRedAllowed" + WAITING_LINE = "waitingLine" + YIELD = "yield" + #: Unrecognized ``type`` string from a newer OpenDRIVE revision, etc. + UNKNOWN = "unknown" + + @classmethod + def fromOpenDrive(cls, type_str: str) -> SignalPriorityType: + """Map an OpenDRIVE ``priority/@type`` string to an enum member. + + Unknown literals become `UNKNOWN` (callers may warn separately). + """ + for member in cls: + if member is not cls.UNKNOWN and member.value == type_str: + return member + return cls.UNKNOWN + + @attr.s(auto_attribs=True, kw_only=True, eq=False) class Maneuver(_ElementReferencer): """Maneuver() @@ -165,6 +201,8 @@ class Maneuver(_ElementReferencer): connectingLane: Union[Lane, None] = None #: intersection where the maneuver takes place, if any (`None` for lane mergers) intersection: Union[Intersection, None] = None + #: traffic signal controlling this maneuver, if any (`None` if uncontrolled or unknown) + signal: Optional[Signal] = None def __attrs_post_init__(self): assert self.type is ManeuverType.STRAIGHT or self.connectingLane is not None @@ -813,6 +851,11 @@ def nominalDirectionsAt(self, point: Vectorlike) -> Tuple[Orientation]: class Signal: """Traffic lights, stop signs, etc. + OpenDRIVE 1.8+ maps may annotate behavior via ````. + Those literals are stored in `priorities`. When that tuple is empty (legacy + maps), classification falls back to country-specific ``type`` codes where + known (e.g. OpenDRIVE ``1000001`` for traffic lights). + .. warning:: Signal parsing is a work in progress and the API is likely to change in the future. @@ -825,12 +868,89 @@ class Signal: country: str #: Type identifier according to country code. type: str + #: Subtype identifier according to country code (``None`` if absent). + subtype: Optional[str] = None + #: OpenDRIVE ``e_signals_semantics_priority`` entries (empty if unknown / pre-1.8). + #: All ```` children are kept; they are not collapsed to a single type. + priorities: Tuple[SignalPriorityType, ...] = () + #: Maneuvers that require this signal to be green (empty if unknown). + controlledManeuvers: Tuple[Maneuver, ...] = () + + def hasPriority(self, priority: SignalPriorityType) -> bool: + """Whether this signal lists the given OpenDRIVE priority semantic.""" + return priority in self.priorities @property def isTrafficLight(self) -> bool: - """Whether or not this signal is a traffic light.""" + """Whether or not this signal is a traffic light. + + Uses ``priorities`` when present; otherwise falls back to the legacy + OpenDRIVE Signal Reference type code ``1000001``. + """ + if self.priorities: + return self.hasPriority(SignalPriorityType.TRAFFIC_LIGHT) return self.type == "1000001" + @property + def isStop(self) -> bool: + """Whether or not this signal is a stop sign (semantics only).""" + return self.hasPriority(SignalPriorityType.STOP) + + @property + def isYield(self) -> bool: + """Whether or not this signal is a yield sign (semantics only).""" + return self.hasPriority(SignalPriorityType.YIELD) + + @property + def isStopLine(self) -> bool: + """Whether or not this signal marks a stop line (semantics only).""" + return self.hasPriority(SignalPriorityType.STOP_LINE) + + @property + def isPriorityRoad(self) -> bool: + """Whether or not this signal marks a priority road (semantics only).""" + return self.hasPriority(SignalPriorityType.PRIORITY_ROAD) + + @property + def isPriorityRoadEnd(self) -> bool: + """Whether or not this signal ends a priority road (semantics only).""" + return self.hasPriority(SignalPriorityType.PRIORITY_ROAD_END) + + @property + def isFourWay(self) -> bool: + """Whether or not this signal is a 4-way / all-way stop (semantics only).""" + return self.hasPriority(SignalPriorityType.FOUR_WAY) + + @property + def isNoTurnOnRed(self) -> bool: + """Whether or not this signal forbids turning on red (semantics only).""" + return self.hasPriority(SignalPriorityType.NO_TURN_ON_RED) + + @property + def isTurnOnRedAllowed(self) -> bool: + """Whether or not this signal allows turning on red (semantics only).""" + return self.hasPriority(SignalPriorityType.TURN_ON_RED_ALLOWED) + + @property + def isWaitingLine(self) -> bool: + """Whether or not this signal marks a waiting line (semantics only).""" + return self.hasPriority(SignalPriorityType.WAITING_LINE) + + @property + def isKeepClearLine(self) -> bool: + """Whether or not this signal marks a keep-clear line (semantics only).""" + return self.hasPriority(SignalPriorityType.KEEP_CLEAR_LINE) + + @property + def isNoParkingLine(self) -> bool: + """Whether or not this signal marks a no-parking line (semantics only).""" + return self.hasPriority(SignalPriorityType.NO_PARKING_LINE) + + @property + def isPriorityToTheRightRule(self) -> bool: + """Whether or not this signal indicates priority-to-the-right (semantics only).""" + return self.hasPriority(SignalPriorityType.PRIORITY_TO_THE_RIGHT_RULE) + @attr.s(auto_attribs=True, kw_only=True, repr=False, eq=False) class Network: diff --git a/src/scenic/formats/opendrive/xodr_parser.py b/src/scenic/formats/opendrive/xodr_parser.py index 8175d2c83..b7d8e17d4 100644 --- a/src/scenic/formats/opendrive/xodr_parser.py +++ b/src/scenic/formats/opendrive/xodr_parser.py @@ -1164,6 +1164,8 @@ def getEdges(forward): openDriveID=signal_.id_, country=signal_.country, type=signal_.type_, + subtype=signal_.subtype, + priorities=signal_.priorities, ) roadSignals.append(signal) @@ -1231,15 +1233,26 @@ def getEdges(forward): class Signal: - """Traffic lights, stop signs, etc.""" + """Traffic lights, stop signs, etc. (parser representation).""" - def __init__(self, id_, country, type_, subtype, orientation, validity=None): + def __init__( + self, + id_, + country, + type_, + subtype, + orientation, + validity=None, + priorities=(), + ): self.id_ = id_ self.country = country self.type_ = type_ self.subtype = subtype self.orientation = orientation self.validity = validity + #: Tuple of `roadDomain.SignalPriorityType` from ````. + self.priorities = tuple(priorities) def is_valid(self): return self.validity is None or self.validity != [0, 0] @@ -1459,6 +1472,52 @@ def __parse_signal_validity(self, validity_elem): return None return [int(validity_elem.get("fromLane")), int(validity_elem.get("toLane"))] + # OpenDRIVE / CARLA country="OpenDRIVE" type codes with a known priority meaning. + _LEGACY_TYPE_TO_PRIORITY = { + "1000001": roadDomain.SignalPriorityType.TRAFFIC_LIGHT, + "206": roadDomain.SignalPriorityType.STOP, + "205": roadDomain.SignalPriorityType.YIELD, + } + + def __warn_priority_type_disagreement(self, signal): + """Warn if a known legacy ``type`` conflicts with ```` semantics.""" + legacy = self._LEGACY_TYPE_TO_PRIORITY.get(signal.type_) + if legacy is not None and signal.priorities and legacy not in signal.priorities: + listed = ", ".join(p.value for p in signal.priorities) + warn( + f'signal {signal.id_} has OpenDRIVE type "{signal.type_}" ' + f"(legacy {legacy.value}) but lists [{listed}]; " + f"using priorities for classification" + ) + + def __parse_signal_priorities(self, signal_elem): + """Parse ```` children (OpenDRIVE 1.8+). + + Returns a tuple of `roadDomain.SignalPriorityType`. Unknown literals are + mapped to `UNKNOWN` and emit an `OpenDriveWarning`. Other semantic + categories (````, ````, …) are ignored for now. + """ + semantics_elem = signal_elem.find("semantics") + if semantics_elem is None: + return () + priorities = [] + for priority_elem in semantics_elem.findall("priority"): + type_str = priority_elem.get("type") + if type_str is None: + warn( + f'signal {signal_elem.get("id")} has without type; ' + "skipping it" + ) + continue + priority = roadDomain.SignalPriorityType.fromOpenDrive(type_str) + if priority is roadDomain.SignalPriorityType.UNKNOWN: + warn( + f'signal {signal_elem.get("id")} has unrecognized ' + f'priority type "{type_str}"; storing as UNKNOWN' + ) + priorities.append(priority) + return tuple(priorities) + def __parse_signal(self, signal_elem): return Signal( signal_elem.get("id"), @@ -1466,7 +1525,13 @@ def __parse_signal(self, signal_elem): signal_elem.get("type"), signal_elem.get("subtype"), signal_elem.get("orientation"), + # other required fields not parsed: + # dynamic signal_elem.get("dynamic"), + # s signal_elem.get("s"), + # t signal_elem.get("t"), + # zOffset signal_elem.get("zOffset"), self.__parse_signal_validity(signal_elem.find("validity")), + self.__parse_signal_priorities(signal_elem), ) def __parse_signal_reference(self, signal_reference_elem): @@ -1677,12 +1742,17 @@ def popLastSectionIfShort(l): for signal_elem in signals.iter("signal"): signal = self.__parse_signal(signal_elem) if signal.is_valid(): + # Check once here (not in __parse_signal): signals are also + # parsed earlier into _temp_signals for signalReference. + self.__warn_priority_type_disagreement(signal) road.signals.append(signal) for signal_ref_elem in signals.iter("signalReference"): signalReference = self.__parse_signal_reference(signal_ref_elem) if signalReference.is_valid(): referencedSignal = _temp_signals[signalReference.id_] + # Semantics (priorities) come from the canonical ; + # the reference only overrides orientation / validity. signal = Signal( referencedSignal.id_, referencedSignal.country, @@ -1690,6 +1760,7 @@ def popLastSectionIfShort(l): referencedSignal.subtype, signalReference.orientation, signalReference.validity, + referencedSignal.priorities, ) road.signals.append(signal) @@ -1890,12 +1961,37 @@ def registerAll(elements): allRoads.append(outgoingRoad) seenRoads.add(outgoingRoad.id) + # Find the signal controlling this maneuver, if any + controllingSignal = None + # Candidate sources in priority order. Each triple pairs a + # raw parser road (signals retain .validity) with its + # converted Scenic road (domain Signal objects to store), + # plus the OpenDRIVE lane ID to test against validity: + # 1. connecting road / toID — turn-specific controls + # 2. incoming road / fromID — approach signals + for rawRoad, scenicRoad, laneID in ( + (self.roads[connectingID], connectingRoad, toID), + (oldRoad, incomingRoad, fromID), + ): + for rawSignal, scenicSignal in zip( + rawRoad.signals, scenicRoad.signals + ): + validity = rawSignal.validity + if validity is None or min(validity) <= laneID <= max( + validity + ): + controllingSignal = scenicSignal + break + if controllingSignal is not None: + break + # TODO future OpenDRIVE extension annotating left/right turns? maneuver = roadDomain.Maneuver( startLane=fromLane.lane, connectingLane=toLane.lane, endLane=outgoingLane, intersection=None, # will be patched once the Intersection is created + signal=controllingSignal, ) maneuversForLane[fromLane.lane].append(maneuver) @@ -1906,6 +2002,11 @@ def registerAll(elements): lane.maneuvers = tuple(maneuvers) allManeuvers.extend(maneuvers) + # Reverse mapping: accumulate maneuvers onto each controlling Signal. + for maneuver in allManeuvers: + if maneuver.signal is not None: + maneuver.signal.controlledManeuvers += (maneuver,) + # Order connected roads and lanes by adjacency def cyclicOrder(elements, contactStart=None): points = [] diff --git a/tests/formats/opendrive/test_signals.py b/tests/formats/opendrive/test_signals.py new file mode 100644 index 000000000..7472e06f5 --- /dev/null +++ b/tests/formats/opendrive/test_signals.py @@ -0,0 +1,645 @@ +"""Integration tests for signal↔maneuver linking via inline OpenDRIVE fixtures.""" + +from pathlib import Path + +from scenic.domains.driving.roads import ManeuverType, Network + +# Minimal maps exercised below (same geometries as the local demo/signals maps). +MAP_VALIDITY_LANES = """ + +
+ + + + + + + + +
+ + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + +""" + +MAP_APPROACH_SIGNAL = """ + +
+ + + + + + + + +
+ + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + + + + +""" + +MAP_T_JUNCTION = """ + +
+ + + + + + + + +
+ + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + + + + + + + +""" + +MAP_UNSIGNALIZED = """ + +
+ + + + + + + + +
+ + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + + + + +""" + + +def load_network(tmp_path: Path, xml: str) -> Network: + path = tmp_path / "map.xodr" + path.write_text(xml) + return Network.fromFile(path, useCache=False) + + +def open_drive_id(lane) -> int: + ids = {sec.openDriveID for sec in lane.sections} + assert len(ids) == 1, lane + return next(iter(ids)) + + +def all_maneuvers(network: Network): + return [m for inter in network.intersections for m in inter.maneuvers] + + +def assert_bidirectional(maneuver): + assert maneuver.signal is not None + assert maneuver in maneuver.signal.controlledManeuvers + for other in maneuver.signal.controlledManeuvers: + assert other.signal is maneuver.signal + + +def test_validity_lanes_per_lane_signals(tmp_path): + network = load_network(tmp_path, MAP_VALIDITY_LANES) + mans = all_maneuvers(network) + assert len(mans) == 2 + + by_start = {open_drive_id(m.startLane): m for m in mans} + assert set(by_start) == {-1, -2} + + man_inner, man_outer = by_start[-1], by_start[-2] + assert man_inner.type is ManeuverType.STRAIGHT + assert man_outer.type is ManeuverType.STRAIGHT + assert man_inner.signal.openDriveID == "101" + assert man_outer.signal.openDriveID == "102" + assert man_inner.signal is not man_outer.signal + assert man_inner.signal.isTrafficLight + assert_bidirectional(man_inner) + assert_bidirectional(man_outer) + assert man_inner.signal.controlledManeuvers == (man_inner,) + assert man_outer.signal.controlledManeuvers == (man_outer,) + + +def test_approach_signal_shared_by_both_lanes(tmp_path): + network = load_network(tmp_path, MAP_APPROACH_SIGNAL) + mans = all_maneuvers(network) + assert len(mans) == 2 + assert mans[0].signal is mans[1].signal + assert mans[0].signal.openDriveID == "201" + for man in mans: + assert_bidirectional(man) + assert set(mans[0].signal.controlledManeuvers) == set(mans) + + +def test_t_junction_distinct_signals_per_maneuver(tmp_path): + network = load_network(tmp_path, MAP_T_JUNCTION) + mans = all_maneuvers(network) + assert len(mans) == 2 + + by_type = {m.type: m for m in mans} + assert set(by_type) == {ManeuverType.STRAIGHT, ManeuverType.RIGHT_TURN} + assert ( + by_type[ManeuverType.STRAIGHT].startLane + is by_type[ManeuverType.RIGHT_TURN].startLane + ) + + straight, right = by_type[ManeuverType.STRAIGHT], by_type[ManeuverType.RIGHT_TURN] + assert straight.signal.openDriveID == "301" + assert right.signal.openDriveID == "302" + assert straight.signal is not right.signal + assert_bidirectional(straight) + assert_bidirectional(right) + + +def test_unsignalized_maneuvers_have_no_signal(tmp_path): + network = load_network(tmp_path, MAP_UNSIGNALIZED) + mans = all_maneuvers(network) + assert len(mans) == 2 + assert all(m.signal is None for m in mans) + for road in list(network.roads) + list(network.connectingRoads): + for sig in road.signals: + assert sig.controlledManeuvers == () + + +def test_intersection_backrefs_consistent(tmp_path): + for xml in ( + MAP_VALIDITY_LANES, + MAP_APPROACH_SIGNAL, + MAP_T_JUNCTION, + MAP_UNSIGNALIZED, + ): + network = load_network(tmp_path, xml) + for inter in network.intersections: + for man in inter.maneuvers: + assert man.intersection is inter + if man.signal is not None: + assert_bidirectional(man) + assert man.signal.isTrafficLight + + +# --------------------------------------------------------------------------- +# Legacy OpenDRIVE type codes (CARLA: 1000001 / 206 / 205) and 1.8+ priorities +# --------------------------------------------------------------------------- + +DEMO_SIGNALS = ( + Path(__file__).resolve().parents[3] / "assets" / "maps" / "demo" / "signals" +) + + +def test_legacy_stop_and_yield_type_codes(): + """CARLA maps use country=OpenDRIVE type 206 (stop) and 205 (yield).""" + network = Network.fromFile(DEMO_SIGNALS / "06_legacy_stop_yield.xodr", useCache=False) + by_start = {open_drive_id(m.startLane): m for m in all_maneuvers(network)} + assert set(by_start) == {-1, -2} + + stop_man, yield_man = by_start[-1], by_start[-2] + assert stop_man.signal.openDriveID == "601" + assert yield_man.signal.openDriveID == "602" + + assert stop_man.signal.priorities == () + assert yield_man.signal.priorities == () + assert stop_man.signal.type == "206" + assert yield_man.signal.type == "205" + + assert stop_man.signal.isStop + assert not stop_man.signal.isYield + assert not stop_man.signal.isTrafficLight + + assert yield_man.signal.isYield + assert not yield_man.signal.isStop + assert not yield_man.signal.isTrafficLight + + assert_bidirectional(stop_man) + assert_bidirectional(yield_man) + + +def test_priority_semantics_without_legacy_types(): + """```` classifies signals even when country type is uninformative.""" + from scenic.domains.driving.roads import SignalPriorityType + + network = Network.fromFile( + DEMO_SIGNALS / "07_priority_semantics.xodr", useCache=False + ) + by_start = {open_drive_id(m.startLane): m for m in all_maneuvers(network)} + assert set(by_start) == {-1, -2, -3} + + stop_man, yield_man, light_man = by_start[-1], by_start[-2], by_start[-3] + assert stop_man.signal.type == "-1" + assert yield_man.signal.type == "-1" + assert light_man.signal.type == "-1" + + assert stop_man.signal.isStop + assert not stop_man.signal.isTrafficLight + assert stop_man.signal.priorities == (SignalPriorityType.STOP,) + + assert yield_man.signal.isYield + assert not yield_man.signal.isStop + assert yield_man.signal.priorities == (SignalPriorityType.YIELD,) + + assert light_man.signal.isTrafficLight + assert light_man.signal.isStopLine + assert not light_man.signal.isStop + assert light_man.signal.priorities == ( + SignalPriorityType.TRAFFIC_LIGHT, + SignalPriorityType.STOP_LINE, + ) + + for man in (stop_man, yield_man, light_man): + assert_bidirectional(man) + + +def test_updated_traffic_light_demo_maps_have_priorities(): + """Regenerated TL demos keep type 1000001 and add ````.""" + from scenic.domains.driving.roads import SignalPriorityType + + for name in ( + "01_signal_validity_lanes.xodr", + "02_signal_on_approach.xodr", + "03_t_junction_turn_signals.xodr", + "05_four_way_protected_left.xodr", + ): + network = Network.fromFile(DEMO_SIGNALS / name, useCache=False) + signals = [ + sig + for road in list(network.roads) + list(network.connectingRoads) + for sig in road.signals + ] + assert signals, name + for sig in signals: + assert sig.type == "1000001", (name, sig.openDriveID) + assert SignalPriorityType.TRAFFIC_LIGHT in sig.priorities, ( + name, + sig.openDriveID, + ) + assert sig.isTrafficLight + assert not sig.isStop + assert not sig.isYield + + +def test_legacy_traffic_light_fixture_still_works(tmp_path): + """Inline fixtures without ```` still detect type 1000001 lights.""" + network = load_network(tmp_path, MAP_VALIDITY_LANES) + for man in all_maneuvers(network): + assert man.signal.priorities == () + assert man.signal.isTrafficLight + assert man.signal.subtype == "-1" + + +def test_priority_type_disagreement_warns_and_prefers_priorities(tmp_path): + """Legacy type and ```` disagree → warn; priorities win.""" + import warnings + + from scenic.domains.driving.roads import SignalPriorityType + from scenic.formats.opendrive.xodr_parser import OpenDriveWarning + + # Same geometry as MAP_VALIDITY_LANES but type 206 (stop) with trafficLight priority. + xml = MAP_VALIDITY_LANES.replace( + 'type="1000001" country="OpenDRIVE"\n subtype="-1" value="-1">\n' + ' \n' + " ", + 'type="206" country="OpenDRIVE"\n subtype="-1" value="-1">\n' + " \n" + ' \n' + " \n" + ' \n' + " ", + 1, # only the first signal (id 101) + ) + # Second signal unchanged (still legacy 1000001, no semantics). + + with warnings.catch_warnings(record=True) as caught: + warnings.simplefilter("always", OpenDriveWarning) + network = load_network(tmp_path, xml) + + disagreement = [ + w + for w in caught + if issubclass(w.category, OpenDriveWarning) + and "using priorities for classification" in str(w.message) + and 'type "206"' in str(w.message) + ] + assert len(disagreement) == 1, [str(w.message) for w in caught] + + by_start = {open_drive_id(m.startLane): m for m in all_maneuvers(network)} + # Lane -1: priorities win → traffic light, not stop + assert by_start[-1].signal.isTrafficLight + assert not by_start[-1].signal.isStop + assert SignalPriorityType.TRAFFIC_LIGHT in by_start[-1].signal.priorities + # Lane -2: unchanged legacy light + assert by_start[-2].signal.isTrafficLight + assert by_start[-2].signal.priorities == ()