From bb899860ab60aa2c9ebd79da59f95604016bf335 Mon Sep 17 00:00:00 2001 From: Cursor Agent Date: Fri, 31 Jul 2026 10:08:24 +0000 Subject: [PATCH 1/3] Add regression tests for predictor-gated obstacle sweeps. Cover that movers without SingleTrajectory stay static boxes, predicted agents sweep forward, and beside/behind gates honor beside_rear_window. Co-authored-by: Majid Khonji --- .../c50_common/test_c55_collision_checking.py | 78 ++++++++++++++++++- 1 file changed, 77 insertions(+), 1 deletion(-) diff --git a/test/c50_common/test_c55_collision_checking.py b/test/c50_common/test_c55_collision_checking.py index 4c39337..61e594c 100644 --- a/test/c50_common/test_c55_collision_checking.py +++ b/test/c50_common/test_c55_collision_checking.py @@ -6,15 +6,31 @@ - precompute_obstacle_polygons returns one polygon per agent. - Ego + obstacle margins combine to ~1 m body-to-body clearance. - Ego length extension catches front-corner side overlaps. +- Forward sweeps require prediction; movers without trajectories stay static. +- Beside/behind gating uses beside_rear_window / beside_sweep_time. """ import numpy as np -from avlite.c10_perception.c11_perception_model import AgentState, EgoState, PerceptionModel +from avlite.c10_perception.c11_perception_model import ( + AgentState, + EgoState, + PerceptionModel, + SingleTrajectory, +) from avlite.c50_common.c54_trajectory_tracker import TrajectoryTracker from avlite.c50_common.c55_collision_checking import check_collision, precompute_obstacle_polygons +def _forward_prediction(agent: AgentState, *, dt: float = 0.1, n_steps: int = 40) -> SingleTrajectory: + steps = np.empty((n_steps, 2)) + for t in range(n_steps): + time = (t + 1) * dt + steps[t, 0] = agent.x + agent.velocity * np.cos(agent.theta) * time + steps[t, 1] = agent.y + agent.velocity * np.sin(agent.theta) * time + return SingleTrajectory(predict_delta_t=dt, trajectories={agent.agent_id: steps}) + + def _straight_trajectory(x_start: float, x_end: float, n: int = 20, y: float = 0.0) -> TrajectoryTracker: xs = [x_start + (x_end - x_start) * i / (n - 1) for i in range(n)] path = [(x, y) for x in xs] @@ -129,3 +145,63 @@ def test_front_corner_side_overlap_detected(self): collision_safety_margin=margin, ) assert hit is True + + +class TestPredictorGatedSweep: + """precompute_obstacle_polygons must not fabricate constant-velocity sweeps.""" + + def test_moving_agent_without_prediction_stays_static_box(self): + agent = AgentState(x=50.0, y=0.0, theta=0.0, velocity=5.0, agent_id=1) + pm = PerceptionModel( + ego_vehicle=EgoState(x=0.0, y=0.0, theta=0.0, velocity=5.0), + agent_vehicles=[agent], + ) + polys = precompute_obstacle_polygons(pm, total_time=2.0) + assert abs(polys[0][0].centroid.x - 50.0) < 2.0 + + def test_moving_agent_with_prediction_sweeps_forward(self): + agent = AgentState(x=50.0, y=0.0, theta=0.0, velocity=5.0, agent_id=1) + pm = PerceptionModel( + ego_vehicle=EgoState(x=0.0, y=0.0, theta=0.0, velocity=5.0), + agent_vehicles=[agent], + prediction=_forward_prediction(agent), + ) + polys = precompute_obstacle_polygons(pm, total_time=2.0) + # Convex hull of current + predicted poses reaches ~x=60. + minx, _, maxx, _ = polys[0][0].bounds + assert minx < 52.0 + assert maxx > 58.0 + + def test_far_behind_agent_not_beside_swept(self): + agent = AgentState(x=-30.0, y=0.0, theta=0.0, velocity=5.0, agent_id=1) + pm = PerceptionModel( + ego_vehicle=EgoState(x=0.0, y=0.0, theta=0.0, velocity=5.0), + agent_vehicles=[agent], + prediction=_forward_prediction(agent), + ) + polys = precompute_obstacle_polygons( + pm, + total_time=2.0, + beside_sweep_time=1.0, + beside_rear_window=10.0, + ) + assert abs(polys[0][0].centroid.x - (-30.0)) < 2.0 + + def test_just_behind_agent_uses_beside_sweep(self): + agent = AgentState(x=-5.0, y=0.0, theta=0.0, velocity=5.0, agent_id=1) + pm = PerceptionModel( + ego_vehicle=EgoState(x=0.0, y=0.0, theta=0.0, velocity=5.0), + agent_vehicles=[agent], + prediction=_forward_prediction(agent), + ) + polys = precompute_obstacle_polygons( + pm, + total_time=2.0, + beside_sweep_time=1.0, + beside_rear_window=10.0, + ) + minx, _, maxx, _ = polys[0][0].bounds + assert minx < -3.0 + # 1 s of forward motion from x=-5 at 5 m/s → ~0, not the 2 s total_time tip. + assert maxx > -2.0 + assert maxx < 4.0 From 9e3b408c92346d537a6c9bbc391a0edae2818e28 Mon Sep 17 00:00:00 2001 From: Cursor Agent Date: Fri, 31 Jul 2026 10:08:24 +0000 Subject: [PATCH 2/3] Add async executer shared sensor-snapshot regression tests. Prove combined planner/perception worker fetches once per iteration and skips get_sensor_frame when no stage module is active. Co-authored-by: Majid Khonji --- .../c40_execution/test_c42_sensor_snapshot.py | 87 +++++++++++++++++++ 1 file changed, 87 insertions(+) diff --git a/test/c40_execution/test_c42_sensor_snapshot.py b/test/c40_execution/test_c42_sensor_snapshot.py index 6c9b242..6ded38f 100644 --- a/test/c40_execution/test_c42_sensor_snapshot.py +++ b/test/c40_execution/test_c42_sensor_snapshot.py @@ -2,6 +2,7 @@ from __future__ import annotations +import time from dataclasses import dataclass, field from types import SimpleNamespace from typing import Optional @@ -12,6 +13,7 @@ from avlite.c20_planning.c21_planning_model import LocalPlan from avlite.c30_control.c31_control_model import AckermannControlCommand from avlite.c40_execution.c44_sync_executer import SyncExecuter +from avlite.c40_execution.c45_async_threaded_executer import AsyncThreadedExecuter from avlite.c40_execution.c41_world_bridge import WorldBridge from avlite.c40_execution.c49_settings import ExecutionSettings from avlite.c50_common.c51_capabilities import StackCapability @@ -128,3 +130,88 @@ def test_tick_with_no_stage_due_skips_the_fetch(): sim_dt=0.01, perception_dt=1e6, replan_dt=1e6, control_dt=1e6, localization_dt=1e6, ) assert len(world.fetches) == 1 + + +def test_async_combined_worker_shares_one_snapshot_per_iteration(): + """Planner+perception combined mode must fetch once and share the frame.""" + world = _CountingWorld() + seen: dict = {} + + def perceive(*, perception_model=None, sensors=None): + seen["perceive"] = sensors + # Perception runs after replan in the same iteration; stop once both saw the frame. + exec_.stopped = True + + def replan(*, perception_model=None, sensors=None): + seen["replan"] = sensors + + exec_ = AsyncThreadedExecuter( + perception_model=PerceptionModel(ego_vehicle=EgoState(x=0.0, y=0.0)), + world=world, + perception=SimpleNamespace( + world_requirements=frozenset(), + stack_requirements=frozenset(), + stack_capabilities=frozenset(), + perceive=perceive, + ), + localization=None, + global_planner=None, + local_planner=SimpleNamespace( + world_requirements=frozenset(), + stack_requirements=frozenset(), + stack_capabilities=frozenset(), + replan=replan, + get_local_plan=lambda: LocalPlan(), + step=lambda state: None, + global_plan=None, + ), + controller=None, + combined_perception_planning=True, + perception_dt=0.01, + replan_dt=0.01, + ) + # Avoid the cold-start perception stall gate (dt_p from last=0 looks huge). + exec_._perception_fps_tracker.last = time.time() + exec_.call_perceive = True + exec_.call_replan = True + exec_.call_localize = False + exec_.pace_perception = False + exec_.pace_replan = False + + exec_.worker_planning() + + assert "replan" in seen and "perceive" in seen + assert seen["replan"] is seen["perceive"] + assert len(world.fetches) == 1 + assert seen["replan"] is world.fetches[0] + + +def test_async_idle_gates_skip_sensor_fetch(): + """When no stage module is active, the planner worker must not fetch sensors.""" + import threading + + world = _CountingWorld() + exec_ = AsyncThreadedExecuter( + perception_model=PerceptionModel(ego_vehicle=EgoState(x=0.0, y=0.0)), + world=world, + perception=None, + localization=None, + global_planner=None, + # Presence gates: without a local planner, do_replan stays false. + local_planner=None, + controller=None, + combined_perception_planning=True, + replan_dt=0.01, + ) + exec_.call_perceive = False + exec_.call_replan = True + exec_.call_localize = False + exec_.pace_replan = False # 1 ms free-run sleep so stop is observed quickly + + worker = threading.Thread(target=exec_.worker_planning, daemon=True) + worker.start() + time.sleep(0.05) + exec_.stopped = True + worker.join(timeout=2.0) + assert not worker.is_alive() + assert world.fetches == [] From f925ad3df227e4dcbb5a8581847b6f6a12fa6cc2 Mon Sep 17 00:00:00 2001 From: Cursor Agent Date: Fri, 31 Jul 2026 10:08:24 +0000 Subject: [PATCH 3/3] Cover stack_event harvest siblings, DETECTION copy, forward waypoint. Add GlobalPlan/localization harvest notify-once tests, DETECTION GT list-copy isolation from world NPCs, and update_waypoint_by_xy_forward anti-backjump behavior. Co-authored-by: Majid Khonji --- test/c40_execution/test_c43_task_strategy.py | 122 +++++++++++++++++- .../test_c54_trajectory_waypoint_update.py | 25 ++++ 2 files changed, 145 insertions(+), 2 deletions(-) diff --git a/test/c40_execution/test_c43_task_strategy.py b/test/c40_execution/test_c43_task_strategy.py index 7a4a205..c2b3174 100644 --- a/test/c40_execution/test_c43_task_strategy.py +++ b/test/c40_execution/test_c43_task_strategy.py @@ -6,8 +6,8 @@ import pytest -from avlite.c10_perception.c11_perception_model import EgoState, PerceptionModel -from avlite.c20_planning.c21_planning_model import LocalPlan +from avlite.c10_perception.c11_perception_model import AgentState, EgoState, PerceptionModel +from avlite.c20_planning.c21_planning_model import GlobalPlan, LocalPlan from avlite.c30_control.c31_control_model import AckermannControlCommand from avlite.c40_execution.c43_task_strategy import ( StackEvent, @@ -81,6 +81,15 @@ def execute(self, executer, event=None) -> None: ControlHaltedListener.calls.append(event) +class GlobalPlanMissingListener(TaskStrategy): + schedule = TaskSchedule.ON_EVENT + listen_events = frozenset({StackEvent.GLOBAL_PLAN_MISSING}) + calls: list = [] + + def execute(self, executer, event=None) -> None: + GlobalPlanMissingListener.calls.append(event) + + class NotifyDuringCycleTask(TaskStrategy): schedule = TaskSchedule.EVERY_CYCLE fired = False @@ -108,6 +117,7 @@ def _reset_task_call_state(): LocalPlanFailedListener.calls = [] ParkingZoneListener.calls = [] ControlHaltedListener.calls = [] + GlobalPlanMissingListener.calls = [] NotifyDuringCycleTask.fired = False ThreadPlacementTask.ran = False yield @@ -308,6 +318,114 @@ def test_harvest_control_stack_event_notifies_once(): assert ControlHaltedListener.calls == [] +def test_harvest_global_plan_stack_event_notifies_once(): + ego = EgoState(x=0.0, y=0.0) + pm = PerceptionModel(ego_vehicle=ego) + world = BasicSim(ego_state=ego, pm=PerceptionModel(ego_vehicle=ego)) + global_plan = GlobalPlan(stack_event=StackEvent.GLOBAL_PLAN_MISSING) + local_planner = SimpleNamespace( + replan=lambda **kwargs: None, + get_local_plan=lambda: LocalPlan(), + global_plan=global_plan, + step=lambda state: None, + stack_capabilities=frozenset(), + stack_requirements=frozenset(), + world_requirements=frozenset(), + ) + executer = SyncExecuter( + perception_model=pm, + world=world, + tasks=[GlobalPlanMissingListener()], + perception=None, + global_planner=None, + local_planner=local_planner, + controller=None, + ) + executer._replan_step(world.get_sensor_frame()) + assert GlobalPlanMissingListener.calls == [StackEvent.GLOBAL_PLAN_MISSING] + assert global_plan.stack_event is None + + GlobalPlanMissingListener.calls = [] + executer._replan_step(world.get_sensor_frame()) + assert GlobalPlanMissingListener.calls == [] + + +def test_harvest_localization_stack_event_notifies_once(): + ego = EgoState(x=0.0, y=0.0) + pm = PerceptionModel(ego_vehicle=ego) + world = BasicSim(ego_state=ego, pm=PerceptionModel(ego_vehicle=ego)) + stamped = {"done": False} + + def localize(*, perception_model=None, sensors=None): + if not stamped["done"]: + perception_model.stack_event = StackEvent.PARKING_ZONE_ENTERED + stamped["done"] = True + + localization = SimpleNamespace( + world_requirements=frozenset(), + stack_requirements=frozenset(), + stack_capabilities=frozenset({StackCapability.LOCALIZATION}), + localize=localize, + ) + executer = SyncExecuter( + perception_model=pm, + world=world, + tasks=[ParkingZoneListener()], + perception=None, + localization=localization, + global_planner=None, + local_planner=None, + controller=None, + ) + executer._localization_step(world.get_sensor_frame()) + assert ParkingZoneListener.calls == [StackEvent.PARKING_ZONE_ENTERED] + assert pm.stack_event is None + + ParkingZoneListener.calls = [] + executer._localization_step(world.get_sensor_frame()) + assert ParkingZoneListener.calls == [] + + +def test_detection_gt_copies_agents_instead_of_aliasing(): + """Clearing stack agents must never wipe the world's spawned NPC list.""" + prev = ExecutionSettings.c41_world_stack_capabilities + ego = EgoState(x=0.0, y=0.0) + world_agent = AgentState(x=10.0, y=0.0, theta=0.0, velocity=0.0, agent_id=1) + world_pm = PerceptionModel(ego_vehicle=ego, agent_vehicles=[world_agent]) + world = BasicSim(ego_state=ego, pm=world_pm) + stack_pm = PerceptionModel(ego_vehicle=ego) + perception = SimpleNamespace( + world_requirements=frozenset(), + stack_requirements=frozenset(), + stack_capabilities=frozenset(), + perceive=lambda **kwargs: None, + ) + executer = SyncExecuter( + perception_model=stack_pm, + world=world, + perception=perception, + global_planner=None, + local_planner=None, + controller=None, + ) + try: + ExecutionSettings.c41_world_stack_capabilities = None # DETECTION GT on + executer._perception_step(world.get_sensor_frame()) + assert len(stack_pm.agent_vehicles) == 1 + assert stack_pm.agent_vehicles[0].agent_id == 1 + assert stack_pm.agent_vehicles is not world_pm.agent_vehicles + + stack_pm.agent_vehicles.clear() + assert len(world_pm.agent_vehicles) == 1 + + ExecutionSettings.c41_world_stack_capabilities = [] # DETECTION GT off + executer._perception_step(world.get_sensor_frame()) + assert stack_pm.agent_vehicles == [] + assert len(world_pm.agent_vehicles) == 1 + finally: + ExecutionSettings.c41_world_stack_capabilities = prev + + def test_non_inline_placement_falls_back_to_inline(): ego = EgoState(x=0.0, y=0.0) pm = PerceptionModel(ego_vehicle=ego) diff --git a/test/c50_common/test_c54_trajectory_waypoint_update.py b/test/c50_common/test_c54_trajectory_waypoint_update.py index 11e8f2d..be59e15 100644 --- a/test/c50_common/test_c54_trajectory_waypoint_update.py +++ b/test/c50_common/test_c54_trajectory_waypoint_update.py @@ -64,3 +64,28 @@ 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_update_waypoint_by_xy_forward_never_moves_backward(): + """Global closest-point search can jump to an earlier segment; forward clamp prevents it.""" + tj = _path_tj(4) + tj.update_waypoint_by_wp(2) + assert tj.current_wp == 2 + assert tj.next_wp == 3 + + # Pose nearer the start than the current waypoint. + tj.update_waypoint_by_xy(5.0, 0.0) + assert tj.current_wp < 2 + + tj.update_waypoint_by_wp(2) + tj.update_waypoint_by_xy_forward(5.0, 0.0) + assert tj.current_wp == 2 + assert tj.next_wp == 3 + + +def test_update_waypoint_by_xy_forward_honors_min_wp_floor(): + tj = _path_tj(5) + tj.update_waypoint_by_wp(1) + tj.update_waypoint_by_xy_forward(5.0, 0.0, min_wp=3) + assert tj.current_wp == 3 + assert tj.next_wp == 4