From e743a2b56c6079b78dd442e767e7d0f73e079739 Mon Sep 17 00:00:00 2001 From: Cursor Agent Date: Mon, 3 Aug 2026 10:05:00 +0000 Subject: [PATCH] Add regression tests for lattice partial replan and e-stop switch. Cover obstacle_polygons hot-path glue in _partial_replan, track_end_s extension gate, edge-traversal handoff, and trailing-velocity emergency stop recovery in should_switch_plan. Co-authored-by: Majid Khonji --- .../test_c28_local_lattice_planners.py | 173 ++++++++++++++++++ 1 file changed, 173 insertions(+) diff --git a/test/c20_planning/test_c28_local_lattice_planners.py b/test/c20_planning/test_c28_local_lattice_planners.py index c96c912..fc7f15f 100644 --- a/test/c20_planning/test_c28_local_lattice_planners.py +++ b/test/c20_planning/test_c28_local_lattice_planners.py @@ -657,3 +657,176 @@ 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 TestShouldSwitchPlanEmergencyStop: + """Trailing-velocity emergency-stop recovery (not all-zeros; last < 0.5 and mean < 3).""" + + def test_recovers_from_trailing_low_velocity_to_clean_plan(self, fixed_planner_time): + global_plan = _straight_global_plan() + pm = PerceptionModel(ego_vehicle=EgoState(x=0.0, y=0.0, theta=0.0, velocity=2.0)) + planner = GreedyLatticePlanner(global_plan=global_plan, env=pm) + + estop = _edge_at(global_plan.trajectory, 0.0, 20.0) + n = len(estop.local_trajectory.velocity) + # Mean stays below 3.0 while only the trailing sample is near-stop. + estop.local_trajectory.velocity = [2.0] * (n - 1) + [0.2] + clean = _edge_with_velocity(global_plan.trajectory, velocity=8.0) + + planner.selected_local_plan = estop + planner._last_plan_change_time = _FIXED_PLANNER_TIME + + assert planner.should_switch_plan(clean) is True + + def test_holds_low_speed_cruise_that_is_not_emergency_stop(self, fixed_planner_time): + global_plan = _straight_global_plan() + pm = PerceptionModel(ego_vehicle=EgoState(x=0.0, y=0.0, theta=0.0, velocity=2.5)) + planner = GreedyLatticePlanner(global_plan=global_plan, env=pm) + + cruise = _chain_with_uniform_velocity(global_plan.trajectory, velocity=2.5) + alternative = _chain_with_uniform_velocity(global_plan.trajectory, velocity=2.5) + + planner.selected_local_plan = cruise + planner._last_plan_change_time = _FIXED_PLANNER_TIME + + assert planner.should_switch_plan(alternative) is False + + def test_trailing_velocity_above_threshold_is_not_emergency_stop(self, fixed_planner_time): + global_plan = _straight_global_plan() + pm = PerceptionModel(ego_vehicle=EgoState(x=0.0, y=0.0, theta=0.0, velocity=2.0)) + planner = GreedyLatticePlanner(global_plan=global_plan, env=pm) + + current = _edge_at(global_plan.trajectory, 0.0, 20.0) + n = len(current.local_trajectory.velocity) + current.local_trajectory.velocity = [2.0] * (n - 1) + [0.6] + alternative = _edge_with_velocity(global_plan.trajectory, velocity=2.0) + + planner.set_selected_plan(current) + planner._last_plan_change_time = _FIXED_PLANNER_TIME + + assert planner.should_switch_plan(alternative) is False + + +class TestPartialReplanPassesObstaclePolygons: + """Sliding-window glue: with agents, every check_collision gets 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)], + ) + planner = GreedyLatticePlanner(global_plan=global_plan, env=pm) + planner.set_selected_plan(_edge_at(global_plan.trajectory, 0.0, 20.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, + ) + planner._partial_replan() + + assert seen, "expected at least one edge collision check" + assert all(polys is not None and polys != "MISSING" for polys in seen) + assert planner.selected_local_plan.selected_next_local_plan is not None + + 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 = GreedyLatticePlanner(global_plan=global_plan, env=pm) + planner.set_selected_plan(_edge_at(global_plan.trajectory, 0.0, 20.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, + ) + planner._partial_replan() + + assert seen + assert all(polys is None for polys in seen) + + +class TestPartialReplanTrackEndGate: + """Partial replan must skip extension when tail + maneuver exceeds track_end_s.""" + + def test_skips_extension_past_track_end_s(self): + global_plan = _straight_global_plan() + pm = PerceptionModel(ego_vehicle=EgoState(x=0.0, y=0.0, theta=0.0, velocity=5.0)) + planner = GreedyLatticePlanner(global_plan=global_plan, env=pm) + + # track_end_s == 100, maneuver_distance == 30 → any tail end.s > 70 skips. + planner.set_selected_plan(_edge_at(global_plan.trajectory, 75.0, 80.0)) + planner._partial_replan() + + assert planner.selected_local_plan.selected_next_local_plan is None + + def test_extends_when_tail_has_room_before_track_end(self): + global_plan = _straight_global_plan() + pm = PerceptionModel(ego_vehicle=EgoState(x=0.0, y=0.0, theta=0.0, velocity=5.0)) + planner = GreedyLatticePlanner(global_plan=global_plan, env=pm) + + planner.set_selected_plan(_edge_at(global_plan.trajectory, 0.0, 20.0)) + planner._partial_replan() + + nxt = planner.selected_local_plan.selected_next_local_plan + assert nxt is not None + assert nxt.end.s == pytest.approx(20.0 + planner.maneuver_distance) + + +class TestAdvanceLocalPlanHandoff: + """Finishing an edge advances the committed chain and triggers sliding-window replan.""" + + def test_advance_selects_successor_and_calls_on_edge_traversed(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 = GreedyLatticePlanner(global_plan=global_plan, env=pm) + + e0 = _edge_at(global_plan.trajectory, 0.0, 20.0) + e1 = _edge_at(global_plan.trajectory, 20.0, 40.0) + planner.set_selected_plan(_link_edges([e0, e1])) + + calls: list[str] = [] + monkeypatch.setattr(planner, "_partial_replan", lambda: calls.append("partial")) + + # Mark the head edge as fully traversed before the advance step. + head_tj = planner.selected_local_plan.local_trajectory + head_tj.current_wp = len(head_tj.path) - 1 + head_tj.next_wp = head_tj.current_wp + + planner._advance_local_plan(EgoState(x=20.0, y=0.0, theta=0.0, velocity=5.0)) + + assert planner.selected_local_plan is e1 + assert calls == ["partial"] + + def test_advance_holds_when_no_successor(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 = GreedyLatticePlanner(global_plan=global_plan, env=pm) + + head = _edge_at(global_plan.trajectory, 0.0, 20.0) + planner.set_selected_plan(head) + + calls: list[str] = [] + monkeypatch.setattr(planner, "_partial_replan", lambda: calls.append("partial")) + + head_tj = planner.selected_local_plan.local_trajectory + head_tj.current_wp = len(head_tj.path) - 1 + head_tj.next_wp = head_tj.current_wp + + planner._advance_local_plan(EgoState(x=20.0, y=0.0, theta=0.0, velocity=5.0)) + + assert planner.selected_local_plan is head + assert calls == []