Fix agent-cap wipe, Town03 map crash, and short HDMap smoothing

avlite/c10_perception/c11_perception_model.py

@@ -48,9 +48,15 @@ class PerceptionModel:
 
     def add_agent_vehicle(self, agent: AgentState) -> int: # return agent_id
         """ Add an agent vehicle to the perception model and assign a unique agent_id."""
-        if len(self.agent_vehicles) == self.max_agent_vehicles:
-            log.info("Max num of agent reached. Deleteing Old agents")
-            self.agent_vehicles = []
+        if self.max_agent_vehicles <= 0:
+            log.info("Max num of agents is %s; not adding", self.max_agent_vehicles)
+            return -1
+        while len(self.agent_vehicles) >= self.max_agent_vehicles:
+            evicted = self.agent_vehicles.pop(0)
+            log.info(
+                "Max num of agents reached. Deleting oldest agent %s",
+                evicted.agent_id,
+            )
         ids = {a.agent_id for a in self.agent_vehicles}
         agent.agent_id = next(i for i in range(1, len(ids) + 2) if i not in ids)
         self.agent_vehicles.append(agent)
@@ -415,30 +421,49 @@ def find_nearest_lane_and_idx(self, x: float, y: float) -> tuple[Lane | None, in
         idx = int(np.argmin(dists))
         return lane, idx
 
+    @staticmethod
+    def _center_col(center: np.ndarray, idx: int) -> np.ndarray:
+        """Index a centerline column, clamping short junction polylines."""
+        n = center.shape[1]
+        if idx >= 0:
+            idx = min(idx, n - 1)
+        else:
+            idx = max(idx, -n)
+        return center[:, idx]
+
     def can_laneA_access_laneB(self, lane_a: Lane, lane_b: Lane) -> bool:
+        if (
+            lane_a.center_line.size == 0
+            or lane_b.center_line.size == 0
+            or lane_a.center_line.ndim < 2
+            or lane_b.center_line.ndim < 2
+            or lane_a.center_line.shape[1] < 2
+            or lane_b.center_line.shape[1] < 2
+        ):
+            return False
         check1 = lane_b in lane_a.neighbors
-        b_start_end = [lane_b.center_line[:, 0], lane_b.center_line[:, -1]]
-        a = lane_a.center_line[:, -1] if int(lane_a.id) < 0 else lane_a.center_line[:, 0]
+        b_start_end = [self._center_col(lane_b.center_line, 0), self._center_col(lane_b.center_line, -1)]
+        a = self._center_col(lane_a.center_line, -1) if int(lane_a.id) < 0 else self._center_col(lane_a.center_line, 0)
         dists = [(np.linalg.norm(a - b), j) for j, b in enumerate(b_start_end)]
         min_dist, b_idx = min(dists, key=lambda item: item[0])
         check2 = min_dist < 0.5
         check3 = False
         if check2:
             if int(lane_a.id) < 0:
-                vec_a = lane_a.center_line[:, -3] - lane_a.center_line[:, -1]
+                vec_a = self._center_col(lane_a.center_line, -3) - self._center_col(lane_a.center_line, -1)
             else:
-                vec_a = lane_a.center_line[:, 2] - lane_a.center_line[:, 0]
+                vec_a = self._center_col(lane_a.center_line, 2) - self._center_col(lane_a.center_line, 0)
             if int(lane_b.id) < 0:
                 vec_b = (
-                    lane_b.center_line[:, 0] - lane_b.center_line[:, 2]
+                    self._center_col(lane_b.center_line, 0) - self._center_col(lane_b.center_line, 2)
                     if b_idx == 0
-                    else lane_b.center_line[:, -1] - lane_b.center_line[:, -3]
+                    else self._center_col(lane_b.center_line, -1) - self._center_col(lane_b.center_line, -3)
                 )
             else:
                 vec_b = (
-                    lane_b.center_line[:, 1] - lane_b.center_line[:, 0]
+                    self._center_col(lane_b.center_line, 1) - self._center_col(lane_b.center_line, 0)
                     if b_idx == 0
-                    else lane_b.center_line[:, -1] - lane_b.center_line[:, -3]
+                    else self._center_col(lane_b.center_line, -1) - self._center_col(lane_b.center_line, -3)
                 )
             norm_a = np.linalg.norm(vec_a)
             norm_b = np.linalg.norm(vec_b)

avlite/c10_perception/c18_hdmap_parser.py

@@ -358,13 +358,17 @@ def _connect_lanes(hdmap: HDMap) -> None:
             for pred_road in lane.road.predecessors:
                 pred_uid = f"{pred_road.id}_{lane.pred_id}"
                 pred_lane = lane_by_uid.get(pred_uid)
+                if pred_lane is None:
+                    continue
                 lane.neighbors.add(pred_lane)
                 pred_lane.neighbors.add(lane)
 
         if lane.succ_id and lane.succ_type == "lane":
             for succ_road in lane.road.successors:
                 succ_uid = f"{succ_road.id}_{lane.succ_id}"
                 succ_lane = lane_by_uid.get(succ_uid)
+                if succ_lane is None:
+                    continue
                 lane.neighbors.add(succ_lane)
                 succ_lane.neighbors.add(lane)
 

avlite/c20_planning/c24_global_hdmap_planners.py

@@ -290,12 +290,17 @@ def smoothen_path_savgol(plan: GlobalPlan, min_spacing=0.5, window_length=7, pol
 
     n = len(cleaned_path)
     if n >= 3:
-        if window_length >= n:
-            window_length = n // 2 * 2 + 1  # Make it a valid odd number
-        path_np = np.array(cleaned_path)
-        x_smooth = savgol_filter(path_np[:, 0], window_length, polyorder, mode="interp")
-        y_smooth = savgol_filter(path_np[:, 1], window_length, polyorder, mode="interp")
-        cleaned_path = list(zip(x_smooth, y_smooth))
+        max_odd = n if n % 2 == 1 else n - 1
+        if window_length > max_odd:
+            window_length = max_odd
+        if window_length % 2 == 0:
+            window_length -= 1
+        polyorder = min(polyorder, window_length - 1)
+        if window_length >= 3 and polyorder >= 1:
+            path_np = np.array(cleaned_path)
+            x_smooth = savgol_filter(path_np[:, 0], window_length, polyorder, mode="interp")
+            y_smooth = savgol_filter(path_np[:, 1], window_length, polyorder, mode="interp")
+            cleaned_path = list(zip(x_smooth, y_smooth))
 
     plan.path = cleaned_path
     plan.velocity = cleaned_velocity

test/c10_perception/test_c11_agent_ids.py

@@ -32,6 +32,29 @@ def test_add_agent_skips_zero():
     assert all(a.agent_id >= 1 for a in pm.agent_vehicles)
 
 
+def test_add_agent_evicts_oldest_when_cap_reached():
+    """Over-cap adds must not wipe the whole detection set (FastBEV / spawn)."""
+    pm = PerceptionModel()
+    pm.max_agent_vehicles = 3
+    kept_xy = []
+    for i in range(5):
+        pm.add_agent_vehicle(AgentState(x=float(i), y=0.0))
+        kept_xy.append((float(i), 0.0))
+        assert 1 <= len(pm.agent_vehicles) <= 3
+
+    assert len(pm.agent_vehicles) == 3
+    xs = [a.x for a in pm.agent_vehicles]
+    assert xs == [2.0, 3.0, 4.0]
+    assert all(a.agent_id >= 1 for a in pm.agent_vehicles)
+
+
+def test_add_agent_refuses_non_positive_cap():
+    pm = PerceptionModel()
+    pm.max_agent_vehicles = 0
+    assert pm.add_agent_vehicle(AgentState(x=1.0)) == -1
+    assert pm.agent_vehicles == []
+
+
 @dataclass
 class _StubBridge(WorldBridge, abstract=True):
     ego_state: EgoState = field(default_factory=EgoState)

test/c10_perception/test_c18_hdmap_parser.py

@@ -33,6 +33,46 @@ def test_parses_road_and_driving_lanes(self, minimal_opendrive_path):
         assert len(driving) >= 2
         assert all(len(lane.center_line) > 0 for lane in driving)
 
+    def test_short_centerline_access_check_does_not_index_error(self):
+        """CARLA junction stubs often have only 2 sampled points."""
+        hdmap = HDMap()
+        lane_a = HDMap.Lane(
+            id=-1,
+            uid="a_-1",
+            lane_element=ET.Element("lane"),
+            center_line=np.array([[0.0, 1.0], [0.0, 0.0]]),
+        )
+        lane_b = HDMap.Lane(
+            id=-1,
+            uid="b_-1",
+            lane_element=ET.Element("lane"),
+            center_line=np.array([[1.0, 2.0], [0.0, 0.0]]),
+        )
+        lane_a.neighbors.add(lane_b)
+        hdmap.can_laneA_access_laneB(lane_a, lane_b)
+        empty = HDMap.Lane(id=-1, uid="empty", lane_element=ET.Element("lane"))
+        assert hdmap.can_laneA_access_laneB(lane_a, empty) is False
+
+    def test_unresolved_lane_link_does_not_crash(self):
+        """Town03-style sidewalk/missing predecessor must not None-deref neighbors."""
+        from pathlib import Path
+
+        fixture = Path(__file__).resolve().parents[1] / "fixtures" / "opendrive_unresolved_lane_link.xodr"
+        hdmap = HDMap.from_path(fixture)
+        driving = [lane for lane in hdmap.lanes if lane.type == "driving"]
+        assert len(driving) == 1
+        assert None not in driving[0].neighbors
+
+    def test_bundled_town03_loads(self):
+        from pathlib import Path
+
+        town03 = Path(__file__).resolve().parents[2] / "avlite" / "data" / "Town03_Opt.xodr"
+        hdmap = HDMap.from_path(town03)
+        assert len(hdmap.roads) > 0
+        assert all(lane is not None for lane in hdmap.lanes)
+        for lane in hdmap.lanes:
+            assert None not in lane.neighbors
+
     def test_reference_point_from_geo_reference(self, minimal_opendrive_path):
         hdmap = HDMap.from_path(minimal_opendrive_path)
         ref = hdmap.reference_point

test/c20_planning/test_c24_global_hdmap_planners.py

@@ -0,0 +1,36 @@
+"""Tests for HDMap global-plan smoothing."""
+
+import pytest
+
+from avlite.c20_planning.c21_planning_model import GlobalPlan
+from avlite.c20_planning.c24_global_hdmap_planners import smoothen_path_savgol
+
+
+def _plan_with_n_points(n: int, spacing: float = 1.0) -> GlobalPlan:
+    plan = GlobalPlan()
+    plan.path = [(i * spacing, 0.0) for i in range(n)]
+    plan.velocity = [1.0] * n
+    plan.left_boundary_d = [1.0] * n
+    plan.right_boundary_d = [-1.0] * n
+    return plan
+
+
+@pytest.mark.parametrize("n", [2, 3, 4, 5, 6, 7, 8])
+def test_smoothen_path_savgol_handles_short_paths(n):
+    """Near-duplicate pruning can leave 3–6 points on a short same-lane HDMap route."""
+    out = smoothen_path_savgol(_plan_with_n_points(n))
+    assert len(out.path) == n
+    assert len(out.velocity) == n
+    assert len(out.left_boundary_d) == n
+    assert len(out.right_boundary_d) == n
+
+
+def test_smoothen_path_savgol_prunes_near_duplicates_then_smooths():
+    plan = _plan_with_n_points(4, spacing=0.1)
+    plan.path.append((3.0, 0.0))
+    plan.velocity.append(1.0)
+    plan.left_boundary_d.append(1.0)
+    plan.right_boundary_d.append(-1.0)
+    out = smoothen_path_savgol(plan, min_spacing=0.5)
+    assert len(out.path) >= 2
+    assert len(out.path) == len(out.velocity)

test/fixtures/opendrive_unresolved_lane_link.xodr

@@ -0,0 +1,54 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<OpenDRIVE>
+    <header revMajor="1" revMinor="4" name="unresolved-link" version="1" date="2026-08-17"
+            north="10" south="0" east="40" west="0" vendor="avlite-test">
+        <geoReference>+proj=tmerc +lat_0=45.0 +lon_0=55.0 +units=m +no_defs</geoReference>
+    </header>
+    <road name="pred_sidewalk" length="20.0" id="2" junction="-1">
+        <link>
+            <successor elementType="road" elementId="1"/>
+        </link>
+        <planView>
+            <geometry s="0" x="0" y="0" hdg="0" length="20.0">
+                <line/>
+            </geometry>
+        </planView>
+        <lanes>
+            <laneSection s="0">
+                <center>
+                    <lane id="0" type="none" level="false"/>
+                </center>
+                <right>
+                    <lane id="-1" type="sidewalk" level="false">
+                        <width sOffset="0" a="1.5" b="0" c="0" d="0"/>
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+    </road>
+    <road name="succ_driving" length="20.0" id="1" junction="-1">
+        <link>
+            <predecessor elementType="road" elementId="2"/>
+        </link>
+        <planView>
+            <geometry s="0" x="20" y="0" hdg="0" length="20.0">
+                <line/>
+            </geometry>
+        </planView>
+        <lanes>
+            <laneSection s="0">
+                <center>
+                    <lane id="0" type="none" level="false"/>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link>
+                            <predecessor id="-1"/>
+                        </link>
+                        <width sOffset="0" a="3.5" b="0" c="0" d="0"/>
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+    </road>
+</OpenDRIVE>