diff --git a/src/scenic/domains/driving/roads.py b/src/scenic/domains/driving/roads.py index f06377c5a..c0928286b 100644 --- a/src/scenic/domains/driving/roads.py +++ b/src/scenic/domains/driving/roads.py @@ -237,11 +237,25 @@ class NetworkElement(_ElementReferencer, PolygonalRegion): #: 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. + #: Optional speed limit, which may be inherited from parent. (depreciated) speedLimit: Union[float, None] = None + #: Optional ``(s_start, s_end, speed_mps)`` ranges along this element's centerline. + speedLimitRanges: Tuple[Tuple[float, float, float], ...] = () #: Uninterpreted semantic tags, e.g. 'roundabout'. tags: FrozenSet[str] = frozenset() + @distributionFunction + def speedLimitAt(self, s: float) -> Union[float, None]: + """Get the speed limit at coordinate *s* along this element, in meters.""" + s = max(0, s) + if self.speedLimitRanges: + for range_start, range_end, speed in self.speedLimitRanges: + if range_start <= s < range_end: + return speed # may be None -> genuine "no limit" stretch + # s is at/beyond the final (half-open) range's end: clamp to it. + return speed + return self.speedLimit + def __attrs_post_init__(self): assert self.uid is not None or self.id is not None if self.uid is None: @@ -1293,6 +1307,24 @@ def laneSectionAt(self, point: Vectorlike, reject=False) -> Union[LaneSection, N lane = self.laneAt(point, reject=reject) return None if lane is None else lane.sectionAt(point) + @distributionMethod + def speedLimitAt(self, point: Vectorlike, reject=False) -> Union[float, None]: + """Get the speed limit at a given point, if any. + + Looks up the `LaneSection` containing the point and returns its speed + limit there. When the section has multiple ``speedLimitRanges``, the + limit is taken from the range covering the point's projected distance + along the section centerline. + """ + point = _toVector(point) + section = self.laneSectionAt(point, reject=reject) + if section is None: + return None + if len(section.speedLimitRanges) <= 1: + return section.speedLimit + s = section.centerline.lineString.project(geometry.makeShapelyPoint(point)) + return section.speedLimitAt(s) + @distributionMethod def laneGroupAt(self, point: Vectorlike, reject=False) -> Union[LaneGroup, None]: """Get the `LaneGroup` passing through a given point.""" diff --git a/src/scenic/formats/opendrive/xodr_parser.py b/src/scenic/formats/opendrive/xodr_parser.py index 8175d2c83..30aee57ae 100644 --- a/src/scenic/formats/opendrive/xodr_parser.py +++ b/src/scenic/formats/opendrive/xodr_parser.py @@ -34,6 +34,153 @@ def warn(message): warnings.warn(message, OpenDriveWarning, stacklevel=2) +def speed_to_mps(speed_elem): + """Convert an OpenDRIVE ```` element to m/s.""" + raw = speed_elem.get("max") + if raw in ("no limit", "undefined"): + return None + value = float(raw) + unit = speed_elem.get("unit", "m/s") + if unit == "m/s": + return value + elif unit == "km/h": + return value / 3.6 + elif unit == "mph": + return value * 0.44704 + else: + raise ValueError(f"unsupported speed unit: {unit!r}") + + +def _speed_limit_ranges_from_s_speed_records(sorted_records, domain_length): + """Build ``(s_start, s_end, speed_mps)`` ranges from ``[(s, speed_mps), ...]``. + + Coverage is total over ``[0, domain_length)``: each record keeps its own + speed (no inheritance from a previous record), and stretches with no defined + limit are emitted as explicit ``None`` ranges. This lets ``speedLimitAt`` + distinguish a genuine "no limit" stretch from a lookup past the end. + """ + # Already sorted in speed_limit_ranges_from_type_records + # Guarantee coverage from 0: an undefined leading stretch stays None. + if sorted_records[0][0] > 0: + sorted_records = [(0.0, None)] + sorted_records + + ranges = [] + for i, (s, speed_mps) in enumerate(sorted_records): + s_end = sorted_records[i + 1][0] if i + 1 < len(sorted_records) else domain_length + if s_end <= s: + continue + if ranges and ranges[-1][2] == speed_mps and ranges[-1][1] == s: + ranges[-1] = (ranges[-1][0], s_end, speed_mps) + else: + ranges.append((s, s_end, speed_mps)) + + return ranges + + +def speed_limit_ranges_from_type_records(type_records, road_length): + """Build ``(s_start, s_end, speed_mps)`` ranges from OpenDRIVE ```` records.""" + if not type_records: + return [] + + sorted_records = sorted(type_records, key=lambda record: record[0]) + # Each keeps its own speed (may be None); no inheritance across types. + s_speed_records = [(s, speed_mps) for s, _road_type, speed_mps in sorted_records] + return _speed_limit_ranges_from_s_speed_records(s_speed_records, road_length) + + +def speed_limit_ranges_from_lane_records(speed_records, section_length): + """Build ``(s_start, s_end, speed_mps)`` ranges from lane ```` records. + + ``speed_records`` are ``(sOffset, speed_mps)`` pairs relative to the lane + section start; ``section_length`` is the lane section length in meters. + """ + if not speed_records: + return [] + + return _speed_limit_ranges_from_s_speed_records(speed_records, section_length) + + +def _clip_ranges_to_interval(ranges, interval_start, interval_end): + """Clip ``(s_start, s_end, speed_mps)`` ranges to ``[interval_start, interval_end)``.""" + clipped = [] + for range_start, range_end, speed in ranges: + if range_end <= interval_start or range_start >= interval_end: + continue + clipped.append( + ( + max(range_start, interval_start), + min(range_end, interval_end), + speed, + ) + ) + return clipped + + +def effective_speed_limit_ranges(road_ranges, lane_ranges, section_s0, section_length): + """Merge road and lane speed ranges for one OpenDRIVE lane section. + + Returns ``(s_start, s_end, speed_mps)`` ranges with *s* relative to the lane + section start. When lane speeds are present they fully replace road speeds. + """ + if lane_ranges: + return lane_ranges + + section_end = section_s0 + section_length + road_in_section = _clip_ranges_to_interval(road_ranges, section_s0, section_end) + return [ + (range_start - section_s0, range_end - section_s0, speed) + for range_start, range_end, speed in road_in_section + ] + + +def speed_limits_for_s_interval(ranges, s_start, s_end): + """Speed limits applying to the interval ``[s_start, s_end)``. + + Returns a frozenset of every limit in ``ranges`` that overlaps the interval. + """ + if not ranges: + return frozenset() + + # A range overlaps the interval iff it starts before s_end and ends after s_start. + return frozenset( + speed + for range_start, range_end, speed in ranges + if range_end > s_start and range_start < s_end and speed is not None + ) + + +def assign_speed_limit_from_ranges(element, ranges, warn_context=None): + """Set ``speedLimit`` and ``speedLimitRanges`` from effective speed ranges.""" + if not ranges: + return + + speeds = {speed for _, _, speed in ranges if speed is not None} + + element.speedLimit = min(speeds) if speeds else None + element.speedLimitRanges = tuple(ranges) + if len(speeds) > 1 and warn_context is not None: + speeds_text = ", ".join(f"{speed:.4g} m/s" for speed in sorted(speeds)) + warn( + f"{warn_context}: spans multiple speed limits {{{speeds_text}}};" + f" using minimum {element.speedLimit:.4g} m/s" + ) + + +def assign_semantic_tags(road_map): + """Populate each ``Road.extra_tags`` from the type of its junction. + + A road belonging to a junction (``road.junction`` equals the junction id) + inherits that junction's semantic tags; all other roads get no extra tags. + """ + junction_tags = {jid: junction.tags for jid, junction in road_map.junctions.items()} + for road in road_map.roads.values(): + if road.junction is not None: + # road.junction is the raw OpenDRIVE id string; junctions are keyed by int. + road.extra_tags = junction_tags.get(int(road.junction), frozenset()) + else: + road.extra_tags = frozenset() + + def buffer_union(polys, tolerance=0.01): return polygonUnion(polys, buf=tolerance, tolerance=tolerance) @@ -286,6 +433,7 @@ class Lane: def __init__(self, id_, type_, pred=None, succ=None): self.id_ = id_ self.width = [] # List of tuples (Poly3, int) for width and s-offset. + self.speed_records = [] # [(sOffset, speed_mps), ...] from elements. self.type_ = type_ self.pred = pred self.succ = succ @@ -374,9 +522,11 @@ def __init__(self, incoming_id, connecting_id, connecting_contact, lane_links): # dict mapping incoming to connecting lane ids (empty = identity mapping) self.lane_links = lane_links - def __init__(self, id_, name): + def __init__(self, id_, name, type_="default"): self.id_ = id_ self.name = name + self.type_ = type_ + self.tags = frozenset({type_} if type_ and type_ != "default" else ()) self.connections = [] # Ids of roads that are paths within junction: self.paths = [] @@ -419,6 +569,9 @@ def __init__(self, name, id_, length, junction, drive_on_right=True): self.remappedStartLanes = None # hack for handling spurious initial lane sections + self.type_records = [] # [(s, openDriveRoadType, speedLimitMps), ...] + self.extra_tags = frozenset() + def get_ref_line_offset(self, s): if not self.offset: return 0 @@ -767,15 +920,49 @@ def calculate_geometry( def toScenicRoad(self, tolerance): assert self.sec_points + + # collect speed ranges from type records and total road length + speed_ranges = speed_limit_ranges_from_type_records( + self.type_records, self.length + ) + + type_tags = frozenset( + open_drive_type + for _, open_drive_type, _ in self.type_records + # if open_drive_type + ) + road_level_tags = frozenset(type_tags | self.extra_tags) allElements = [] # Create lane and road sections roadSections = [] last_section = None forwardSidewalks, backwardSidewalks = [], [] forwardShoulders, backwardShoulders = [], [] - for sec, pts, sec_poly, lane_polys in zip( - self.lane_secs, self.sec_points, self.sec_polys, self.sec_lane_polys + for sec_index, (sec, pts, sec_poly, lane_polys) in enumerate( + zip( + self.lane_secs, + self.sec_points, + self.sec_polys, + self.sec_lane_polys, + ) ): + s_start = sec.s0 + s_end = ( + self.lane_secs[sec_index + 1].s0 + if sec_index + 1 < len(self.lane_secs) + else self.length + ) + overlapping_speeds = speed_limits_for_s_interval(speed_ranges, s_start, s_end) + section_speed_limit = min(overlapping_speeds) if overlapping_speeds else None + if len(overlapping_speeds) > 1: + speeds_text = ", ".join( + f"{speed:.4g} m/s" for speed in sorted(overlapping_speeds) + ) + warn( + f"road {self.id_} section s=[{s_start},{s_end}):" + f" spans multiple speed limits {{{speeds_text}}};" + f" using minimum {section_speed_limit:.4g} m/s" + ) pts = [pt[:2] for pt in pts] # drop s coordinate assert sec.drivable_lanes laneSections = {} @@ -809,6 +996,7 @@ def toScenicRoad(self, tolerance): road=None, openDriveID=id_, isForward=id_ < 0, + tags=frozenset({lane.type_} if lane.type_ else ()), ) section._original_lane = lane laneSections[id_] = section @@ -823,9 +1011,31 @@ def toScenicRoad(self, tolerance): predecessor=last_section, road=None, # will set later lanesByOpenDriveID=laneSections, + tags=road_level_tags, ) roadSections.append(section) allElements.append(section) + + if section_speed_limit is not None and section.speedLimit is None: + section.speedLimit = section_speed_limit + section_length = s_end - s_start + for id_, lane_section in laneSections.items(): + lane = sec.drivable_lanes[id_] + lane_ranges = speed_limit_ranges_from_lane_records( + lane.speed_records, section_length + ) + effective_ranges = effective_speed_limit_ranges( + speed_ranges, lane_ranges, s_start, section_length + ) + if effective_ranges: + assign_speed_limit_from_ranges( + lane_section, + effective_ranges, + warn_context=( + f"road {self.id_} lane {id_} section s=[{s_start},{s_end})" + ), + ) + last_section = section fss, bss = {}, {} @@ -1017,6 +1227,20 @@ def makeShoulder(sections, backward=False): adj.append(lane._laneToRight) lane.adjacentLanes = tuple(adj) + section_speed_limits = [ + section.speedLimit + for section in roadSections + if section.speedLimit is not None + ] + road_speed_limit = min(section_speed_limits) if section_speed_limits else None + if road_speed_limit is not None: + for section in roadSections: + if section.speedLimit is None: + section.speedLimit = road_speed_limit + for lane_section in section.lanesByOpenDriveID.values(): + if lane_section.speedLimit is None: + lane_section.speedLimit = road_speed_limit + # Gather lane sections into lanes nextID = 0 forwardLanes, backwardLanes = [], [] @@ -1058,6 +1282,11 @@ def makeShoulder(sections, backward=False): leftEdge = PolylineRegion(cleanChain(leftPoints)) rightEdge = PolylineRegion(cleanChain(rightPoints)) centerline = PolylineRegion(cleanChain(centerPoints)) + lane_section_speed_limits = [ + section.speedLimit + for section in sections + if section.speedLimit is not None + ] lane = roadDomain.Lane( id=f"road{self.id_}_lane{nextID}", polygon=ls.parent_lane_poly, @@ -1068,6 +1297,12 @@ def makeShoulder(sections, backward=False): road=None, sections=tuple(sections), successor=successorLane, # will correct inter-road links later + tags=frozenset().union(*(sec.tags for sec in sections)), + speedLimit=( + min(lane_section_speed_limits) + if lane_section_speed_limits + else None + ), ) nextID += 1 for section in sections: @@ -1128,6 +1363,7 @@ def getEdges(forward): bikeLane=None, shoulder=forwardShoulder, opposite=None, + tags=road_level_tags, ) allElements.append(forwardGroup) else: @@ -1149,6 +1385,7 @@ def getEdges(forward): bikeLane=None, shoulder=backwardShoulder, opposite=forwardGroup, + tags=road_level_tags, ) allElements.append(backwardGroup) if forwardGroup: @@ -1178,6 +1415,7 @@ def getEdges(forward): else: leftEdge = forwardGroup.leftEdge centerline = PolylineRegion(tuple(pt[:2] for pt in self.ref_line_points)) + road = roadDomain.Road( name=self.name, uid=f"road{self.id_}", # need prefix to prevent collisions with intersections @@ -1192,8 +1430,17 @@ def getEdges(forward): sections=roadSections, signals=tuple(roadSignals), crossings=(), # TODO add these! + speedLimit=road_speed_limit, + tags=road_level_tags, ) allElements.append(road) + if road_speed_limit is not None: + for group in (forwardGroup, backwardGroup): + if group is not None and group.speedLimit is None: + group.speedLimit = road_speed_limit + for lane in lanes: + if lane.speedLimit is None: + lane.speedLimit = road_speed_limit # Set up parent references if forwardGroup: @@ -1266,12 +1513,19 @@ def __init__( "driving", "entry", "exit", - "offRamp", "onRamp", + "offRamp", "connectingRamp", + "slipLane", + ), + sidewalk_lane_types=("walking", "sidewalk"), # sidewalk deprecated + shoulder_lane_types=( + "shoulder", + "border", + "restricted", + "parking", + "stop", ), - sidewalk_lane_types=("sidewalk",), - shoulder_lane_types=("shoulder", "parking", "stop", "border"), elide_short_roads=False, ): self.tolerance = self.defaultTolerance if tolerance is None else tolerance @@ -1425,6 +1679,11 @@ def __parse_lanes(self, lanes_elem): float(w.get("d")), ) lane.width.append((w_poly, float(w.get("sOffset")))) + for speed_elem in l.iter("speed"): + lane.speed_records.append( + (float(speed_elem.get("sOffset", 0)), speed_to_mps(speed_elem)) + ) + lane.speed_records.sort(key=lambda record: record[0]) lanes[id_] = lane return lanes @@ -1484,7 +1743,7 @@ def parse(self, path): # parse junctions for j in root.iter("junction"): - junction = Junction(int(j.get("id")), j.get("name")) + junction = Junction(int(j.get("id")), j.get("name"), j.get("type", "default")) for c in j.iter("connection"): ty = c.get("type", "default") if ty != "default": @@ -1530,6 +1789,13 @@ def parse(self, path): else: pred_link = succ_link = None + for type_elem in r.findall("type"): + s = float(type_elem.get("s")) + road_type = type_elem.get("type") + speed_elem = type_elem.find("speed") + speed_mps = speed_to_mps(speed_elem) if speed_elem is not None else None + road.type_records.append((s, road_type, speed_mps)) + if road.length < self.tolerance: warn( f"road {road.id_} has length shorter than tolerance;" @@ -1718,6 +1984,7 @@ def popLastSectionIfShort(l): def toScenicNetwork(self): assert self.intersection_region is not None + assign_semantic_tags(self) # Prepare registry of network elements allElements = {} diff --git a/tests/formats/opendrive/conftest.py b/tests/formats/opendrive/conftest.py new file mode 100644 index 000000000..fd90bd4ee --- /dev/null +++ b/tests/formats/opendrive/conftest.py @@ -0,0 +1,145 @@ +import xml.etree.ElementTree as ET + +from scenic.formats.opendrive.xodr_parser import Junction, RoadMap + +DEFAULT_PLAN_VIEW = """ + + + + """ + + +def write_xodr_with_type(tmp_path, plan_view=DEFAULT_PLAN_VIEW, road_extras=""): + path = tmp_path / "test.xodr" + path.write_text( + f""" + + + {road_extras} + {plan_view} + + + +
+ +
+ + + + + + + + + +""" + ) + return path + + +def write_xodr_multi_lane( + tmp_path, lanes_xml, road_extras="", junction_xml="", junction_id="-1" +): + path = tmp_path / "test.xodr" + path.write_text( + f""" + + {junction_xml} + + {road_extras} + {DEFAULT_PLAN_VIEW} + + + +
+ +
+ +{lanes_xml} + + + + + +""" + ) + return path + + +def write_xodr_lane_speeds(tmp_path, lanes_xml, road_extras=""): + path = tmp_path / "test.xodr" + path.write_text( + f""" + + + {road_extras} + {DEFAULT_PLAN_VIEW} + + + +
+ +
+ +{lanes_xml} + + + + + +""" + ) + return path + + +def parse_scenic_network( + tmp_path, + road_extras="", + plan_view=DEFAULT_PLAN_VIEW, + *, + lanes_xml=None, + junction_xml="", + junction_id="-1", + junction_type=None, + junction_name=None, +): + if lanes_xml is None: + path = write_xodr_with_type(tmp_path, plan_view, road_extras=road_extras) + else: + path = write_xodr_multi_lane( + tmp_path, + lanes_xml, + road_extras=road_extras, + junction_xml=junction_xml, + junction_id=junction_id, + ) + road_map = RoadMap() + road_map.parse(path) + road_map.calculate_geometry(num=5, calc_intersect=True) + if junction_id != "-1": + jid = int(junction_id) + if jid not in road_map.junctions: + road_map.junctions[jid] = Junction( + jid, junction_name, junction_type or "default" + ) + return road_map.toScenicNetwork() + + +def scenic_road(network, road_id=7): + return next(road for road in network.allRoads if road.id == road_id) + + +def type_tags_from_road_extras(road_extras): + root = ET.fromstring(f"{road_extras}") + return frozenset(elem.get("type") for elem in root.iter("type") if elem.get("type")) + + +def assert_road_level_tags_propagated(road): + """Road-level tags should match lane groups and road sections only.""" + expected = road.tags + if road.forwardLanes is not None: + assert road.forwardLanes.tags == expected + if road.backwardLanes is not None: + assert road.backwardLanes.tags == expected + for section in road.sections: + assert section.tags == expected diff --git a/tests/formats/opendrive/test_speeds.py b/tests/formats/opendrive/test_speeds.py new file mode 100644 index 000000000..2f1f50ec1 --- /dev/null +++ b/tests/formats/opendrive/test_speeds.py @@ -0,0 +1,257 @@ +import xml.etree.ElementTree as ET + +import pytest + +from scenic.formats.opendrive.xodr_parser import ( + OpenDriveWarning, + RoadMap, + effective_speed_limit_ranges, + speed_limit_ranges_from_lane_records, + speed_limit_ranges_from_type_records, + speed_limits_for_s_interval, + speed_to_mps, +) + +from .conftest import parse_scenic_network, scenic_road, write_xodr_lane_speeds + + +def test_speed_limit_ranges_from_type_records(): + records = [ + (0.0, "town", 45.0), + (7.0, "town", 30.0), + (10.0, "town", 45.0), + (15.0, "town", 45.0), + ] + assert speed_limit_ranges_from_type_records(records, 30.0) == [ + (0.0, 7.0, 45.0), + (7.0, 10.0, 30.0), + (10.0, 30.0, 45.0), + ] + + +def test_speed_limit_ranges_carry_forward_missing_speed(): + records = [ + (0.0, "town", 45.0), + (10.0, "motorway", None), + (20.0, "motorway", 30.0), + ] + assert speed_limit_ranges_from_type_records(records, 30.0) == [ + (0.0, 10.0, 45.0), + (10.0, 20.0, None), + (20.0, 30.0, 30.0), + ] + + +def test_speed_limits_for_s_interval_single_and_multiple(): + ranges = [ + (0.0, 7.0, 45.0), + (7.0, 10.0, 30.0), + (10.0, 30.0, 45.0), + ] + overlapping = speed_limits_for_s_interval(ranges, 10.0, 20.0) + assert overlapping == frozenset({45.0}) + assert min(overlapping) == pytest.approx(45.0) + + overlapping = speed_limits_for_s_interval(ranges, 0.0, 10.0) + assert overlapping == frozenset({45.0, 30.0}) + assert min(overlapping) == pytest.approx(30.0) + + +def test_speed_limit_ranges_from_lane_records(): + records = [(0.0, 20.0), (5.0, 30.0)] + assert speed_limit_ranges_from_lane_records(records, 10.0) == [ + (0.0, 5.0, 20.0), + (5.0, 10.0, 30.0), + ] + + +def test_speed_limit_ranges_from_lane_records_carry_forward(): + records = [(0.0, 20.0), (5.0, None), (8.0, 30.0)] + assert speed_limit_ranges_from_lane_records(records, 10.0) == [ + (0.0, 5.0, 20.0), + (5.0, 8.0, None), + (8.0, 10.0, 30.0), + ] + + +def test_effective_speed_limit_ranges_lane_overrides_road(): + road_ranges = [(0.0, 10.0, 50.0)] + lane_ranges = [(0.0, 10.0, 80.0)] + assert effective_speed_limit_ranges(road_ranges, lane_ranges, 0.0, 10.0) == [ + (0.0, 10.0, 80.0), + ] + + +def test_effective_speed_limit_ranges_multiple_lane_speeds(): + road_ranges = [(0.0, 10.0, 50.0)] + lane_ranges = [(0.0, 5.0, 20.0), (5.0, 10.0, 30.0)] + assert effective_speed_limit_ranges(road_ranges, lane_ranges, 0.0, 10.0) == [ + (0.0, 5.0, 20.0), + (5.0, 10.0, 30.0), + ] + + +def test_effective_speed_limit_ranges_lane_speed_below_road(): + road_ranges = [(0.0, 10.0, 50.0)] + lane_ranges = [(0.0, 10.0, 30.0)] + assert effective_speed_limit_ranges(road_ranges, lane_ranges, 0.0, 10.0) == [ + (0.0, 10.0, 30.0), + ] + + +def test_effective_speed_limit_ranges_road_only_uses_road_ranges(): + road_ranges = [(0.0, 5.0, 50.0), (5.0, 10.0, 30.0)] + assert effective_speed_limit_ranges(road_ranges, [], 0.0, 10.0) == [ + (0.0, 5.0, 50.0), + (5.0, 10.0, 30.0), + ] + + +def test_parse_lane_speed_records(tmp_path): + lanes_xml = """ + + + + """ + path = write_xodr_lane_speeds(tmp_path, lanes_xml) + road_map = RoadMap() + road_map.parse(path) + lane = road_map.roads[7].lane_secs[0].get_lane(-1) + assert lane.speed_records == [(0.0, 20.0), (5.0, 30.0)] + assert speed_limit_ranges_from_lane_records(lane.speed_records, 10.0) == [ + (0.0, 5.0, 20.0), + (5.0, 10.0, 30.0), + ] + + +def test_lane_speed_ranges_on_scenic_lane_section(tmp_path): + lanes_xml = """ + + + + """ + network = parse_scenic_network(tmp_path, lanes_xml=lanes_xml) + lane_section = scenic_road(network).sections[0].lanes[0] + + assert lane_section.speedLimit == pytest.approx(20.0) + assert lane_section.speedLimitRanges == ( + (0.0, 5.0, 20.0), + (5.0, 20.0, 30.0), + ) + assert lane_section.speedLimitAt(2.0) == pytest.approx(20.0) + assert lane_section.speedLimitAt(7.0) == pytest.approx(30.0) + + # Network.speedLimitAt projects onto the section centerline and looks up + # the matching range. + slow_point = lane_section.centerline.pointAlongBy(2.0) + fast_point = lane_section.centerline.pointAlongBy(7.0) + assert network.speedLimitAt(slow_point) == pytest.approx(20.0) + assert network.speedLimitAt(fast_point) == pytest.approx(30.0) + assert network.speedLimitAt((1000.0, 1000.0)) is None + + +def test_network_speed_limit_at_uniform_section(tmp_path): + road_extras = '' + network = parse_scenic_network(tmp_path, road_extras=road_extras) + road = scenic_road(network) + point = road.lanes[0].sections[0].centerline.pointAlongBy(5.0) + assert network.speedLimitAt(point) == pytest.approx(50 / 3.6) + + +def test_no_limit_speed_sets_ranges_with_none(tmp_path): + road_extras = '' + network = parse_scenic_network(tmp_path, road_extras=road_extras) + lane_section = scenic_road(network).lanes[0].sections[0] + assert lane_section.speedLimit is None + assert lane_section.speedLimitRanges == ((0.0, 20.0, None),) + point = lane_section.centerline.pointAlongBy(5.0) + assert network.speedLimitAt(point) is None + + +def test_road_speed_limits_vary_by_lane_section(): + speed_ranges = speed_limit_ranges_from_type_records( + [ + (0.0, "town", 45.0), + (7.0, "town", 30.0), + (10.0, "town", 45.0), + (15.0, "town", 45.0), + (20.0, "town", 45.0), + (30.0, "town", 45.0), + ], + 30.0, + ) + section_intervals = [(0.0, 10.0), (10.0, 20.0), (20.0, 30.0)] + section_speeds = [ + speed_limits_for_s_interval(speed_ranges, s_start, s_end) + for s_start, s_end in section_intervals + ] + + assert section_speeds[0] == frozenset({45.0, 30.0}) + assert section_speeds[1] == frozenset({45.0}) + assert section_speeds[2] == frozenset({45.0}) + assert min(min(speeds) for speeds in section_speeds) == pytest.approx(30.0) + + +def test_lane_speed_limit_overrides_when_higher(tmp_path): + lanes_xml = """ + + + + + + + + + + """ + road_extras = '' + network = parse_scenic_network(tmp_path, lanes_xml=lanes_xml, road_extras=road_extras) + road = scenic_road(network) + + road_limit = 50 / 3.6 + fast_limit = 80 / 3.6 + assert road.speedLimit == pytest.approx(road_limit) + limits_by_od_id = { + section.openDriveID: section.speedLimit for section in road.sections[0].lanes + } + assert limits_by_od_id[-1] == pytest.approx(road_limit) + assert limits_by_od_id[-2] == pytest.approx(fast_limit) + assert limits_by_od_id[-3] == pytest.approx(40 / 3.6) + + +def _speed_elem(max_value, unit=None): + attrs = f'max="{max_value}"' + if unit is not None: + attrs += f' unit="{unit}"' + return ET.fromstring(f"") + + +def test_speed_to_mps_unit_conversions(): + assert speed_to_mps(_speed_elem(36, "km/h")) == pytest.approx(10.0) + assert speed_to_mps(_speed_elem(10, "m/s")) == pytest.approx(10.0) + assert speed_to_mps(_speed_elem(100, "mph")) == pytest.approx(44.704) + # A missing unit defaults to m/s. + assert speed_to_mps(_speed_elem(15)) == pytest.approx(15.0) + + +def test_speed_to_mps_unlimited_returns_none(): + assert speed_to_mps(_speed_elem("no limit", "km/h")) is None + assert speed_to_mps(_speed_elem("undefined")) is None + + +def test_speed_to_mps_rejects_unknown_unit(): + with pytest.raises(ValueError, match="unsupported speed unit"): + speed_to_mps(_speed_elem(50, "furlongs/fortnight")) + + +def test_section_spanning_multiple_speeds_warns(tmp_path): + road_extras = ( + '' + '' + ) + # The single 20 m lane section [0,20) straddles both the 50 and 30 km/h limits. + lanes_xml = """ + + """ + with pytest.warns(OpenDriveWarning, match="spans multiple speed limits"): + parse_scenic_network(tmp_path, lanes_xml=lanes_xml, road_extras=road_extras) diff --git a/tests/formats/opendrive/test_tags.py b/tests/formats/opendrive/test_tags.py new file mode 100644 index 000000000..dcd1d21d3 --- /dev/null +++ b/tests/formats/opendrive/test_tags.py @@ -0,0 +1,112 @@ +import pytest + +from scenic.formats.opendrive.xodr_parser import Junction, assign_semantic_tags + +from .conftest import ( + assert_road_level_tags_propagated, + parse_scenic_network, + scenic_road, + type_tags_from_road_extras, +) + + +def test_map_tags_propagate_to_lane_hierarchy(tmp_path): + road_extras = '' + network = parse_scenic_network(tmp_path, road_extras=road_extras) + road = network.roads[0] + assert road.tags == type_tags_from_road_extras(road_extras) + assert_road_level_tags_propagated(road) + assert road.lanes[0].tags == frozenset({"driving"}) + assert road.lanes[0].sections[0].tags == road.lanes[0].tags + + +def test_map_tags_from_all_type_segments_propagate(tmp_path): + road_extras = ( + '' + '' + ) + network = parse_scenic_network(tmp_path, road_extras=road_extras) + road = network.roads[0] + assert road.tags == type_tags_from_road_extras(road_extras) + assert_road_level_tags_propagated(road) + assert road.lanes[0].tags == frozenset({"driving"}) + + +def test_lane_type_tags_are_lane_specific(tmp_path): + lanes_xml = """ + + + + + """ + road_extras = '' + network = parse_scenic_network(tmp_path, lanes_xml=lanes_xml, road_extras=road_extras) + road = scenic_road(network) + + tags_by_od_id = { + section.openDriveID: section.tags for section in road.sections[0].lanes + } + assert tags_by_od_id[-1] == frozenset({"driving"}) + assert tags_by_od_id[-2] == frozenset({"onRamp"}) + assert "onRamp" not in tags_by_od_id[-1] + assert "driving" not in tags_by_od_id[-2] + assert road.tags == frozenset({"motorway"}) + + +def test_junction_ramp_tags_apply_only_to_ramp_lanes(tmp_path): + lanes_xml = """ + + + + + """ + road_extras = '' + network = parse_scenic_network( + tmp_path, + lanes_xml=lanes_xml, + road_extras=road_extras, + junction_id="5", + junction_type="onramp", + junction_name="J5", + ) + road = scenic_road(network) + + assert road.tags == frozenset({"motorway", "onramp"}) + tags_by_od_id = { + section.openDriveID: section.tags for section in road.sections[0].lanes + } + assert tags_by_od_id[-1] == frozenset({"driving"}) + assert tags_by_od_id[-2] == frozenset({"onRamp"}) + assert "onramp" not in tags_by_od_id[-1] + assert "onramp" not in tags_by_od_id[-2] + assert "motorway" not in tags_by_od_id[-1] + + +def test_junction_tags_from_type(): + assert Junction(1, "Roundabout A", "roundabout").tags == frozenset({"roundabout"}) + # The default junction type carries no semantic meaning and is dropped. + assert Junction(2, None, "default").tags == frozenset() + # type_ defaults to "default"; junction names are not semantic tags. + assert Junction(3, "J3").tags == frozenset() + + +class _FakeRoad: + def __init__(self, junction): + self.junction = junction + self.extra_tags = frozenset() + + +class _FakeRoadMap: + def __init__(self, roads, junctions): + self.roads = roads + self.junctions = junctions + + +def test_assign_semantic_tags_tags_only_roads_in_junction(): + road_map = _FakeRoadMap( + roads={10: _FakeRoad(junction=5), 11: _FakeRoad(junction=None)}, + junctions={5: Junction(5, "J5", "roundabout")}, + ) + assign_semantic_tags(road_map) + assert road_map.roads[10].extra_tags == frozenset({"roundabout"}) + assert road_map.roads[11].extra_tags == frozenset()