Add regression tests for velocity collision glue, Kalman tracks, and OpenDRIVE geometry

test/c10_perception/test_c15_kalman_tracker.py

@@ -0,0 +1,74 @@
+"""Regression tests for KalmanTracker association and track lifecycle."""
+
+from avlite.c10_perception.c11_perception_model import AgentState, EgoState, PerceptionModel
+from avlite.c10_perception.c15_perception_algs import KalmanTracker
+
+
+def _pm_with_detections(*xy: tuple[float, float]) -> PerceptionModel:
+    agents = [
+        AgentState(x=x, y=y, theta=0.0, velocity=0.0, agent_id=i, length=4.0, width=2.0)
+        for i, (x, y) in enumerate(xy)
+    ]
+    return PerceptionModel(
+        ego_vehicle=EgoState(x=0.0, y=0.0, theta=0.0, velocity=0.0),
+        agent_vehicles=agents,
+    )
+
+
+class TestKalmanTrackerLifecycle:
+    def test_persists_id_and_estimates_velocity_across_frames(self):
+        tracker = KalmanTracker(dt=1.0, gate_distance=2.0, max_missed=2, min_speed=0.1)
+
+        out1 = tracker.track(_pm_with_detections((0.0, 0.0)))
+        assert len(out1.agent_vehicles) == 1
+        track_id = out1.agent_vehicles[0].agent_id
+        assert out1.agent_vehicles[0].velocity == 0.0
+
+        # Same detection id is reassigned each frame; tracker must keep track_id.
+        out2 = tracker.track(_pm_with_detections((1.0, 0.0)))
+        assert len(out2.agent_vehicles) == 1
+        assert out2.agent_vehicles[0].agent_id == track_id
+        assert out2.agent_vehicles[0].velocity > 0.5
+
+    def test_far_detection_spawns_new_track(self):
+        tracker = KalmanTracker(dt=1.0, gate_distance=2.0, max_missed=2, min_speed=0.1)
+
+        out1 = tracker.track(_pm_with_detections((0.0, 0.0)))
+        first_id = out1.agent_vehicles[0].agent_id
+
+        # Outside gate → new track rather than hijacking the existing one.
+        out2 = tracker.track(_pm_with_detections((10.0, 0.0)))
+        assert len(out2.agent_vehicles) == 1
+        assert out2.agent_vehicles[0].agent_id != first_id
+
+    def test_missed_tracks_are_pruned_after_max_missed(self):
+        tracker = KalmanTracker(dt=1.0, gate_distance=2.0, max_missed=1, min_speed=0.1)
+
+        tracker.track(_pm_with_detections((0.0, 0.0)))
+        # One unmatched frame increments missed; still kept internally but not emitted.
+        empty = tracker.track(
+            PerceptionModel(
+                ego_vehicle=EgoState(x=0.0, y=0.0, theta=0.0, velocity=0.0),
+                agent_vehicles=[],
+            )
+        )
+        assert empty.agent_vehicles == []
+        assert len(tracker._tracks) == 1
+
+        # Second miss exceeds max_missed and drops the track.
+        tracker.track(
+            PerceptionModel(
+                ego_vehicle=EgoState(x=0.0, y=0.0, theta=0.0, velocity=0.0),
+                agent_vehicles=[],
+            )
+        )
+        assert tracker._tracks == []
+
+    def test_requires_perception_model(self):
+        tracker = KalmanTracker()
+        try:
+            tracker.track(None)
+        except ValueError as exc:
+            assert "perception_model" in str(exc)
+        else:
+            raise AssertionError("expected ValueError when perception_model is None")

test/c10_perception/test_c18_hdmap_parser.py

@@ -12,7 +12,11 @@
 import xml.etree.ElementTree as ET
 
 from avlite.c10_perception.c11_perception_model import HDMap
-from avlite.c10_perception.c18_hdmap_parser import parse_geo_reference_from_root, sample_OpenDrive_geometry
+from avlite.c10_perception.c18_hdmap_parser import (
+    _get_lane_offset_at_s,
+    parse_geo_reference_from_root,
+    sample_OpenDrive_geometry,
+)
 
 
 class TestHDMapLoadable:
@@ -61,3 +65,54 @@ def test_line_geometry_endpoints(self):
         assert x_vals[0] == pytest.approx(0.0)
         assert x_vals[-1] == pytest.approx(10.0, rel=0.01)
         assert np.allclose(y_vals, 0.0)
+
+    def test_arc_geometry_quarter_circle_endpoint(self):
+        """Positive curvature arc of π/2 with radius 10 ends near (10, 10)."""
+        curvature = 0.1
+        radius = 1.0 / curvature
+        length = (np.pi / 2.0) * radius
+        x_vals, y_vals = sample_OpenDrive_geometry(
+            0.0,
+            0.0,
+            0.0,
+            length,
+            "arc",
+            attributes={"curvature": str(curvature)},
+            n_pts=25,
+        )
+        assert x_vals[0] == pytest.approx(0.0, abs=1e-9)
+        assert y_vals[0] == pytest.approx(0.0, abs=1e-9)
+        assert x_vals[-1] == pytest.approx(radius, abs=1e-6)
+        assert y_vals[-1] == pytest.approx(radius, abs=1e-6)
+
+    def test_zero_curvature_spiral_falls_back_to_line(self):
+        x_vals, y_vals = sample_OpenDrive_geometry(
+            0.0,
+            0.0,
+            0.0,
+            8.0,
+            "spiral",
+            attributes={"curvStart": "0", "curvEnd": "0"},
+            n_pts=5,
+        )
+        assert x_vals[-1] == pytest.approx(8.0, abs=1e-9)
+        assert np.allclose(y_vals, 0.0)
+
+
+class TestLaneOffsetAtS:
+    def test_empty_offsets_are_zero(self):
+        assert _get_lane_offset_at_s([], 5.0) == 0.0
+
+    def test_uses_latest_applicable_polynomial_segment(self):
+        offsets = [
+            {"s": "0.0", "a": "1.0", "b": "0.0", "c": "0.0", "d": "0.0"},
+            {"s": "10.0", "a": "2.0", "b": "0.5", "c": "0.0", "d": "0.0"},
+        ]
+        # Before second segment: constant a=1.
+        assert _get_lane_offset_at_s(offsets, 5.0) == pytest.approx(1.0)
+        # Inside second segment: a + b*(s-10) = 2 + 0.5*4.
+        assert _get_lane_offset_at_s(offsets, 14.0) == pytest.approx(4.0)
+
+    def test_before_first_offset_s_is_zero(self):
+        offsets = [{"s": "5.0", "a": "3.0", "b": "0.0", "c": "0.0", "d": "0.0"}]
+        assert _get_lane_offset_at_s(offsets, 1.0) == 0.0

test/c20_planning/test_c27_local_behavioral_and_velocity_planners.py

@@ -237,6 +237,82 @@ def test_apply_speed_match_without_ref_velocity(self):
         assert tj.velocity[tj.current_wp] <= 3.5
 
 
+class TestProfileTrajectoryObstaclePolygons:
+    """Hot-path glue: profile_trajectory must pass precomputed polygons into check_collision."""
+
+    def test_agents_trigger_precomputed_polygons_kwarg(self, monkeypatch):
+        global_plan = _straight_global_plan()
+        pm = PerceptionModel(
+            ego_vehicle=EgoState(x=0.0, y=0.0, theta=0.0, velocity=5.0),
+            agent_vehicles=[AgentState(x=40.0, y=0.0, theta=0.0, velocity=0.0, agent_id=1)],
+        )
+        planner = VelocityLocalPlanner(global_plan=global_plan, env=pm)
+        tj = TrajectoryTracker(path=list(global_plan.path), velocity=list(global_plan.velocity))
+
+        sentinel = ["sentinel-polygons"]
+        precompute_kwargs: list[dict] = []
+        seen: list[object] = []
+
+        def _fake_precompute(pm_arg, **kwargs):
+            precompute_kwargs.append(kwargs)
+            return sentinel
+
+        def _capture_check_collision(pm_arg, traj, **kwargs):
+            seen.append(kwargs.get("obstacle_polygons", "MISSING"))
+            return False, -1, 0.0, 10.0
+
+        monkeypatch.setattr(
+            "avlite.c20_planning.c27_local_behavioral_and_velocity_planners.precompute_obstacle_polygons",
+            _fake_precompute,
+        )
+        monkeypatch.setattr(
+            "avlite.c20_planning.c27_local_behavioral_and_velocity_planners.check_collision",
+            _capture_check_collision,
+        )
+
+        planner.profile_trajectory(tj)
+
+        assert precompute_kwargs, "expected precompute when agents are present"
+        assert "total_time" in precompute_kwargs[0]
+        assert "min_velocity_threshold" in precompute_kwargs[0]
+        assert "obstacle_inflation_margin" in precompute_kwargs[0]
+        # Velocity planner must not pass lattice beside_* sweep windows.
+        assert "beside_sweep_time" not in precompute_kwargs[0]
+        assert "beside_rear_window" not in precompute_kwargs[0]
+        assert seen == [sentinel]
+
+    def test_no_agents_leaves_obstacle_polygons_none(self, monkeypatch):
+        global_plan = _straight_global_plan()
+        pm = PerceptionModel(ego_vehicle=EgoState(x=0.0, y=0.0, theta=0.0, velocity=5.0))
+        planner = VelocityLocalPlanner(global_plan=global_plan, env=pm)
+        tj = TrajectoryTracker(path=list(global_plan.path), velocity=list(global_plan.velocity))
+
+        seen: list[object] = []
+        precompute_calls = {"n": 0}
+
+        def _fake_precompute(*_args, **_kwargs):
+            precompute_calls["n"] += 1
+            return ["should-not-be-used"]
+
+        def _capture_check_collision(pm_arg, traj, **kwargs):
+            seen.append(kwargs.get("obstacle_polygons", "MISSING"))
+            return False, -1, 0.0, 10.0
+
+        monkeypatch.setattr(
+            "avlite.c20_planning.c27_local_behavioral_and_velocity_planners.precompute_obstacle_polygons",
+            _fake_precompute,
+        )
+        monkeypatch.setattr(
+            "avlite.c20_planning.c27_local_behavioral_and_velocity_planners.check_collision",
+            _capture_check_collision,
+        )
+
+        planner.profile_trajectory(tj)
+
+        assert precompute_calls["n"] == 0
+        assert seen == [None]
+
+
 class TestCruiseBehavioralPlanner:
     def test_sets_cruise_behavior(self):
         from avlite.c20_planning.c21_planning_model import LocalBehavior, LocalPlan