diff --git a/assets/maps/demo/lht/01_two_lane_oneway.xodr b/assets/maps/demo/lht/01_two_lane_oneway.xodr new file mode 100644 index 000000000..ebd47a6e8 --- /dev/null +++ b/assets/maps/demo/lht/01_two_lane_oneway.xodr @@ -0,0 +1,33 @@ + + + +
+ + + + + + + + + + + + + + + + + + + + +
+ +
+
+
+
+ diff --git a/assets/maps/demo/lht/02_three_lane_speeds.xodr b/assets/maps/demo/lht/02_three_lane_speeds.xodr new file mode 100644 index 000000000..4a0a765f5 --- /dev/null +++ b/assets/maps/demo/lht/02_three_lane_speeds.xodr @@ -0,0 +1,38 @@ + + + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+
+
+
+ diff --git a/assets/maps/demo/lht/03_two_way.xodr b/assets/maps/demo/lht/03_two_way.xodr new file mode 100644 index 000000000..2c3c2535a --- /dev/null +++ b/assets/maps/demo/lht/03_two_way.xodr @@ -0,0 +1,41 @@ + + + +
+ + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + +
+
+
+ diff --git a/tests/domains/driving/test_lht_driving.py b/tests/domains/driving/test_lht_driving.py new file mode 100644 index 000000000..9dcf97cba --- /dev/null +++ b/tests/domains/driving/test_lht_driving.py @@ -0,0 +1,104 @@ +"""Driving-domain tests on left-hand traffic demo maps.""" + +import inspect +from pathlib import Path + +import pytest + +from tests.utils import compileScenic, sampleScene + +pytestmark = pytest.mark.filterwarnings( + "ignore::scenic.formats.opendrive.OpenDriveWarning" +) + +LHT = Path(__file__).resolve().parents[3] / "assets" / "maps" / "demo" / "lht" + +ALL_MAPS = ( + "01_two_lane_oneway.xodr", + "02_three_lane_speeds.xodr", + "03_two_way.xodr", +) + +template = inspect.cleandoc( + """ + param map = r'{map}' + param map_options = dict(useCache=False) + model scenic.domains.driving.model + """ +) + +basicScenario = inspect.cleandoc( + """ + lane = Uniform(*network.lanes) + ego = new Car in lane + follower = new Car on visible lane.centerline + """ +) + +fasterLaneScenario = inspect.cleandoc( + """ + candidates = [lane for lane in network.lanes if lane.sections[0]._fasterLane] + require len(candidates) > 0 + lane = Uniform(*candidates) + ego = new Car in lane + other = new Car in lane.sections[0].fasterLane.lane + """ +) + +slowerLaneScenario = inspect.cleandoc( + """ + candidates = [lane for lane in network.lanes if lane.sections[0]._slowerLane] + require len(candidates) > 0 + lane = Uniform(*candidates) + ego = new Car in lane + other = new Car in lane.sections[0].slowerLane.lane + """ +) + + +def _compile(path, code): + preamble = template.format(map=path) + return compileScenic(preamble + "\n" + code, mode2D=True) + + +def _sample_two_car_scene(name, scenario_code): + scenario = _compile(LHT / name, scenario_code) + scene = sampleScene(scenario, maxIterations=1000) + assert len(scene.objects) == 2 + ego, other = scene.objects + assert ego is scene.egoObject + return ego, other + + +@pytest.mark.parametrize("name", ALL_MAPS) +def test_lht_basic_car_placement(name): + """Ego and a second car can be placed on the same sampled lane.""" + ego, follower = _sample_two_car_scene(name, basicScenario) + + assert ego.lane is follower.lane + assert ego.laneSection.lane is follower.laneSection.lane + assert follower.lane.containsPoint(follower.position) + + +@pytest.mark.parametrize("name", ALL_MAPS) +def test_lht_faster_lane_placement(name): + """Ego sits on a slower lane; other is placed on its fasterLane neighbor.""" + ego, other = _sample_two_car_scene(name, fasterLaneScenario) + + ego_sec = ego.laneSection + assert ego_sec._fasterLane is not None + assert other.lane is ego_sec.fasterLane.lane + assert other.lane is not ego.lane + assert other.laneSection.lane is ego_sec.fasterLane.lane + + +@pytest.mark.parametrize("name", ALL_MAPS) +def test_lht_slower_lane_placement(name): + """Ego sits on a faster lane; other is placed on its slowerLane neighbor.""" + ego, other = _sample_two_car_scene(name, slowerLaneScenario) + + ego_sec = ego.laneSection + assert ego_sec._slowerLane is not None + assert other.lane is ego_sec.slowerLane.lane + assert other.lane is not ego.lane + assert other.laneSection.lane is ego_sec.slowerLane.lane diff --git a/tests/formats/opendrive/test_lht.py b/tests/formats/opendrive/test_lht.py new file mode 100644 index 000000000..38c686b85 --- /dev/null +++ b/tests/formats/opendrive/test_lht.py @@ -0,0 +1,163 @@ +"""Left-hand traffic OpenDRIVE maps — centerlines and faster/slower lanes.""" + +from pathlib import Path + +import pytest + +from scenic.core.distributions import RejectionException +from scenic.domains.driving.roads import Network + +pytestmark = pytest.mark.filterwarnings( + "ignore::scenic.formats.opendrive.OpenDriveWarning" +) + +DEMO = Path(__file__).resolve().parents[3] / "assets" / "maps" / "demo" / "lht" + +ALL_MAPS = ( + "01_two_lane_oneway.xodr", + "02_three_lane_speeds.xodr", + "03_two_way.xodr", +) + + +def load(name): + return Network.fromFile(DEMO / name, useCache=False) + + +def section_by_id(network, od_id): + for road in network.roads: + for sec in road.sections: + if od_id in sec.lanesByOpenDriveID: + return sec.lanesByOpenDriveID[od_id] + raise AssertionError(f"no lane section with OpenDRIVE id {od_id}") + + +def centerline_delta(lane): + pts = lane.centerline.points + return pts[-1][0] - pts[0][0], pts[-1][1] - pts[0][1] + + +def carriageway_groups(road): + if road.forwardLanes: + yield road.forwardLanes + if road.backwardLanes: + yield road.backwardLanes + + +def assert_faster_slower(network, sec, *, faster_id, slower_id): + faster = section_by_id(network, faster_id) if faster_id else None + slower = section_by_id(network, slower_id) if slower_id else None + assert sec._fasterLane is faster + assert sec._slowerLane is slower + if faster_id is None: + with pytest.raises(RejectionException): + _ = sec.fasterLane + else: + assert sec.fasterLane is faster + if slower_id is None: + with pytest.raises(RejectionException): + _ = sec.slowerLane + else: + assert sec.slowerLane is slower + + +@pytest.mark.parametrize( + "name, n_lanes, oneway", + [ + ("01_two_lane_oneway.xodr", 2, True), + ("02_three_lane_speeds.xodr", 3, True), + ("03_two_way.xodr", 4, False), + ], +) +def test_lht_network_structure(name, n_lanes, oneway): + network = load(name) + road = network.roads[0] + + assert len(network.roads) == 1 + assert len(network.lanes) == n_lanes + assert road.is1Way is oneway + + for lane in network.lanes: + assert not lane.polygon.is_empty + assert lane.polygon.is_valid + for sec in lane.sections: + assert sec.containsRegion(sec.centerline, tolerance=0.5) + assert sec.containsRegion(sec.leftEdge, tolerance=0.5) + assert sec.containsRegion(sec.rightEdge, tolerance=0.5) + + +@pytest.mark.parametrize("name", ALL_MAPS) +def test_centerlines_follow_traffic_flow(name): + """Centerlines point with traffic; orientation matches flow at lane midpoints.""" + road = load(name).roads[0] + + for group in carriageway_groups(road): + deltas = [centerline_delta(lane) for lane in group.lanes] + signs = [1 if dx > 0 else -1 if dx < 0 else 0 for dx, _ in deltas] + assert 0 not in signs, f"{name}: zero-length centerline" + assert len(set(signs)) == 1, f"{name}: mixed centerline directions {deltas}" + + for lane in group.lanes: + pt = lane.centerline.pointAlongBy(0.5, normalized=True) + dirs = road.network.nominalDirectionsAt(pt) + assert pytest.approx(lane.orientation[pt]) in dirs + assert lane.containsPoint(pt) + + if not road.is1Way: + fwd = centerline_delta(road.forwardLanes.lanes[0])[0] + bwd = centerline_delta(road.backwardLanes.lanes[0])[0] + assert fwd * bwd < 0 + + +def test_two_lane_faster_slower_toward_median(): + network = load("01_two_lane_oneway.xodr") + assert_faster_slower(network, section_by_id(network, 2), faster_id=1, slower_id=None) + assert_faster_slower(network, section_by_id(network, 1), faster_id=None, slower_id=2) + + +def test_three_lane_faster_slower_chain(): + network = load("02_three_lane_speeds.xodr") + assert_faster_slower(network, section_by_id(network, 3), faster_id=2, slower_id=None) + assert_faster_slower(network, section_by_id(network, 2), faster_id=1, slower_id=3) + assert_faster_slower(network, section_by_id(network, 1), faster_id=None, slower_id=2) + + +def test_two_way_faster_slower_each_carriageway(): + network = load("03_two_way.xodr") + assert_faster_slower(network, section_by_id(network, 2), faster_id=1, slower_id=None) + assert_faster_slower(network, section_by_id(network, 1), faster_id=None, slower_id=2) + assert_faster_slower( + network, section_by_id(network, -2), faster_id=-1, slower_id=None + ) + assert_faster_slower( + network, section_by_id(network, -1), faster_id=None, slower_id=-2 + ) + + +@pytest.mark.parametrize("name", ALL_MAPS) +def test_lane_adjacency_helpers(name): + """shiftedBy, faster/slower neighbors, and same-direction constraints.""" + network = load(name) + for road in network.roads: + for sec in road.sections: + for lane_sec in sec.lanes: + neighbors = {lane_sec._laneToLeft, lane_sec._laneToRight} - {None} + if lane_sec._laneToLeft: + assert lane_sec.shiftedBy(1) is lane_sec._laneToLeft + if lane_sec._laneToLeft.isForward == lane_sec.isForward: + assert lane_sec._laneToLeft.shiftedBy(-1) is lane_sec + if lane_sec._laneToRight: + assert lane_sec.shiftedBy(-1) is lane_sec._laneToRight + if lane_sec._laneToRight.isForward == lane_sec.isForward: + assert lane_sec._laneToRight.shiftedBy(1) is lane_sec + if lane_sec._fasterLane: + assert lane_sec._fasterLane in neighbors + assert lane_sec._fasterLane.isForward == lane_sec.isForward + if lane_sec._slowerLane: + assert lane_sec._slowerLane in neighbors + assert lane_sec._slowerLane.isForward == lane_sec.isForward + + if name == "03_two_way.xodr": + road = network.roads[0] + assert road.forwardLanes._opposite is road.backwardLanes + assert road.backwardLanes.opposite is road.forwardLanes