From 1785e936ef99da315312985e405a887f3f62515e Mon Sep 17 00:00:00 2001 From: Cursor Agent Date: Tue, 4 Aug 2026 11:18:28 +0000 Subject: [PATCH] Fix corridor-only collision index falling back to path end. _find_collision_index now uses centerline distance within ego half-width plus safety margin, matching the buffered corridor check, so side obstacles trigger braking at the mid-path hit instead of n-1. Co-authored-by: Majid Khonji --- CHANGELOG.md | 1 + avlite/c50_common/c55_collision_checking.py | 28 +++++++++++++----- .../c50_common/test_c55_collision_checking.py | 29 +++++++++++++++++++ 3 files changed, 50 insertions(+), 8 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 025328f..8be5d3a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -25,6 +25,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 - Docs: plugin registry field tables list every field with a required column (README, Overview, Plugin Development) ### Fixed +- Common: corridor-only side collisions now return the mid-path index — `_find_collision_index` uses centerline distance ≤ ego half-width + margin (same criterion as the buffered corridor) instead of a bare `LineString.intersects` that fell back to `n-1` and delayed braking until path end - Common: `TrajectoryTracker` initializes `path_s` from cumulative arc-length instead of re-projecting the reference through KD-tree Frenet conversion — closed tracks with `first==last` (e.g. bundled Yas Marina race line) no longer get non-monotonic `path_s` with `path_s[-1] == 0` - Common: Frenet XY→SD picks the better adjacent segment around the nearest waypoint (and SD→XY brackets by arc-length) — on-path points after corners no longer pick up a huge false CTE from the previous segment - Common / Planning: lattice sampling, replan end-of-track gates, and race lap detection use `TrajectoryTracker.track_end_s` (`path_s[-1]`) instead of the stale `path_s[-2]` workaround — avoids `IndexError` on 1-point paths and restores the final closed-track segment after the cumulative `path_s` fix diff --git a/avlite/c50_common/c55_collision_checking.py b/avlite/c50_common/c55_collision_checking.py index a09136d..1503ff3 100644 --- a/avlite/c50_common/c55_collision_checking.py +++ b/avlite/c50_common/c55_collision_checking.py @@ -174,7 +174,7 @@ def check_collision( min_clearance = _LARGE_CLEARANCE for obstacle, agent_velocity in obstacles: if trajectory_corridor.intersects(obstacle): - idx = _find_collision_index(trajectory_line, obstacle, path_x, path_y) + idx = _find_collision_index(trajectory_line, obstacle, path_x, path_y, radius) if best_idx is None or idx < best_idx: best_idx, best_vel = idx, agent_velocity min_clearance = 0.0 @@ -190,28 +190,40 @@ def check_collision( return False, -1, -1, min_clearance -def _find_collision_index(trajectory_line: LineString, obstacle_polygon: Polygon, - path_x: np.ndarray, path_y: np.ndarray) -> int: +def _find_collision_index( + trajectory_line: LineString, + obstacle_polygon: Polygon, + path_x: np.ndarray, + path_y: np.ndarray, + radius: float = 0.0, +) -> int: """ - Find the approximate trajectory index where collision with obstacle occurs. - Uses binary search for efficiency. + Find the approximate trajectory index where the ego corridor first hits the obstacle. + + Detection uses a buffered centerline (``radius`` = ego half-width + safety margin). + Index search must use the same criterion: a centerline-only intersect check misses + buffer-only side hits and silently falls back to ``n-1``, which delays braking until + the path end. """ n = len(path_x) if n < 2: return 0 - # Binary search to find first collision point + # Binary search to find first corridor collision point left, right = 1, n - 1 # Start from 1 to ensure at least 2 points collision_idx = n - 1 # default to end if can't find + # Match corridor.intersects: distance to the obstacle within ego half-width + margin. + # Tiny epsilon absorbs shapely flat-cap / float noise at the touch boundary. + touch_dist = float(radius) + 1e-9 while left <= right: mid = (left + right) // 2 - # Check if segment from start to mid intersects obstacle + # Check if corridor from start to mid touches obstacle # Ensure we have at least 2 points for a valid LineString end_idx = max(2, mid + 1) partial_line = LineString(list(zip(path_x[:end_idx], path_y[:end_idx]))) - if partial_line.intersects(obstacle_polygon): + if partial_line.distance(obstacle_polygon) <= touch_dist: collision_idx = mid right = mid - 1 # Search earlier else: diff --git a/test/c50_common/test_c55_collision_checking.py b/test/c50_common/test_c55_collision_checking.py index 4c39337..a2af583 100644 --- a/test/c50_common/test_c55_collision_checking.py +++ b/test/c50_common/test_c55_collision_checking.py @@ -129,3 +129,32 @@ def test_front_corner_side_overlap_detected(self): collision_safety_margin=margin, ) assert hit is True + + +class TestBufferOnlyCollisionIndex: + """Corridor-only side hits must report the mid-path index, not path end.""" + + def test_side_obstacle_index_is_near_agent_not_path_end(self): + # Centerline clears the agent polygon, but ego half-width + margin corridor hits. + # Old binary search used bare LineString.intersects and fell back to n-1. + ego_w = 2.0 + margin = 0.3 + n = 31 + agent_x = 30.0 + # Agent half-width 1.0 → body gap to y=0 centerline is 0.5 m (< radius 1.3). + agent_y = 1.5 + trajectory = _straight_trajectory(0.0, 100.0, n=n) + pm = PerceptionModel( + ego_vehicle=EgoState(x=0.0, y=0.0, theta=0.0, velocity=5.0, width=ego_w), + agent_vehicles=[ + AgentState(x=agent_x, y=agent_y, theta=0.0, velocity=0.0, agent_id=1, width=2.0), + ], + ) + hit, idx, _, clearance = check_collision( + pm, trajectory, collision_safety_margin=margin, + ) + assert hit is True + assert clearance == 0.0 + expected = int(round(agent_x / 100.0 * (n - 1))) + assert abs(idx - expected) <= 2, f"collision idx {idx} far from agent wp {expected} (n-1={n - 1})" + assert idx < n - 1