Add regression tests for predictor sweeps, async snapshot, and harvest siblings

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 == []

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)

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

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