Add regression tests for LidarLocalization ICP and BasicSim kinematics

test/c10_perception/test_c11_state_geometry.py

@@ -0,0 +1,47 @@
+"""Regression tests for State bounding-box corner geometry."""
+
+import math
+
+import numpy as np
+import pytest
+
+from avlite.c10_perception.c11_perception_model import State
+
+
+def test_axis_aligned_bb_corners():
+    state = State(x=10.0, y=-2.0, theta=0.0, length=4.0, width=2.0)
+    corners = state.get_bb_corners()
+    expected = np.array(
+        [
+            [8.0, -3.0],
+            [12.0, -3.0],
+            [12.0, -1.0],
+            [8.0, -1.0],
+        ]
+    )
+    np.testing.assert_allclose(corners, expected, atol=1e-9)
+    assert state.get_bb_polygon().contains(state.get_bb_polygon().centroid)
+    # Explicit center containment via shapely
+    from shapely.geometry import Point
+
+    assert state.get_bb_polygon().contains(Point(state.x, state.y))
+
+
+def test_yaw_90_swaps_length_and_width_axes():
+    state = State(x=0.0, y=0.0, theta=math.pi / 2, length=4.0, width=2.0)
+    corners = state.get_bb_corners()
+    # Body (cx, cy) → world (-cy, cx) at θ=π/2.
+    expected = np.array(
+        [
+            [1.0, -2.0],
+            [1.0, 2.0],
+            [-1.0, 2.0],
+            [-1.0, -2.0],
+        ]
+    )
+    np.testing.assert_allclose(corners, expected, atol=1e-9)
+    xs, ys = corners[:, 0], corners[:, 1]
+    assert xs.min() == pytest.approx(-1.0)
+    assert xs.max() == pytest.approx(1.0)
+    assert ys.min() == pytest.approx(-2.0)
+    assert ys.max() == pytest.approx(2.0)

test/c10_perception/test_c16_lidar_localization.py

@@ -0,0 +1,116 @@
+"""Regression tests for LidarLocalization ICP scan-to-map pose updates."""
+
+import math
+
+import numpy as np
+import pytest
+
+from avlite.c10_perception.c11_perception_model import EgoState, PerceptionModel
+from avlite.c10_perception.c16_localization_algs import LidarLocalization
+from avlite.c10_perception.c19_settings import PerceptionSettingsSchema
+from avlite.c50_common.c52_world_sensor_datatypes import SensorFrame
+
+
+def _loc(ego: EgoState | None = None) -> tuple[LidarLocalization, PerceptionModel]:
+    ego = ego or EgoState(x=0.0, y=0.0, theta=0.0, velocity=0.0)
+    pm = PerceptionModel(ego_vehicle=ego)
+    setting = PerceptionSettingsSchema()
+    return LidarLocalization(pm, setting=setting), pm
+
+
+def _asymmetric_map(n: int = 40, seed: int = 0) -> np.ndarray:
+    """Non-collinear cloud so translation and yaw are uniquely recoverable."""
+    rng = np.random.default_rng(seed)
+    return rng.normal(size=(n, 2)) * np.array([3.0, 2.0]) + np.array([8.0, 1.0])
+
+
+def test_first_scan_seeds_map_without_mutating_ego():
+    ego = EgoState(x=1.5, y=-0.25, theta=0.1, velocity=0.0)
+    loc, _ = _loc(ego)
+    scan = _asymmetric_map()
+
+    loc.localize(sensors=SensorFrame(lidar=scan))
+
+    assert loc._map is not None
+    np.testing.assert_allclose(loc._map, scan)
+    assert loc._x == pytest.approx(1.5)
+    assert loc._y == pytest.approx(-0.25)
+    assert loc._theta == pytest.approx(0.1)
+    assert ego.x == pytest.approx(1.5)
+    assert ego.y == pytest.approx(-0.25)
+    assert ego.theta == pytest.approx(0.1)
+
+
+def test_reset_clears_map_and_pose_estimate():
+    loc, _ = _loc()
+    loc.localize(sensors=SensorFrame(lidar=_asymmetric_map()))
+    assert loc._map is not None
+
+    loc.reset()
+
+    assert loc._map is None
+    assert loc._x is None
+    assert loc._y is None
+    assert loc._theta is None
+
+
+def test_pure_translation_recovers_ego_xy():
+    """Body-frame scan = map − (dx, dy) recovers ego translation via ICP."""
+    dx, dy = 0.4, -0.25
+    ego = EgoState(x=0.0, y=0.0, theta=0.0, velocity=0.0)
+    loc, _ = _loc(ego)
+    map_scan = _asymmetric_map()
+    loc.localize(sensors=SensorFrame(lidar=map_scan))
+
+    # Seed estimate near truth so correspondences stay within max distance.
+    ego.x, ego.y, ego.theta = 0.1, -0.05, 0.0
+    loc._x, loc._y, loc._theta = ego.x, ego.y, ego.theta
+
+    moved = map_scan - np.array([dx, dy])
+    loc.localize(sensors=SensorFrame(lidar=moved))
+
+    assert ego.x == pytest.approx(dx, abs=5e-3)
+    assert ego.y == pytest.approx(dy, abs=5e-3)
+    assert ego.theta == pytest.approx(0.0, abs=1e-2)
+
+
+def test_pure_yaw_recovers_ego_theta():
+    """Body-frame = R(-yaw) @ map recovers heading near +yaw."""
+    yaw = math.radians(8.0)
+    ego = EgoState(x=0.0, y=0.0, theta=0.0, velocity=0.0)
+    loc, _ = _loc(ego)
+    map_scan = _asymmetric_map()
+    loc.localize(sensors=SensorFrame(lidar=map_scan))
+
+    c, s = math.cos(-yaw), math.sin(-yaw)
+    rot = np.array([[c, -s], [s, c]])
+    rotated = (rot @ map_scan.T).T
+
+    ego.theta = math.radians(6.0)
+    loc._theta = ego.theta
+    loc.localize(sensors=SensorFrame(lidar=rotated))
+
+    assert ego.theta == pytest.approx(yaw, abs=math.radians(0.5))
+    assert ego.x == pytest.approx(0.0, abs=5e-2)
+    assert ego.y == pytest.approx(0.0, abs=5e-2)
+
+
+@pytest.mark.parametrize(
+    "lidar",
+    [
+        None,
+        np.zeros((0, 2)),
+        np.array([[1.0, 0.0], [2.0, 0.0]]),  # fewer than 3 points
+        np.array([[1.0, 0.0, 10.0], [2.0, 0.0, 11.0], [3.0, 0.0, 12.0]]),  # all outside z-band
+    ],
+    ids=["none", "empty", "two_points", "z_band_empty"],
+)
+def test_invalid_or_filtered_scans_are_noops(lidar):
+    ego = EgoState(x=3.0, y=4.0, theta=0.5, velocity=1.0)
+    loc, _ = _loc(ego)
+    before = (ego.x, ego.y, ego.theta, ego.velocity)
+
+    loc.localize(sensors=SensorFrame(lidar=lidar))
+
+    assert (ego.x, ego.y, ego.theta, ego.velocity) == before
+    assert loc._map is None

test/c40_execution/test_c46_basic_sim_kinematics.py

@@ -0,0 +1,112 @@
+"""Regression tests for BasicSim bicycle ego and NPC kinematics."""
+
+import math
+
+import numpy as np
+import pytest
+
+from avlite.c10_perception.c11_perception_model import AgentState, EgoState, PerceptionModel
+from avlite.c20_planning.c21_planning_model import GlobalPlan
+from avlite.c30_control.c31_control_model import ControlCommand
+from avlite.c40_execution.c46_basic_sim import BasicSim
+from avlite.c40_execution.c49_settings import ExecutionSettingsSchema
+from avlite.c50_common.c54_trajectory_tracker import TrajectoryTracker
+
+
+def _straight_global_plan(v: float = 5.0) -> GlobalPlan:
+    trajectory = TrajectoryTracker(
+        path=[(0.0, 0.0), (20.0, 0.0), (40.0, 0.0)],
+        velocity=[v, v, v],
+    )
+    return GlobalPlan(
+        start_point=(0.0, 0.0),
+        goal_point=(40.0, 0.0),
+        path=list(trajectory.path),
+        velocity=list(trajectory.velocity),
+        trajectory=trajectory,
+    )
+
+
+def test_ego_straight_cruise_uses_pre_update_velocity_for_xy():
+    """XY integrates with pre-accel v; yaw uses post-accel v (steer=0 → no yaw)."""
+    ego = EgoState(x=0.0, y=0.0, theta=0.0, velocity=10.0)
+    setting = ExecutionSettingsSchema(c46_npc_control=False)
+    sim = BasicSim(ego_state=ego, pm=PerceptionModel(ego_vehicle=ego), setting=setting)
+    dt = 0.01
+    accel = 2.0
+
+    sim.control_ego_state(ControlCommand(acceleration=accel, steer=0.0), dt=dt)
+
+    assert ego.x == pytest.approx(10.0 * dt)
+    assert ego.y == pytest.approx(0.0)
+    assert ego.velocity == pytest.approx(10.0 + accel * dt)
+    assert ego.theta == pytest.approx(0.0)
+
+
+def test_ego_constant_steer_matches_bicycle_yaw_rate():
+    ego = EgoState(x=0.0, y=0.0, theta=0.0, velocity=5.0)
+    setting = ExecutionSettingsSchema(c46_npc_control=False)
+    sim = BasicSim(ego_state=ego, pm=PerceptionModel(ego_vehicle=ego), setting=setting)
+    dt = 0.01
+    steer = 0.2
+    L = 2.5  # default when no ego_controller is attached
+
+    sim.control_ego_state(ControlCommand(acceleration=0.0, steer=steer), dt=dt)
+
+    # accel=0 → post-update v equals pre-update v for yaw integration
+    assert ego.theta == pytest.approx((5.0 / L) * steer * dt)
+    assert ego.x == pytest.approx(5.0 * dt)
+    assert ego.y == pytest.approx(0.0)
+
+
+def test_npc_control_advances_agent_with_steer_slew_limit():
+    ego = EgoState(x=0.0, y=0.0, theta=0.0, velocity=0.0)
+    pm = PerceptionModel(ego_vehicle=ego)
+    setting = ExecutionSettingsSchema(c46_npc_control=True)
+    sim = BasicSim(ego_state=ego, pm=pm, setting=setting)
+    plan = _straight_global_plan(v=5.0)
+
+    agent = AgentState(x=0.0, y=0.0, theta=0.0, velocity=0.0)
+    sim.spawn_agent(agent, global_plan=plan)
+    assert agent.agent_id in sim.npc_controllers
+    ctrl = sim.npc_controllers[agent.agent_id]
+    # Force a large commanded steer so the slew clamp is the observable contract.
+    ctrl._npc_steer = 0.0
+
+    class _HugeSteer:
+        acceleration = 0.0
+        steer = 1.0
+
+    ctrl.control = lambda *args, **kwargs: _HugeSteer()  # type: ignore[method-assign]
+
+    x0, y0, theta0, v0 = agent.x, agent.y, agent.theta, agent.velocity
+    dt = 0.01
+    sim.control_ego_state(ControlCommand(acceleration=0.0, steer=0.0), dt=dt)
+
+    max_dsteer = 3.0 * dt
+    assert ctrl._npc_steer == pytest.approx(max_dsteer)
+    assert agent.velocity == pytest.approx(v0)  # accel 0
+    # NPC integrates velocity→yaw→xy (post-update v for position).
+    expected_theta = theta0 + agent.velocity / ctrl.ego_distance_front_axle * max_dsteer * dt
+    assert agent.theta == pytest.approx(expected_theta)
+    assert agent.x == pytest.approx(x0 + agent.velocity * math.cos(agent.theta) * dt)
+    assert agent.y == pytest.approx(y0 + agent.velocity * math.sin(agent.theta) * dt)
+    assert abs(agent.x - x0) + abs(agent.y - y0) + abs(agent.theta - theta0) > 0.0
+
+
+def test_npc_control_disabled_leaves_agents_frozen():
+    ego = EgoState(x=0.0, y=0.0, theta=0.0, velocity=8.0)
+    pm = PerceptionModel(ego_vehicle=ego)
+    setting = ExecutionSettingsSchema(c46_npc_control=False)
+    sim = BasicSim(ego_state=ego, pm=pm, setting=setting)
+    plan = _straight_global_plan()
+
+    agent = AgentState(x=1.0, y=0.0, theta=0.0, velocity=5.0)
+    sim.spawn_agent(agent, global_plan=plan)
+    assert sim.npc_controllers == {}
+
+    before = (agent.x, agent.y, agent.theta, agent.velocity)
+    sim.control_ego_state(ControlCommand(acceleration=0.0, steer=0.0), dt=0.01)
+
+    assert (agent.x, agent.y, agent.theta, agent.velocity) == before
+    assert ego.x == pytest.approx(8.0 * 0.01)