Add regression tests for LiDAR hits, CV/FastBEV perception, and control dual-write

test/c10_perception/test_c15_constant_velocity_prediction.py

@@ -0,0 +1,50 @@
+"""Regression tests for ConstantVelocityPrediction (default predictor)."""
+
+from __future__ import annotations
+
+import math
+
+import numpy as np
+import pytest
+
+from avlite.c10_perception.c11_perception_model import AgentState, EgoState, PerceptionModel
+from avlite.c10_perception.c15_perception_algs import ConstantVelocityPrediction
+from avlite.c10_perception.c19_settings import PerceptionSettings
+
+
+def test_constant_velocity_predict_requires_perception_model():
+    with pytest.raises(ValueError, match="perception_model is required"):
+        ConstantVelocityPrediction().predict(perception_model=None)
+
+
+def test_constant_velocity_predict_empty_agents_clears_trajectories():
+    pm = PerceptionModel(ego_vehicle=EgoState(), agent_vehicles=[])
+    out = ConstantVelocityPrediction().predict(pm)
+    assert out.prediction is not None
+    assert out.prediction.trajectories == {}
+    assert out.prediction.predict_delta_t == PerceptionSettings.c11_predict_delta_t
+
+
+def test_constant_velocity_predict_extrapolates_along_heading():
+    dt = PerceptionSettings.c11_predict_delta_t
+    horizon = PerceptionSettings.c15_prediction_horizon
+    n_steps = max(1, int(round(horizon / dt)))
+
+    a_east = AgentState(x=0.0, y=0.0, theta=0.0, velocity=10.0, agent_id=7)
+    a_north = AgentState(x=1.0, y=2.0, theta=math.pi / 2, velocity=2.0, agent_id=8)
+    pm = PerceptionModel(ego_vehicle=EgoState(), agent_vehicles=[a_east, a_north])
+
+    out = ConstantVelocityPrediction().predict(pm)
+    trajs = out.prediction.trajectories
+    assert sorted(trajs) == [7, 8]
+    assert out.prediction.predict_delta_t == dt
+    assert trajs[7].shape == (n_steps, 2)
+
+    expected_east = np.array(
+        [[10.0 * (t + 1) * dt, 0.0] for t in range(n_steps)], dtype=float
+    )
+    expected_north = np.array(
+        [[1.0, 2.0 + 2.0 * (t + 1) * dt] for t in range(n_steps)], dtype=float
+    )
+    np.testing.assert_allclose(trajs[7], expected_east, atol=1e-9)
+    np.testing.assert_allclose(trajs[8], expected_north, atol=1e-9)

test/c10_perception/test_c15_fast_bev_detection.py

@@ -0,0 +1,115 @@
+"""Regression tests for FastBEVLidarDetection segmentation and MBR fitting."""
+
+from __future__ import annotations
+
+import numpy as np
+import pytest
+
+from avlite.c10_perception.c11_perception_model import EgoState, PerceptionModel
+from avlite.c10_perception.c15_perception_algs import FastBEVLidarDetection
+
+
+def _rectangle_cluster(cx: float, cy: float, length: float = 2.0, width: float = 2.0, n: int = 8):
+    """Axis-aligned rectangle outline in contiguous scan order (CCW from near face)."""
+    x0, x1 = cx - length / 2, cx + length / 2
+    y0, y1 = cy - width / 2, cy + width / 2
+    pts = []
+    for y in np.linspace(y0, y1, n):
+        pts.append((x0, y))
+    for x in np.linspace(x0, x1, n)[1:]:
+        pts.append((x, y1))
+    for y in np.linspace(y1, y0, n)[1:]:
+        pts.append((x1, y))
+    for x in np.linspace(x1, x0, n)[1:-1]:
+        pts.append((x, y0))
+    return np.asarray(pts, dtype=float)
+
+
+def test_fast_bev_detect_requires_perception_model():
+    with pytest.raises(ValueError, match="perception_model is required"):
+        FastBEVLidarDetection().detect(perception_model=None, lidar_data=np.zeros((2, 2)))
+
+
+def test_fast_bev_empty_or_none_lidar_clears_clusters():
+    det = FastBEVLidarDetection()
+    pm = PerceptionModel(ego_vehicle=EgoState())
+    pm.detection_clusters = np.zeros((3, 2))
+    pm.agent_vehicles = []
+
+    out = det.detect(pm, lidar_data=None)
+    assert out.detection_clusters is None
+
+    pm.detection_clusters = np.zeros((3, 2))
+    out = det.detect(pm, lidar_data=np.empty((0, 2)))
+    assert out.detection_clusters is None
+
+
+def test_fast_bev_gap_splits_clusters_and_fits_boxes():
+    det = FastBEVLidarDetection(mu=0.5, delta_min=1.0, delta_max=6.0)
+    c1 = _rectangle_cluster(9.0, 0.0, length=2.0, width=2.0)
+    c2 = _rectangle_cluster(21.0, 0.0, length=2.0, width=2.0)
+    assert np.linalg.norm(c2[0] - c1[-1]) > det._mu
+
+    pm = PerceptionModel(ego_vehicle=EgoState())
+    det.detect(pm, lidar_data=np.vstack([c1, c2]))
+
+    assert len(pm.agent_vehicles) == 2
+    xs = sorted(a.x for a in pm.agent_vehicles)
+    np.testing.assert_allclose(xs, [9.0, 21.0], atol=0.15)
+    for agent in pm.agent_vehicles:
+        assert agent.length == pytest.approx(2.0, abs=0.2)
+        assert agent.width == pytest.approx(2.0, abs=0.2)
+
+
+def test_fast_bev_drops_clusters_outside_diagonal_range():
+    det = FastBEVLidarDetection(mu=0.5, delta_min=1.0, delta_max=6.0)
+    # Tiny cluster: axis-aligned span diagonal << delta_min.
+    tiny = np.array([[5.0, 0.0], [5.1, 0.0], [5.2, 0.05]], dtype=float)
+    diag = float(np.linalg.norm(tiny.max(axis=0) - tiny.min(axis=0)))
+    assert diag < det._delta_min
+
+    pm = PerceptionModel(ego_vehicle=EgoState())
+    det.detect(pm, lidar_data=tiny)
+    assert pm.agent_vehicles == []
+    assert pm.detection_clusters is None
+
+
+def test_fast_bev_edge_on_cluster_uses_default_box_pushed_from_ego():
+    """Collinear face collapses width → default L/W box pushed along ego heading."""
+    det = FastBEVLidarDetection(
+        mu=0.5,
+        delta_min=0.5,
+        delta_max=6.0,
+        min_length=0.5,
+        min_width=0.5,
+        default_length=4.5,
+        default_width=2.0,
+    )
+    # Vertical line at x=10 (only the near face visible).
+    line = np.array([[10.0, y] for y in np.linspace(-1.0, 1.0, 12)], dtype=float)
+    pm = PerceptionModel(ego_vehicle=EgoState(x=0.0, y=0.0, theta=0.0))
+    det.detect(pm, lidar_data=line)
+
+    assert len(pm.agent_vehicles) == 1
+    agent = pm.agent_vehicles[0]
+    assert agent.length == pytest.approx(4.5)
+    assert agent.width == pytest.approx(2.0)
+    # Centre pushed away from ego by length/2 along +x.
+    assert agent.x == pytest.approx(10.0 + 4.5 / 2.0, abs=1e-6)
+    assert agent.y == pytest.approx(0.0, abs=1e-6)
+
+
+def test_fast_bev_3d_z_band_filters_points():
+    det = FastBEVLidarDetection(z_min=-1.5, z_max=0.5, mu=0.5, delta_min=1.0, delta_max=6.0)
+    cluster = _rectangle_cluster(8.0, 0.0, length=2.0, width=2.0)
+    # All points outside the z-band → cleared.
+    lidar_high = np.column_stack([cluster, np.full(len(cluster), 2.0)])
+    pm = PerceptionModel(ego_vehicle=EgoState())
+    det.detect(pm, lidar_data=lidar_high)
+    assert pm.agent_vehicles == []
+    assert pm.detection_clusters is None
+
+    lidar_ok = np.column_stack([cluster, np.full(len(cluster), 0.0)])
+    det.detect(pm, lidar_data=lidar_ok)
+    assert len(pm.agent_vehicles) == 1
+    assert pm.agent_vehicles[0].x == pytest.approx(8.0, abs=0.15)

test/c40_execution/test_c46_basic_sim_lidar.py

@@ -0,0 +1,131 @@
+"""Regression tests for BasicSim LiDAR raycasting hits."""
+
+from __future__ import annotations
+
+import numpy as np
+import pytest
+
+from avlite.c10_perception.c11_perception_model import (
+    AgentState,
+    EgoState,
+    PerceptionModel,
+    RaceMap,
+)
+from avlite.c40_execution.c46_basic_sim import BasicSim
+from avlite.c40_execution.c49_settings import ExecutionSettingsSchema
+
+
+def _sim_with_settings(**lidar_kwargs) -> BasicSim:
+    setting = ExecutionSettingsSchema()
+    for key, value in lidar_kwargs.items():
+        setattr(setting, key, value)
+    ego = EgoState(x=0.0, y=0.0, theta=0.0, velocity=0.0)
+    return BasicSim(ego_state=ego, pm=PerceptionModel(ego_vehicle=ego), setting=setting)
+
+
+def test_lidar_empty_scene_returns_empty_cloud():
+    sim = _sim_with_settings(
+        c46_lidar_num_beams=36,
+        c46_lidar_fov_deg=360.0,
+        c46_lidar_range=50.0,
+    )
+    pts = sim._simulate_lidar_2d()
+    cloud = sim.get_lidar_data()
+    assert pts.shape == (0, 2)
+    assert cloud.shape == (0, 4)
+
+
+def test_lidar_hits_nearest_boundary_wall_ahead():
+    """Forward beams should hit a wall at x=10, not a farther wall behind."""
+    left = np.array([[10.0, -5.0], [10.0, 5.0]])
+    right = np.array([[-1.0, -5.0], [-1.0, 5.0]])
+    race = RaceMap(source_path="synthetic", left_bound=left, right_bound=right)
+    setting = ExecutionSettingsSchema(
+        c46_lidar_num_beams=5,
+        c46_lidar_fov_deg=20.0,
+        c46_lidar_range=50.0,
+    )
+    ego = EgoState(x=0.0, y=0.0, theta=0.0)
+    sim = BasicSim(
+        ego_state=ego,
+        pm=PerceptionModel(ego_vehicle=ego),
+        map=race,
+        setting=setting,
+    )
+
+    pts = sim._simulate_lidar_2d()
+    cloud = sim.get_lidar_data()
+
+    assert len(pts) == 5
+    np.testing.assert_allclose(pts[:, 0], 10.0, atol=1e-6)
+    assert np.all(np.abs(pts[:, 1]) < 2.0)
+    assert cloud.shape == (5, 4)
+    np.testing.assert_allclose(cloud[:, :2], pts, atol=1e-5)
+    np.testing.assert_allclose(cloud[:, 2:], 0.0)
+
+
+def test_lidar_hits_agent_bounding_box_face():
+    """Agent BB edges are raycast; the near face of an ahead agent is at x=13."""
+    setting = ExecutionSettingsSchema(
+        c46_lidar_num_beams=9,
+        c46_lidar_fov_deg=40.0,
+        c46_lidar_range=50.0,
+    )
+    ego = EgoState(x=0.0, y=0.0, theta=0.0)
+    pm = PerceptionModel(ego_vehicle=ego)
+    pm.agent_vehicles = [
+        AgentState(x=15.0, y=0.0, theta=0.0, length=4.0, width=2.0, agent_id=1)
+    ]
+    sim = BasicSim(ego_state=ego, pm=pm, map=None, setting=setting)
+
+    pts = sim._simulate_lidar_2d()
+    assert len(pts) >= 1
+    # Near face of axis-aligned 4 m box centred at x=15 is x=13.
+    assert float(pts[:, 0].min()) == pytest.approx(13.0, abs=1e-6)
+
+
+def test_lidar_misses_obstacles_beyond_range():
+    left = np.array([[10.0, -5.0], [10.0, 5.0]])
+    race = RaceMap(
+        source_path="synthetic",
+        left_bound=left,
+        right_bound=np.array([[10.0, -5.0], [10.0, 5.0]]),
+    )
+    setting = ExecutionSettingsSchema(
+        c46_lidar_num_beams=5,
+        c46_lidar_fov_deg=20.0,
+        c46_lidar_range=5.0,
+    )
+    ego = EgoState(x=0.0, y=0.0, theta=0.0)
+    sim = BasicSim(
+        ego_state=ego,
+        pm=PerceptionModel(ego_vehicle=ego),
+        map=race,
+        setting=setting,
+    )
+    assert sim._simulate_lidar_2d().shape == (0, 2)
+
+
+def test_lidar_prefers_nearest_of_overlapping_hits():
+    """Wall at x=10 must win over an agent whose near face is at x=13."""
+    left = np.array([[10.0, -5.0], [10.0, 5.0]])
+    race = RaceMap(
+        source_path="synthetic",
+        left_bound=left,
+        right_bound=np.empty((0, 2)),
+    )
+    setting = ExecutionSettingsSchema(
+        c46_lidar_num_beams=5,
+        c46_lidar_fov_deg=20.0,
+        c46_lidar_range=50.0,
+    )
+    ego = EgoState(x=0.0, y=0.0, theta=0.0)
+    pm = PerceptionModel(ego_vehicle=ego)
+    pm.agent_vehicles = [
+        AgentState(x=15.0, y=0.0, theta=0.0, length=4.0, width=2.0, agent_id=1)
+    ]
+    sim = BasicSim(ego_state=ego, pm=pm, map=race, setting=setting)
+
+    pts = sim._simulate_lidar_2d()
+    assert len(pts) == 5
+    np.testing.assert_allclose(pts[:, 0], 10.0, atol=1e-6)

test/plugins/test_p60_visualizer_tk/test_p61_apply_global_plan.py

@@ -1,11 +1,15 @@
-"""Tests for VisualizerApp.apply_global_plan ROS delegation."""
+"""Tests for VisualizerApp.apply_global_plan and apply_world_control."""
 
 from __future__ import annotations
 
 from unittest.mock import MagicMock
 
+import pytest
+
 from avlite import TrajectoryTracker
+from avlite.c10_perception.c11_perception_model import EgoState, PerceptionModel
 from avlite.c20_planning.c21_planning_model import GlobalPlan
+from avlite.c30_control.c31_control_model import ControlCommand
 from avlite.plugins.p60_visualizer_tk.p61_visualizer_app import VisualizerApp
 
 
@@ -46,3 +50,39 @@ def __init__(self):
     stub.local_planner.set_global_plan.assert_called_once_with(plan, ego_xy=(3.0, 4.0))
     stub.controller.set_trajectory_tracker.assert_called_once_with(plan.trajectory)
     stub.controller.reset.assert_called_once()
+
+
+def test_apply_world_control_dual_writes_plant_then_stack_pm():
+    """After world/stack ego split, Control Step/Steer must sync pm.ego_vehicle."""
+    app = VisualizerApp.__new__(VisualizerApp)
+
+    world_ego = EgoState(x=0.0, y=0.0, theta=0.0, velocity=5.0)
+    stack_ego = EgoState(x=0.0, y=0.0, theta=0.0, velocity=5.0)
+
+    class _World:
+        def __init__(self, ego):
+            self._ego = ego
+
+        def control_ego_state(self, cmd, dt):
+            del cmd
+            self._ego.x += self._ego.velocity * dt
+            self._calls = getattr(self, "_calls", 0) + 1
+
+        def get_ego_state(self):
+            return self._ego
+
+    class _Exec:
+        def __init__(self):
+            self.world = _World(world_ego)
+            self.pm = PerceptionModel(ego_vehicle=stack_ego)
+
+    stub = _Exec()
+    app.exec = stub
+    VisualizerApp.apply_world_control(app, ControlCommand(), dt=0.2)
+
+    assert stub.world._calls == 1
+    assert world_ego.x == pytest.approx(1.0)
+    assert stub.pm.ego_vehicle.x == pytest.approx(1.0)
+    assert stub.pm.ego_vehicle.velocity == pytest.approx(5.0)
+    # Identity preserved; fields copied in place.
+    assert stub.pm.ego_vehicle is stack_ego