Link OpenDRIVE signals to intersection maneuvers

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 ``<semantics><priority …/>``.
+    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 ``<semantics><priority …/>``.
+    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 ``<priority>`` 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:

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 ``<semantics><priority>``.
+        self.priorities = tuple(priorities)
 
     def is_valid(self):
         return self.validity is None or self.validity != [0, 0]
@@ -1459,14 +1472,66 @@ 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 ``<priority>`` 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 <priority> lists [{listed}]; "
+                f"using priorities for classification"
+            )
+
+    def __parse_signal_priorities(self, signal_elem):
+        """Parse ``<semantics><priority type="…"/>`` children (OpenDRIVE 1.8+).
+
+        Returns a tuple of `roadDomain.SignalPriorityType`. Unknown literals are
+        mapped to `UNKNOWN` and emit an `OpenDriveWarning`. Other semantic
+        categories (``<speed>``, ``<lane>``, …) 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 <priority> 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"),
             signal_elem.get("country"),
             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,19 +1742,25 @@ 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 <signal>;
+                        # the reference only overrides orientation / validity.
                         signal = Signal(
                             referencedSignal.id_,
                             referencedSignal.country,
                             referencedSignal.type_,
                             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 = []