Add regression tests for HD-map chopping, plan load validation, and FTG safety

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)

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 <link/>).
+        # 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.

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
+        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

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})