Add regression tests for separate-thread perception and CV collision sweep

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,129 @@ 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 _stub_local_planner(replan=None):
+    return SimpleNamespace(
+        world_requirements=frozenset(),
+        stack_requirements=frozenset(),
+        stack_capabilities=frozenset(),
+        replan=replan or (lambda *, perception_model=None, sensors=None: None),
+        get_local_plan=lambda: LocalPlan(),
+        step=lambda state: None,
+        global_plan=None,
+    )
+
+
+def _stub_perception(perceive):
+    return SimpleNamespace(
+        world_requirements=frozenset(),
+        stack_requirements=frozenset(),
+        stack_capabilities=frozenset(),
+        perceive=perceive,
+    )
+
+
+def test_create_threads_adds_perception_only_when_separate():
+    """Separate-thread mode owns a Perception worker; combined mode does not."""
+    world = _CountingWorld()
+    pm = PerceptionModel(ego_vehicle=EgoState(x=0.0, y=0.0))
+    perception = _stub_perception(lambda *, perception_model=None, sensors=None: None)
+
+    separate = AsyncThreadedExecuter(
+        perception_model=pm,
+        world=world,
+        perception=perception,
+        localization=None,
+        global_planner=None,
+        local_planner=_stub_local_planner(),
+        controller=None,
+        combined_perception_planning=False,
+    )
+    names = {t.name for t in separate.threads}
+    assert "Perception" in names
+    assert separate.perception_thread is not None
+    assert separate.perception_thread in separate.threads
+    assert separate.perception_thread._target.__func__ is AsyncThreadedExecuter.worker_perception
+
+    combined = AsyncThreadedExecuter(
+        perception_model=pm,
+        world=world,
+        perception=perception,
+        localization=None,
+        global_planner=None,
+        local_planner=_stub_local_planner(),
+        controller=None,
+        combined_perception_planning=True,
+    )
+    assert "Perception" not in {t.name for t in combined.threads}
+    assert combined.perception_thread is None
+
+
+def test_worker_perception_fetches_own_snapshot():
+    """Dedicated perception worker always takes its own sensor frame."""
+    world = _CountingWorld()
+    seen: dict = {}
+
+    def perceive(*, perception_model=None, sensors=None):
+        seen["perceive"] = sensors
+        exec_.stopped = True
+
+    exec_ = AsyncThreadedExecuter(
+        perception_model=PerceptionModel(ego_vehicle=EgoState(x=0.0, y=0.0)),
+        world=world,
+        perception=_stub_perception(perceive),
+        localization=None,
+        global_planner=None,
+        local_planner=_stub_local_planner(),
+        controller=None,
+        combined_perception_planning=False,
+        perception_dt=0.01,
+    )
+    exec_.call_perceive = True
+    exec_.pace_perception = False
+
+    exec_.worker_perception()
+
+    assert "perceive" in seen
+    assert len(world.fetches) == 1
+    assert seen["perceive"] is world.fetches[0]
+
+
+def test_separate_mode_planner_does_not_perceive():
+    """With combined_perception_planning=False, planner must never run perceive."""
+    world = _CountingWorld()
+    seen: dict = {}
+
+    def perceive(*, perception_model=None, sensors=None):
+        seen["perceive"] = sensors
+
+    def replan(*, perception_model=None, sensors=None):
+        seen["replan"] = sensors
+        exec_.stopped = True
+
+    exec_ = AsyncThreadedExecuter(
+        perception_model=PerceptionModel(ego_vehicle=EgoState(x=0.0, y=0.0)),
+        world=world,
+        perception=_stub_perception(perceive),
+        localization=None,
+        global_planner=None,
+        local_planner=_stub_local_planner(replan=replan),
+        controller=None,
+        combined_perception_planning=False,
+        perception_dt=0.01,
+        replan_dt=0.01,
+    )
+    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
+    assert "perceive" not in seen
+    assert len(world.fetches) == 1
+    assert seen["replan"] is world.fetches[0]

test/c50_common/test_c55_collision_checking.py

@@ -129,3 +129,78 @@ def test_front_corner_side_overlap_detected(self):
             collision_safety_margin=margin,
         )
         assert hit is True
+
+
+class TestSlowPathConstantVelocitySweep:
+    """Slow path (no obstacle_polygons) still fabricates a CV sweep for movers.
+
+    This intentionally diverges from precompute_obstacle_polygons, which requires
+    a SingleTrajectory prediction before sweeping.
+    """
+
+    def test_slow_path_sweeps_mover_across_corridor_without_prediction(self):
+        # Agent starts clear of the corridor but drives toward it; CV sweep must hit.
+        # Precompute without prediction keeps a static box → clear.
+        agent_x, agent_y = 40.0, 8.0
+        pm = PerceptionModel(
+            ego_vehicle=EgoState(x=0.0, y=0.0, theta=0.0, velocity=5.0),
+            agent_vehicles=[
+                AgentState(
+                    x=agent_x, y=agent_y, theta=-np.pi / 2, velocity=5.0, agent_id=1,
+                ),
+            ],
+        )
+        trajectory = _straight_trajectory(0.0, 100.0)
+        # path length 100 m @ 5 m/s → total_time ≈ 20 s → predicted y ≈ 8 - 100 = -92
+        hit_slow, idx_slow, vel_slow, _ = check_collision(pm, trajectory)
+        assert hit_slow is True
+        assert idx_slow >= 0
+        assert vel_slow == 5.0
+
+        polys = precompute_obstacle_polygons(pm, total_time=20.0)
+        hit_fast, _, _, clearance = check_collision(
+            pm, trajectory, obstacle_polygons=polys,
+        )
+        assert hit_fast is False
+        assert clearance > 0
+
+    def test_slow_path_static_agent_stays_unswept(self):
+        pm = PerceptionModel(
+            ego_vehicle=EgoState(x=0.0, y=0.0, theta=0.0, velocity=5.0),
+            agent_vehicles=[
+                AgentState(x=40.0, y=8.0, theta=-np.pi / 2, velocity=0.0, agent_id=1),
+            ],
+        )
+        hit, idx, _, clearance = check_collision(pm, _straight_trajectory(0.0, 100.0))
+        assert hit is False
+        assert idx == -1
+        assert clearance > 0
+
+
+class TestDegenerateTrajectoryPoseCheck:
+    def test_none_trajectory_uses_current_pose_bbs(self):
+        pm = PerceptionModel(
+            ego_vehicle=EgoState(x=0.0, y=0.0, theta=0.0, velocity=0.0),
+            agent_vehicles=[
+                AgentState(x=0.5, y=0.0, theta=0.0, velocity=3.0, agent_id=1),
+            ],
+        )
+        hit, idx, vel, clearance = check_collision(pm, None)
+        assert hit is True
+        assert idx == 0
+        assert vel == 3.0
+        assert clearance == 0.0
+
+    def test_one_point_trajectory_skips_corridor_and_cv_sweep(self):
+        # Short path is common after end-of-path tracker fixes; movers must not be CV-swept.
+        pm = PerceptionModel(
+            ego_vehicle=EgoState(x=0.0, y=0.0, theta=0.0, velocity=5.0),
+            agent_vehicles=[
+                AgentState(x=40.0, y=8.0, theta=-np.pi / 2, velocity=5.0, agent_id=1),
+            ],
+        )
+        one_point = TrajectoryTracker(path=[(0.0, 0.0)], velocity=[5.0])
+        hit, idx, _, clearance = check_collision(pm, one_point)
+        assert hit is False
+        assert idx == -1
+        assert clearance > 1e5