Add regression tests for start pose and Frenet XY lookup

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

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

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)

test/c60_apps/test_c62_factory_smoke.py

@@ -179,6 +179,59 @@ 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="",
+    )
+
+    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 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):
+    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 = ""