diff --git a/test/c10_perception/test_c11_state_geometry.py b/test/c10_perception/test_c11_state_geometry.py new file mode 100644 index 0000000..2aefc06 --- /dev/null +++ b/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) diff --git a/test/c10_perception/test_c16_lidar_localization.py b/test/c10_perception/test_c16_lidar_localization.py new file mode 100644 index 0000000..8cd1bfd --- /dev/null +++ b/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 diff --git a/test/c40_execution/test_c46_basic_sim_kinematics.py b/test/c40_execution/test_c46_basic_sim_kinematics.py new file mode 100644 index 0000000..c896f2b --- /dev/null +++ b/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)