From 76091226ba99ac99a779230d8dcbf7415bb36ff0 Mon Sep 17 00:00:00 2001 From: Cursor Agent Date: Tue, 4 Aug 2026 10:07:48 +0000 Subject: [PATCH] Add regression tests for velocity collision glue, Kalman tracks, and OpenDRIVE geometry Cover VelocityLocalPlanner obstacle_polygons hot-path wiring, KalmanTracker ID/association lifecycle, and previously untested arc/spiral/lane-offset parsing. Co-authored-by: Majid Khonji --- .../c10_perception/test_c15_kalman_tracker.py | 74 ++++++++++++++++++ test/c10_perception/test_c18_hdmap_parser.py | 57 +++++++++++++- ..._local_behavioral_and_velocity_planners.py | 76 +++++++++++++++++++ 3 files changed, 206 insertions(+), 1 deletion(-) create mode 100644 test/c10_perception/test_c15_kalman_tracker.py diff --git a/test/c10_perception/test_c15_kalman_tracker.py b/test/c10_perception/test_c15_kalman_tracker.py new file mode 100644 index 0000000..fe2c07f --- /dev/null +++ b/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") diff --git a/test/c10_perception/test_c18_hdmap_parser.py b/test/c10_perception/test_c18_hdmap_parser.py index 26e485b..4fa1bcb 100644 --- a/test/c10_perception/test_c18_hdmap_parser.py +++ b/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 diff --git a/test/c20_planning/test_c27_local_behavioral_and_velocity_planners.py b/test/c20_planning/test_c27_local_behavioral_and_velocity_planners.py index 02bbd2a..3b5f825 100644 --- a/test/c20_planning/test_c27_local_behavioral_and_velocity_planners.py +++ b/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