diff --git a/avlite/c10_perception/c11_perception_model.py b/avlite/c10_perception/c11_perception_model.py index ff934c4..515b78e 100644 --- a/avlite/c10_perception/c11_perception_model.py +++ b/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) diff --git a/avlite/c10_perception/c18_hdmap_parser.py b/avlite/c10_perception/c18_hdmap_parser.py index 3a7ac50..b7e8a98 100644 --- a/avlite/c10_perception/c18_hdmap_parser.py +++ b/avlite/c10_perception/c18_hdmap_parser.py @@ -358,6 +358,8 @@ 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) @@ -365,6 +367,8 @@ def _connect_lanes(hdmap: HDMap) -> None: 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) diff --git a/avlite/c20_planning/c24_global_hdmap_planners.py b/avlite/c20_planning/c24_global_hdmap_planners.py index 9533181..bcdf7f6 100644 --- a/avlite/c20_planning/c24_global_hdmap_planners.py +++ b/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 diff --git a/test/c10_perception/test_c11_agent_ids.py b/test/c10_perception/test_c11_agent_ids.py index b186ae4..279486e 100644 --- a/test/c10_perception/test_c11_agent_ids.py +++ b/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) diff --git a/test/c10_perception/test_c18_hdmap_parser.py b/test/c10_perception/test_c18_hdmap_parser.py index 26e485b..c0a945e 100644 --- a/test/c10_perception/test_c18_hdmap_parser.py +++ b/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 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..a46188f --- /dev/null +++ b/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) diff --git a/test/fixtures/opendrive_unresolved_lane_link.xodr b/test/fixtures/opendrive_unresolved_lane_link.xodr new file mode 100644 index 0000000..5fc323a --- /dev/null +++ b/test/fixtures/opendrive_unresolved_lane_link.xodr @@ -0,0 +1,54 @@ + + +
+ +proj=tmerc +lat_0=45.0 +lon_0=55.0 +units=m +no_defs +
+ + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + +