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..99b48f3 100644 --- a/test/c60_apps/test_c62_factory_smoke.py +++ b/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 = ""