Add regression tests for lap S-crossover, lattice boundaries, and collision precompute glue

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

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)

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)