diff --git a/test/c20_planning/test_c21_planning_model.py b/test/c20_planning/test_c21_planning_model.py new file mode 100644 index 0000000..11e24b8 --- /dev/null +++ b/test/c20_planning/test_c21_planning_model.py @@ -0,0 +1,84 @@ +"""Tests for GlobalPlan file validation and load-time velocity ramping.""" + +from __future__ import annotations + +import json + +import pytest + +from avlite.c20_planning.c21_planning_model import GlobalPlan +from avlite.c20_planning.c29_settings import PlanningSettings + + +def _valid_plan_payload(**overrides) -> dict: + data = { + "ReferenceLine": [[0.0, 0.0, 0.0], [10.0, 0.0, 0.0], [20.0, 0.0, 0.0]], + "ReferenceSpeed": [8.0, 8.0, 8.0], + "LeftBound": [2.0, 2.0, 2.0], + "RightBound": [-2.0, -2.0, -2.0], + } + data.update(overrides) + return data + + +def _write_plan(tmp_path, payload: dict, name: str = "plan.json"): + path = tmp_path / name + path.write_text(json.dumps(payload), encoding="utf-8") + return path + + +class TestGlobalPlanIsLoadable: + def test_accepts_valid_payload(self, tmp_path): + path = _write_plan(tmp_path, _valid_plan_payload()) + assert GlobalPlan.is_loadable(path) is True + + def test_rejects_missing_keys(self, tmp_path): + payload = _valid_plan_payload() + del payload["ReferenceSpeed"] + assert GlobalPlan.is_loadable(_write_plan(tmp_path, payload)) is False + + def test_rejects_empty_reference_line(self, tmp_path): + payload = _valid_plan_payload(ReferenceLine=[], ReferenceSpeed=[], LeftBound=[], RightBound=[]) + assert GlobalPlan.is_loadable(_write_plan(tmp_path, payload)) is False + + def test_rejects_nested_bounds(self, tmp_path): + payload = _valid_plan_payload(LeftBound=[[2.0, 0.0], [2.0, 0.0], [2.0, 0.0]]) + assert GlobalPlan.is_loadable(_write_plan(tmp_path, payload)) is False + + def test_rejects_non_json(self, tmp_path): + path = tmp_path / "plan.json" + path.write_text("not-json", encoding="utf-8") + assert GlobalPlan.is_loadable(path) is False + + def test_rejects_wrong_suffix(self, tmp_path): + path = _write_plan(tmp_path, _valid_plan_payload(), name="plan.txt") + assert GlobalPlan.is_loadable(path) is False + + +class TestGlobalPlanFromFile: + def test_loads_path_velocity_and_frenet_bounds(self, tmp_path): + path = _write_plan(tmp_path, _valid_plan_payload()) + plan = GlobalPlan.from_file(path) + assert list(plan.start_point) == [0.0, 0.0] + assert list(plan.goal_point) == [20.0, 0.0] + assert [list(p) for p in plan.path] == [[0.0, 0.0], [10.0, 0.0], [20.0, 0.0]] + assert plan.velocity == [8.0, 8.0, 8.0] + assert plan.left_boundary_d == [2.0, 2.0, 2.0] + assert plan.right_boundary_d == [-2.0, -2.0, -2.0] + assert plan.trajectory is not None + assert len(plan.left_boundary_x) == 3 + assert plan.left_boundary_y[0] == pytest.approx(2.0) + + def test_zero_start_speed_is_replaced_with_min_ramp(self, tmp_path): + payload = _valid_plan_payload(ReferenceSpeed=[0.0, 8.0, 8.0]) + plan = GlobalPlan.from_file(_write_plan(tmp_path, payload)) + assert plan.velocity[0] == pytest.approx(PlanningSettings.c20_min_ramp_start_velocity) + assert plan.velocity[1:] == [8.0, 8.0] + assert plan.trajectory.velocity[0] == pytest.approx( + PlanningSettings.c20_min_ramp_start_velocity + ) + + def test_negative_start_speed_is_replaced_with_min_ramp(self, tmp_path): + payload = _valid_plan_payload(ReferenceSpeed=[-1.0, 5.0, 5.0]) + plan = GlobalPlan.from_file(_write_plan(tmp_path, payload)) + assert plan.velocity[0] == pytest.approx(PlanningSettings.c20_min_ramp_start_velocity) diff --git a/test/c20_planning/test_c24_global_hdmap_planners.py b/test/c20_planning/test_c24_global_hdmap_planners.py new file mode 100644 index 0000000..9109fef --- /dev/null +++ b/test/c20_planning/test_c24_global_hdmap_planners.py @@ -0,0 +1,131 @@ +"""Tests for HD-map global planner geometry helpers and plan() edge cases. + +chop_path / chop_path_from_two_sides encode OpenDRIVE lane-id sign (negative +lanes travel increasing s; positive lanes are reversed). A regression here +emits a reversed or truncated global path. _inset_d and smoothen_path_savgol +keep corridor boundaries and velocity samples aligned with the reference. +""" + +from __future__ import annotations + +import numpy as np +import pytest + +from avlite.c10_perception.c11_perception_model import HDMap +from avlite.c20_planning.c21_planning_model import GlobalPlan +from avlite.c20_planning.c24_global_hdmap_planners import ( + HDMapGlobalPlanner, + _inset_d, + chop_path, + chop_path_from_two_sides, + smoothen_path_savgol, +) +from avlite.c20_planning.c29_settings import PlanningSettings + + +def _xy_path(n: int = 10) -> np.ndarray: + return np.column_stack([np.arange(n, dtype=float), np.zeros(n)]) + + +class TestInsetD: + def test_margin_shrinks_corridor_inward(self): + left, right = _inset_d(1.75, -1.75, 0.25) + assert left == pytest.approx(1.5) + assert right == pytest.approx(-1.5) + + +class TestChopPath: + def test_start_of_negative_lane_keeps_tail(self): + path = _xy_path(10) + chopped = chop_path(path, lane_id=-1, idx=3, start=True) + np.testing.assert_array_equal(chopped, path[3:]) + + def test_end_of_negative_lane_keeps_head(self): + path = _xy_path(10) + chopped = chop_path(path, lane_id=-1, idx=3, start=False) + np.testing.assert_array_equal(chopped, path[:4]) + + def test_start_of_positive_lane_reverses_after_chop(self): + path = _xy_path(10) + chopped = chop_path(path, lane_id=1, idx=3, start=True) + # Positive lanes travel decreasing s, so the driving-direction prefix + # is path[:idx+1] reversed. + np.testing.assert_array_equal(chopped, path[:4][::-1]) + + def test_end_of_positive_lane_reverses_after_chop(self): + path = _xy_path(10) + chopped = chop_path(path, lane_id=1, idx=3, start=False) + np.testing.assert_array_equal(chopped, path[3:][::-1]) + + +class TestChopPathFromTwoSides: + def test_negative_lane_slices_inclusive(self): + path = _xy_path(10) + chopped = chop_path_from_two_sides(path, lane_id=-1, s_idx=2, g_idx=6) + np.testing.assert_array_equal(chopped, path[2:7]) + + def test_positive_lane_slices_then_reverses(self): + path = _xy_path(10) + chopped = chop_path_from_two_sides(path, lane_id=1, s_idx=6, g_idx=2) + np.testing.assert_array_equal(chopped, path[2:7][::-1]) + + +class TestSmoothenPathSavgol: + def test_short_path_is_unchanged(self): + plan = GlobalPlan( + path=[(0.0, 0.0)], + velocity=[5.0], + left_boundary_d=[1.0], + right_boundary_d=[-1.0], + ) + out = smoothen_path_savgol(plan) + assert out.path == [(0.0, 0.0)] + assert out.velocity == [5.0] + + def test_near_duplicates_drop_and_keep_arrays_aligned(self): + plan = GlobalPlan( + path=[(0.0, 0.0), (0.1, 0.0), (2.0, 0.0), (4.0, 0.0), (6.0, 0.0)], + velocity=[3.0, 4.0, 5.0, 6.0, 7.0], + left_boundary_d=[1.5, 1.5, 1.5, 1.5, 1.5], + right_boundary_d=[-1.5, -1.5, -1.5, -1.5, -1.5], + ) + out = smoothen_path_savgol(plan, min_spacing=0.5, window_length=3, polyorder=1) + assert len(out.path) == len(out.velocity) == len(out.left_boundary_d) == len(out.right_boundary_d) + assert len(out.path) == 4 # (0.1, 0) is within min_spacing of origin + assert out.velocity[0] == pytest.approx(3.0) + assert out.velocity[-1] == pytest.approx(7.0) + + +class TestHDMapGlobalPlanner: + def test_plan_without_start_goal_returns_none(self, minimal_opendrive_path): + hdmap = HDMap.from_path(minimal_opendrive_path) + planner = HDMapGlobalPlanner(hdmap) + assert planner.plan() is None + + def test_plan_on_single_lane_fixture_is_monotonic(self, minimal_opendrive_path): + hdmap = HDMap.from_path(minimal_opendrive_path) + # Isolated fixture roads are parsed but not graph-connected (empty ). + # Seed nodes so plan() can Dijkstra a same-lane route. + for road in hdmap.roads: + hdmap.road_network.add_node(road.id) + for lane in hdmap.lanes: + hdmap.lane_network.add_node(lane.uid) + planner = HDMapGlobalPlanner(hdmap, max_velocity=10.0, wp_to_full_velocity=5) + # Lane -1 (right) center sits near y = -width/2 on this straight road. + planner.set_start_goal((10.0, -1.75), (80.0, -1.75)) + plan = planner.plan() + assert plan is not None + assert len(plan.path) >= 2 + xs = [p[0] for p in plan.path] + assert xs[-1] > xs[0] + assert len(plan.velocity) == len(plan.path) + assert len(plan.left_boundary_d) == len(plan.path) + assert plan.trajectory is not None + assert plan.race_mode is False + margin = PlanningSettings.c20_boundary_margin + assert plan.left_boundary_d[0] == pytest.approx(1.75 - margin) + assert plan.right_boundary_d[0] == pytest.approx(-1.75 + margin) + assert plan.velocity[0] == pytest.approx(PlanningSettings.c20_min_ramp_start_velocity) + assert max(plan.velocity) == pytest.approx(10.0) + # Decel samples sit closer than smoothen min_spacing, so the terminal + # 0 m/s waypoint can be dropped; the ramp start must still be kept. diff --git a/test/c30_control/test_c35_pure_pursuit.py b/test/c30_control/test_c35_pure_pursuit.py index bbd422e..0da2655 100644 --- a/test/c30_control/test_c35_pure_pursuit.py +++ b/test/c30_control/test_c35_pure_pursuit.py @@ -171,4 +171,62 @@ def test_path_bias_prefers_path_aligned_gap(self): no_path = FollowTheGapController(tj=None, setting=_pp_settings()) cmd_wide = no_path.control(ego, sensors=SensorFrame(lidar=lidar)) assert cmd_wide.steer > cmd.steer - assert cmd_wide.steer > 0.0 \ No newline at end of file + assert cmd_wide.steer > 0.0 + + def test_z_band_filter_drops_out_of_range_hits(self): + """3D returns outside c35_lidar_z_* must not contribute to gap selection.""" + setting = _pp_settings(c35_lidar_z_min=-1.5, c35_lidar_z_max=2.0) + controller = FollowTheGapController(setting=setting) + ego = EgoState(x=0.0, y=0.0, theta=0.0, velocity=5.0) + # In-band interior gap slightly left of center; out-of-band wall on the right. + in_band = _lidar_at_angles(np.array([-0.15, 0.35])) + in_band[:, 2] = 0.0 + out_band = _lidar_at_angles(np.linspace(-1.0, -0.2, 12)) + out_band[:, 2] = 8.0 + cmd = controller.control(ego, sensors=SensorFrame(lidar=np.vstack([in_band, out_band]))) + assert cmd.steer > 0.0 + + def test_all_points_outside_z_band_returns_zero(self): + controller = FollowTheGapController(setting=_pp_settings()) + ego = EgoState(x=0.0, y=0.0, theta=0.0, velocity=0.0) + lidar = _lidar_at_angles(np.array([-0.3, 0.0, 0.3])) + lidar[:, 2] = 10.0 + cmd = controller.control(ego, sensors=SensorFrame(lidar=lidar)) + assert cmd.steer == 0.0 + assert cmd.acceleration == 0.0 + + def test_safety_bubble_ignores_near_returns(self): + setting = _pp_settings(c35_bubble_radius=1.5, c35_lookahead_distance=8.0) + controller = FollowTheGapController(setting=setting) + ego = EgoState(x=0.0, y=0.0, theta=0.0, velocity=5.0) + # Close-in clutter at 0.2 m would otherwise dominate bearings. + near = _lidar_at_angles(np.array([-0.8, 0.8]), ranges=0.2) + far = _lidar_at_angles(np.array([-0.2, 0.2]), ranges=6.0) + cmd = controller.control(ego, sensors=SensorFrame(lidar=np.vstack([near, far]))) + assert cmd.steer == pytest.approx(0.0, abs=0.1) + + def test_only_bubble_interior_hits_returns_zero(self): + setting = _pp_settings(c35_bubble_radius=2.0) + controller = FollowTheGapController(setting=setting) + ego = EgoState(x=0.0, y=0.0, theta=0.0, velocity=0.0) + lidar = _lidar_at_angles(np.array([-0.4, 0.0, 0.4]), ranges=0.5) + cmd = controller.control(ego, sensors=SensorFrame(lidar=lidar)) + assert cmd.steer == 0.0 + assert cmd.acceleration == 0.0 + + def test_prefers_interior_gap_over_wider_edge(self): + """±90° edge openings are wider than a narrow corridor; interior must win.""" + controller = FollowTheGapController(setting=_pp_settings()) + ego = EgoState(x=0.0, y=0.0, theta=0.0, velocity=5.0) + # Two returns straddle center: interior gap ~0.3 rad, each edge ~1.4 rad. + lidar = _lidar_at_angles(np.array([-0.15, 0.15])) + cmd = controller.control(ego, sensors=SensorFrame(lidar=lidar)) + assert cmd.steer == pytest.approx(0.0, abs=0.08) + + def test_no_forward_returns_command_zero(self): + controller = FollowTheGapController(setting=_pp_settings()) + ego = EgoState(x=0.0, y=0.0, theta=0.0, velocity=0.0) + lidar = _lidar_at_angles(np.array([np.pi * 0.8, np.pi, -np.pi * 0.8])) + cmd = controller.control(ego, sensors=SensorFrame(lidar=lidar)) + assert cmd.steer == 0.0 + assert cmd.acceleration == 0.0 \ No newline at end of file diff --git a/test/c40_execution/test_c47_execution_tasks.py b/test/c40_execution/test_c47_execution_tasks.py new file mode 100644 index 0000000..b93f193 --- /dev/null +++ b/test/c40_execution/test_c47_execution_tasks.py @@ -0,0 +1,81 @@ +"""GoalArrivalMonitor / StopExecAtGoalTask guards (c47). + +These live in a dedicated module so they do not collide with open drafts that +already append to test_c43_task_strategy.py. +""" + +from __future__ import annotations + +from types import SimpleNamespace + +from avlite.c40_execution.c43_task_strategy import StackEvent, TaskRunner +from avlite.c40_execution.c47_execution_tasks import GoalArrivalMonitor, StopExecAtGoalTask + + +class _Exec: + def __init__(self, *, x=0.0, y=0.0, local_planner=None): + self.ego_state = SimpleNamespace(x=x, y=y) + self.local_planner = local_planner + self.stopped = False + self.task_runner = None + self.elapsed_sim_time = 0.0 + + def dispatch_task(self, task, event=None): + task.execute(self, event=event) + + def stop(self): + self.stopped = True + + +def _planner_with_goal(goal): + return SimpleNamespace(global_plan=SimpleNamespace(goal_point=goal)) + + +def test_goal_monitor_does_not_fire_without_local_planner(): + monitor = GoalArrivalMonitor() + executer = _Exec(x=0.0, y=0.0, local_planner=None) + runner = TaskRunner([monitor, StopExecAtGoalTask()], executer=executer) + executer.task_runner = runner + runner.step(executer) + assert executer.stopped is False + + +def test_goal_monitor_does_not_fire_without_goal_point(): + monitor = GoalArrivalMonitor() + executer = _Exec(x=0.0, y=0.0, local_planner=_planner_with_goal(None)) + runner = TaskRunner([monitor, StopExecAtGoalTask()], executer=executer) + executer.task_runner = runner + runner.step(executer) + assert executer.stopped is False + + +def test_goal_monitor_does_not_fire_on_short_goal_point(): + monitor = GoalArrivalMonitor() + executer = _Exec(x=0.0, y=0.0, local_planner=_planner_with_goal((1.0,))) + runner = TaskRunner([monitor, StopExecAtGoalTask()], executer=executer) + executer.task_runner = runner + runner.step(executer) + assert executer.stopped is False + + +def test_goal_monitor_reset_allows_rising_edge_again(): + monitor = GoalArrivalMonitor() + planner = _planner_with_goal((10.0, 0.0)) + executer = _Exec(x=10.0, y=0.0, local_planner=planner) + runner = TaskRunner([monitor, StopExecAtGoalTask()], executer=executer) + executer.task_runner = runner + + runner.step(executer) + assert executer.stopped is True + + executer.stopped = False + runner.step(executer) + assert executer.stopped is False # still inside radius; edge already consumed + + monitor.reset() + runner.step(executer) + assert executer.stopped is True + + +def test_stop_exec_at_goal_listens_only_for_goal_arrived(): + assert StopExecAtGoalTask.listen_events == frozenset({StackEvent.GOAL_ARRIVED})