From c394e9fe56ab5a8add8702a378b6975530598916 Mon Sep 17 00:00:00 2001 From: Cursor Agent Date: Thu, 30 Jul 2026 10:02:36 +0000 Subject: [PATCH 1/2] Add regression tests for start pose and Frenet XY lookup. Cover State.set_start/reset, BasicSim pose restore without despawn, factory c40_start_pose wiring, and convert_sd_to_xy extrapolation before the path start. Co-authored-by: Majid Khonji --- .../test_c11_state_start_reset.py | 52 +++++++++++++++++++ .../c40_execution/test_c46_basic_sim_spawn.py | 44 ++++++++++++++++ .../test_c54_trajectory_waypoint_update.py | 27 ++++++++++ test/c60_apps/test_c62_factory_smoke.py | 50 ++++++++++++++++++ 4 files changed, 173 insertions(+) create mode 100644 test/c10_perception/test_c11_state_start_reset.py diff --git a/test/c10_perception/test_c11_state_start_reset.py b/test/c10_perception/test_c11_state_start_reset.py new file mode 100644 index 0000000..fb91cba --- /dev/null +++ b/test/c10_perception/test_c11_state_start_reset.py @@ -0,0 +1,52 @@ +"""Regression tests for State.set_start / reset (profile start pose).""" + +import math + +from avlite.c10_perception.c11_perception_model import AgentState, AgentType, EgoState, State + + +def test_state_reset_restores_construction_pose(): + state = State(x=1.0, y=2.0, theta=0.5) + state.x, state.y, state.theta = 9.0, 8.0, -1.0 + state.reset() + assert state.x == 1.0 + assert state.y == 2.0 + assert state.theta == 0.5 + + +def test_set_start_updates_reset_snapshot(): + state = State(x=0.0, y=0.0, theta=0.0) + state.x, state.y, state.theta = 3.0, 4.0, math.pi / 4 + state.set_start() + state.x, state.y, state.theta = 0.0, 0.0, 0.0 + state.reset() + assert state.x == 3.0 + assert state.y == 4.0 + assert state.theta == math.pi / 4 + + +def test_agent_reset_restores_velocity_and_type(): + """Polymorphic copy_from must restore AgentState fields, not only x/y/theta.""" + agent = AgentState(x=1.0, y=2.0, theta=0.3, velocity=5.0, agent_type=AgentType.DIFF_DRIVE) + agent.x, agent.y, agent.theta = 10.0, 20.0, 1.0 + agent.velocity = 0.0 + agent.agent_type = AgentType.PEDESTRIAN + agent.reset() + assert agent.x == 1.0 + assert agent.y == 2.0 + assert agent.theta == 0.3 + assert agent.velocity == 5.0 + assert agent.agent_type == AgentType.DIFF_DRIVE + + +def test_set_start_then_reset_preserves_agent_identity(): + ego = EgoState(x=0.0, y=0.0, velocity=1.0) + original_id = id(ego) + ego.x, ego.y, ego.velocity = 7.0, 8.0, 3.0 + ego.set_start() + ego.x, ego.y, ego.velocity = 0.0, 0.0, 0.0 + ego.reset() + assert id(ego) == original_id + assert ego.x == 7.0 + assert ego.y == 8.0 + assert ego.velocity == 3.0 diff --git a/test/c40_execution/test_c46_basic_sim_spawn.py b/test/c40_execution/test_c46_basic_sim_spawn.py index 32bda14..3bf8b6a 100644 --- a/test/c40_execution/test_c46_basic_sim_spawn.py +++ b/test/c40_execution/test_c46_basic_sim_spawn.py @@ -78,3 +78,47 @@ def test_basic_sim_stack_requirements_control_readable_from_class(): assert StackCapability.DETECTION in BasicSim.stack_capabilities assert StackCapability.TRACKING in BasicSim.stack_capabilities assert StackCapability.LOCALIZATION in BasicSim.stack_capabilities + + +def test_spawn_agent_captures_start_pose_including_plan_velocity(): + ego = EgoState() + pm = PerceptionModel(ego_vehicle=ego) + sim = BasicSim(ego_state=ego, pm=pm) + plan = _straight_global_plan() + + agent = AgentState(x=1.0, y=0.0, theta=0.0, velocity=0.0) + sim.spawn_agent(agent, global_plan=plan) + spawn_velocity = agent.velocity + assert spawn_velocity > 0.0 + + agent.x, agent.y, agent.velocity = 50.0, 50.0, 0.0 + agent.reset() + assert agent.x == 1.0 + assert agent.y == 0.0 + assert agent.velocity == spawn_velocity + + +def test_basic_sim_reset_restores_ego_and_npcs_without_despawning(): + """reset must restore start poses; it must not clear spawned agents (post-0.5.3).""" + ego = EgoState(x=0.0, y=0.0, theta=0.0, velocity=0.0) + pm = PerceptionModel(ego_vehicle=ego) + sim = BasicSim(ego_state=ego, pm=pm) + plan = _straight_global_plan() + + agent = AgentState(x=2.0, y=0.0, theta=0.0, velocity=0.0) + sim.spawn_agent(agent, global_plan=plan) + agent_id = agent.agent_id + spawn_velocity = agent.velocity + + ego.x, ego.y, ego.theta, ego.velocity = 15.0, 4.0, 1.2, 7.0 + agent.x, agent.y, agent.theta, agent.velocity = 18.0, -3.0, -0.5, 0.1 + + sim.reset() + + assert len(pm.agent_vehicles) == 1 + assert pm.agent_vehicles[0] is agent + assert agent.agent_id == agent_id + assert agent_id in sim.npc_controllers + assert ego.x == 0.0 and ego.y == 0.0 and ego.theta == 0.0 and ego.velocity == 0.0 + assert agent.x == 2.0 and agent.y == 0.0 and agent.theta == 0.0 + assert agent.velocity == spawn_velocity diff --git a/test/c50_common/test_c54_trajectory_waypoint_update.py b/test/c50_common/test_c54_trajectory_waypoint_update.py index 11e8f2d..585f15c 100644 --- a/test/c50_common/test_c54_trajectory_waypoint_update.py +++ b/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,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_convert_sd_to_xy_extrapolates_before_path_start(): + """s before path start must extrapolate along the first segment, not snap to wp0.""" + tj = _path_tj(3) + x, y = tj.convert_sd_to_xy(-5.0, 0.0) + assert x == pytest.approx(-5.0) + assert y == pytest.approx(0.0) + + +def test_convert_sd_to_xy_interpolates_mid_segment_with_lateral_offset(): + tj = _path_tj(3) + x, y = tj.convert_sd_to_xy(5.0, 1.0) + assert x == pytest.approx(5.0) + assert y == pytest.approx(1.0) + + +def test_convert_sd_to_xy_extrapolates_on_angled_first_segment(): + path = [(0.0, 0.0), (10.0, 10.0), (20.0, 10.0)] + tj = TrajectoryTracker(path=path, velocity=[1.0, 1.0, 1.0]) + # First segment length is 10*sqrt(2); go half a segment length before start. + s = -0.5 * math.hypot(10.0, 10.0) + x, y = tj.convert_sd_to_xy(s, 0.0) + assert x == pytest.approx(-5.0) + assert y == pytest.approx(-5.0) diff --git a/test/c60_apps/test_c62_factory_smoke.py b/test/c60_apps/test_c62_factory_smoke.py index ff42d15..e7a06b8 100644 --- a/test/c60_apps/test_c62_factory_smoke.py +++ b/test/c60_apps/test_c62_factory_smoke.py @@ -179,6 +179,56 @@ def test_executor_factory_empty_global_plan(minimal_corridor_map_path): assert executer.ego_state.y == 0.0 +def test_executor_factory_uses_c40_start_pose_for_world_and_stack_ego(minimal_corridor_map_path): + ExecutionSettings.c40_map = str(minimal_corridor_map_path.resolve()) + ExecutionSettings.c40_mapping = MapReader.__name__ + ExecutionSettings.c40_start_pose = [12.5, -3.25, 0.75] + + executer = executor_factory( + load_plugins=False, + executer_type=SyncExecuter.__name__, + bridge="BasicSim", + perception_strategy_name="", + localization_strategy_name="", + mapping_strategy_name=MapReader.__name__, + global_planner_strategy_name="GlobalCenterlineRacePlanner", + local_planner_strategy_name="", + controller_strategy_name="", + default_global_trajectory_file="", + ) + + assert executer.ego_state.x == pytest.approx(12.5) + assert executer.ego_state.y == pytest.approx(-3.25) + assert executer.ego_state.theta == pytest.approx(0.75) + assert executer.pm.ego_vehicle.x == pytest.approx(12.5) + assert executer.pm.ego_vehicle.y == pytest.approx(-3.25) + assert executer.pm.ego_vehicle.theta == pytest.approx(0.75) + # World/stack ego must remain distinct objects sharing the start pose. + assert executer.ego_state is not executer.pm.ego_vehicle + + +def test_executor_factory_empty_start_pose_falls_back_to_plan_start(minimal_corridor_map_path): + ExecutionSettings.c40_map = str(minimal_corridor_map_path.resolve()) + ExecutionSettings.c40_mapping = MapReader.__name__ + ExecutionSettings.c40_start_pose = [] + + executer = executor_factory( + load_plugins=False, + executer_type=SyncExecuter.__name__, + bridge="BasicSim", + perception_strategy_name="", + localization_strategy_name="", + mapping_strategy_name=MapReader.__name__, + global_planner_strategy_name="GlobalCenterlineRacePlanner", + local_planner_strategy_name="", + controller_strategy_name="", + default_global_trajectory_file="", + ) + + assert executer.ego_state.x == 0.0 + assert executer.ego_state.y == 0.0 + + def test_executor_factory_raises_for_map_reader_without_map(): ExecutionSettings.c40_map = "" From bd0d975abb61e9092efbc155ff4467e0772613b5 Mon Sep 17 00:00:00 2001 From: Cursor Agent Date: Thu, 30 Jul 2026 10:02:56 +0000 Subject: [PATCH 2/2] Fix factory start-pose test to compare world vs stack ego. executer.ego_state aliases pm.ego_vehicle; assert distinct plant ego via world.get_ego_state(). Co-authored-by: Majid Khonji --- test/c60_apps/test_c62_factory_smoke.py | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/test/c60_apps/test_c62_factory_smoke.py b/test/c60_apps/test_c62_factory_smoke.py index e7a06b8..99b48f3 100644 --- a/test/c60_apps/test_c62_factory_smoke.py +++ b/test/c60_apps/test_c62_factory_smoke.py @@ -197,14 +197,17 @@ def test_executor_factory_uses_c40_start_pose_for_world_and_stack_ego(minimal_co default_global_trajectory_file="", ) - assert executer.ego_state.x == pytest.approx(12.5) - assert executer.ego_state.y == pytest.approx(-3.25) - assert executer.ego_state.theta == pytest.approx(0.75) - assert executer.pm.ego_vehicle.x == pytest.approx(12.5) - assert executer.pm.ego_vehicle.y == pytest.approx(-3.25) - assert executer.pm.ego_vehicle.theta == pytest.approx(0.75) + world_ego = executer.world.get_ego_state() + stack_ego = executer.pm.ego_vehicle + assert world_ego.x == pytest.approx(12.5) + assert world_ego.y == pytest.approx(-3.25) + assert world_ego.theta == pytest.approx(0.75) + assert stack_ego.x == pytest.approx(12.5) + assert stack_ego.y == pytest.approx(-3.25) + assert stack_ego.theta == pytest.approx(0.75) # World/stack ego must remain distinct objects sharing the start pose. - assert executer.ego_state is not executer.pm.ego_vehicle + assert world_ego is not stack_ego + assert executer.ego_state is stack_ego def test_executor_factory_empty_start_pose_falls_back_to_plan_start(minimal_corridor_map_path):