From 9a1de2b9ebbf46628dd1f287fa3b1492511a78cc Mon Sep 17 00:00:00 2001 From: Cursor Agent Date: Sun, 2 Aug 2026 10:04:47 +0000 Subject: [PATCH] Add regression tests for lap S-crossover, lattice boundaries, and collision precompute glue. Cover race lap detection via track_end_s, Lattice._check_boundary_violation clearance rules, generate_lattice_from_nodes obstacle_polygons plumbing, and Frenet orientation composition on the cursor segment. Co-authored-by: Majid Khonji --- .../test_c23_local_planning_pipeline.py | 93 ++++++++++++++- .../test_c28_local_lattice_planners.py | 112 ++++++++++++++++++ .../test_c54_trajectory_waypoint_update.py | 20 ++++ 3 files changed, 224 insertions(+), 1 deletion(-) diff --git a/test/c20_planning/test_c23_local_planning_pipeline.py b/test/c20_planning/test_c23_local_planning_pipeline.py index bbee516..fc640f6 100644 --- a/test/c20_planning/test_c23_local_planning_pipeline.py +++ b/test/c20_planning/test_c23_local_planning_pipeline.py @@ -23,7 +23,13 @@ from avlite.c50_common.c54_trajectory_tracker import TrajectoryTracker -def _straight_global_plan(x_end: float = 100.0, n: int = 20, velocity: float = 10.0) -> GlobalPlan: +def _straight_global_plan( + x_end: float = 100.0, + n: int = 20, + velocity: float = 10.0, + *, + race_mode: bool = True, +) -> GlobalPlan: xs = [x_end * i / (n - 1) for i in range(n)] path = [(x, 0.0) for x in xs] vel = [velocity] * n @@ -40,6 +46,35 @@ def _straight_global_plan(x_end: float = 100.0, n: int = 20, velocity: float = 1 trajectory=trajectory, left_boundary_d=left, right_boundary_d=right, + race_mode=race_mode, + ) + + +def _closed_square_global_plan(side: float = 50.0, velocity: float = 10.0) -> GlobalPlan: + """Closed loop with duplicated finish==start (first==last).""" + path = [ + (0.0, 0.0), + (side, 0.0), + (side, side), + (0.0, side), + (0.0, 0.0), + ] + n = len(path) + vel = [velocity] * n + left = [3.0] * n + right = [-3.0] * n + trajectory = TrajectoryTracker(path=path, velocity=vel) + trajectory.ref_left_boundary_d = left + trajectory.ref_right_boundary_d = right + return GlobalPlan( + start_point=path[0], + goal_point=path[-1], + path=path, + velocity=vel, + trajectory=trajectory, + left_boundary_d=left, + right_boundary_d=right, + race_mode=True, ) @@ -135,3 +170,59 @@ def test_pipeline_step_advances_child(self): state = EgoState(x=5.0, y=0.0, theta=0.0, velocity=5.0) pipeline.step(state) assert pipeline.location_xy == (5.0, 0.0) + + +class TestRaceLapSCrossover: + """Lap detection uses track_end_s (not path_s[-2]) for the near-end threshold.""" + + def _planner(self, global_plan: GlobalPlan) -> VelocityLocalPlanner: + pm = PerceptionModel(ego_vehicle=EgoState(x=0.0, y=0.0, theta=0.0, velocity=5.0)) + return VelocityLocalPlanner(global_plan=global_plan, env=pm) + + def test_lap_increments_on_s_crossover_past_track_end_threshold(self): + plan = _closed_square_global_plan() + planner = self._planner(plan) + track_len = plan.trajectory.track_end_s + assert track_len > 0 + assert plan.trajectory.path_s[-2] < track_len + + # Near finish by track_end_s, then cross to near start. + planner.traversed_s = [track_len * 0.85] + planner.lap = 0 + planner.step(EgoState(x=1.0, y=0.0, theta=0.0, velocity=5.0)) + assert planner.lap == 1 + + def test_no_lap_when_prev_s_only_past_stale_path_s_minus_two(self): + """path_s[-2] is one segment short — must not treat that band as near-end.""" + plan = _closed_square_global_plan() + planner = self._planner(plan) + tj = plan.trajectory + stale = tj.path_s[-2] + track_len = tj.track_end_s + # Above 80% of the stale length, but still below 80% of true lap length. + prev_s = stale * 0.85 + assert prev_s > stale * 0.8 + assert prev_s < track_len * 0.8 + + planner.traversed_s = [prev_s] + planner.lap = 0 + planner.step(EgoState(x=1.0, y=0.0, theta=0.0, velocity=5.0)) + assert planner.lap == 0 + + def test_mid_track_step_does_not_increment_lap(self): + plan = _straight_global_plan() + planner = self._planner(plan) + planner.traversed_s = [40.0] + planner.lap = 0 + planner.step(EgoState(x=50.0, y=0.0, theta=0.0, velocity=5.0)) + assert planner.lap == 0 + + def test_race_mode_off_skips_lap_counting(self): + plan = _closed_square_global_plan() + plan.race_mode = False + planner = self._planner(plan) + track_len = plan.trajectory.track_end_s + planner.traversed_s = [track_len * 0.85] + planner.lap = 0 + planner.step(EgoState(x=1.0, y=0.0, theta=0.0, velocity=5.0)) + assert planner.lap == 0 diff --git a/test/c20_planning/test_c28_local_lattice_planners.py b/test/c20_planning/test_c28_local_lattice_planners.py index c96c912..f0aafb3 100644 --- a/test/c20_planning/test_c28_local_lattice_planners.py +++ b/test/c20_planning/test_c28_local_lattice_planners.py @@ -657,3 +657,115 @@ def test_single_edge_release_is_debounced(self, monkeypatch): planner.replan() assert planner.selected_local_plan is None assert planner._committed_trajectory is None + + +class TestBoundaryViolation: + """Direct coverage for Lattice._check_boundary_violation (left+/right− + clearance).""" + + def _lattice(self, clearance: float = 0.5) -> Lattice: + global_tj = _straight_global_plan().trajectory + n = len(global_tj.path) + lattice = Lattice( + global_trajectory=global_tj, + ref_left_boundary_d=[3.0] * n, + ref_right_boundary_d=[-3.0] * n, + planning_horizon=1, + ) + lattice.boundary_clearance = clearance + return lattice + + def test_centerline_edge_is_inside_boundaries(self): + lattice = self._lattice() + edge = _edge_at_sd(lattice.global_trajectory, 0.0, 0.0, 20.0, 0.0) + assert lattice._check_boundary_violation(edge) is False + + def test_left_bound_without_clearance_is_violation(self): + lattice = self._lattice(clearance=0.5) + # End at left boundary d=3.0; with clearance 0.5 the limit is 2.5. + edge = _edge_at_sd(lattice.global_trajectory, 0.0, 0.0, 20.0, 3.0) + assert lattice._check_boundary_violation(edge) is True + + def test_right_bound_without_clearance_is_violation(self): + lattice = self._lattice(clearance=0.5) + edge = _edge_at_sd(lattice.global_trajectory, 0.0, 0.0, 20.0, -3.0) + assert lattice._check_boundary_violation(edge) is True + + def test_clearance_inset_marks_near_bound_as_violation(self): + lattice = self._lattice(clearance=0.5) + # d=2.6 is inside raw left=3.0 but outside inset 2.5. + edge = _edge_at_sd(lattice.global_trajectory, 0.0, 0.0, 20.0, 2.6) + assert lattice._check_boundary_violation(edge) is True + + def test_missing_parent_frenet_path_is_not_a_violation(self): + lattice = self._lattice() + edge = _edge_at(lattice.global_trajectory, 0.0, 20.0) + edge.local_trajectory.path_s_from_parent = None + edge.local_trajectory.path_d_from_parent = None + assert lattice._check_boundary_violation(edge) is False + + +class TestGenerateLatticePassesObstaclePolygons: + """Hot-path glue: with agents, check_collision must receive precomputed polygons.""" + + def test_agents_trigger_precomputed_polygons_kwarg(self, monkeypatch): + from avlite.c10_perception.c11_perception_model import AgentState + + 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)], + ) + lattice = Lattice( + global_trajectory=global_plan.trajectory, + ref_left_boundary_d=global_plan.left_boundary_d, + ref_right_boundary_d=global_plan.right_boundary_d, + planning_horizon=1, + num_of_points=8, + ) + lattice.sample_nodes( + s=0.0, d=0.0, sample_size=3, maneuver_distance=20.0, + boundary_clearance=0.5, lateral_reach=float("inf"), sample_distribution=0, + ) + + seen: list[object] = [] + + 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.c28_local_lattice_planners.check_collision", + _capture_check_collision, + ) + lattice.generate_lattice_from_nodes(pm=pm) + assert seen, "expected at least one edge collision check" + assert all(polys is not None and polys != "MISSING" for polys in seen) + + 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)) + lattice = Lattice( + global_trajectory=global_plan.trajectory, + ref_left_boundary_d=global_plan.left_boundary_d, + ref_right_boundary_d=global_plan.right_boundary_d, + planning_horizon=1, + num_of_points=8, + ) + lattice.sample_nodes( + s=0.0, d=0.0, sample_size=3, maneuver_distance=20.0, + boundary_clearance=0.5, lateral_reach=float("inf"), sample_distribution=0, + ) + + seen: list[object] = [] + + 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.c28_local_lattice_planners.check_collision", + _capture_check_collision, + ) + lattice.generate_lattice_from_nodes(pm=pm) + assert seen + assert all(polys is None for polys in seen) diff --git a/test/c50_common/test_c54_trajectory_waypoint_update.py b/test/c50_common/test_c54_trajectory_waypoint_update.py index 11e8f2d..c088ce5 100644 --- a/test/c50_common/test_c54_trajectory_waypoint_update.py +++ b/test/c50_common/test_c54_trajectory_waypoint_update.py @@ -1,5 +1,7 @@ """Regression tests for TrajectoryTracker waypoint index updates.""" +import math + import pytest from avlite.c50_common.c54_trajectory_tracker import TrajectoryTracker @@ -64,3 +66,21 @@ def test_create_quintic_trajectory_sd_honors_boundary_derivatives(): assert d1p(s1) == pytest.approx(-0.1, abs=1e-9) assert d2p(s0) == pytest.approx(0.3, abs=1e-9) assert d2p(s1) == pytest.approx(0.05, abs=1e-9) + + +def test_convert_sd_orientation_adds_path_heading_at_cursor_segment(): + """World yaw = Frenet theta + heading of the current→next waypoint segment.""" + # L-path: horizontal then vertical. Cursor on the vertical leg. + path = [(0.0, 0.0), (10.0, 0.0), (10.0, 10.0)] + tj = TrajectoryTracker(path=path, velocity=[5.0] * 3) + tj.update_waypoint_by_wp(1) + assert tj.current_wp == 1 + assert tj.next_wp == 2 + + s_mid = 0.5 * (tj.path_s[1] + tj.path_s[2]) + _x, _y, yaw = tj.convert_sd_orientation_to_xy_orientation(s_mid, 0.0, 0.0) + assert yaw == pytest.approx(math.pi / 2, abs=1e-6) + + # Relative Frenet heading is preserved on top of path heading. + _x2, _y2, yaw2 = tj.convert_sd_orientation_to_xy_orientation(s_mid, 0.0, 0.25) + assert yaw2 == pytest.approx(math.pi / 2 + 0.25, abs=1e-6)