From 81828c847c95690a32f82965cf21626ae1bb4045 Mon Sep 17 00:00:00 2001 From: huxiaoyi-ovo Date: Thu, 29 Oct 2026 02:18:45 -0400 Subject: [PATCH 1/4] fix(prm): check continuous edge clearance against obstacles --- .../probabilistic_road_map.py | 33 ++++++++++--------- 1 file changed, 17 insertions(+), 16 deletions(-) diff --git a/PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py b/PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py index 8bacfd5d19..f4143ad5d9 100644 --- a/PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py +++ b/PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py @@ -69,33 +69,34 @@ def prm_planning(start_x, start_y, goal_x, goal_y, def is_collision(sx, sy, gx, gy, rr, obstacle_kd_tree): - x = sx - y = sy dx = gx - sx dy = gy - sy - yaw = math.atan2(gy - sy, gx - sx) d = math.hypot(dx, dy) if d >= MAX_EDGE_LEN: return True - D = rr - n_step = round(d / D) + if d == 0.0: + dist, _ = obstacle_kd_tree.query([sx, sy]) + return dist <= rr - for i in range(n_step): - dist, _ = obstacle_kd_tree.query([x, y]) - if dist <= rr: - return True # collision - x += D * math.cos(yaw) - y += D * math.sin(yaw) + start = np.array([sx, sy]) + direction = np.array([dx, dy]) - # goal point check - dist, _ = obstacle_kd_tree.query([gx, gy]) - if dist <= rr: - return True # collision + # The edge's swept disk lies within this ball around its midpoint. + # Query only nearby obstacles, then check exact segment clearance. + midpoint = start + direction / 2.0 + nearby = obstacle_kd_tree.query_ball_point(midpoint, d / 2.0 + rr) + if not nearby: + return False - return False # OK + obstacles = obstacle_kd_tree.data[nearby] + fraction = np.clip((obstacles - start) @ direction / (d * d), + 0.0, 1.0) + closest = start + fraction[:, None] * direction + clearance_sq = np.sum((obstacles - closest) ** 2, axis=1) + return bool(np.any(clearance_sq <= rr ** 2)) def generate_road_map(sample_x, sample_y, rr, obstacle_kd_tree): """ From 24c7bd66426d85e7a319b42e23b7b5eeb80f5523 Mon Sep 17 00:00:00 2001 From: huxiaoyi-ovo Date: Thu, 29 Oct 2026 02:19:12 -0400 Subject: [PATCH 2/4] test(prm): cover continuous collision and zero-radius edges --- tests/test_probabilistic_road_map.py | 41 ++++++++++++++++++++++++++++ 1 file changed, 41 insertions(+) diff --git a/tests/test_probabilistic_road_map.py b/tests/test_probabilistic_road_map.py index 6c5eb540b1..307174d211 100644 --- a/tests/test_probabilistic_road_map.py +++ b/tests/test_probabilistic_road_map.py @@ -1,5 +1,6 @@ import conftest # Add root path to sys.path import numpy as np +from scipy.spatial import KDTree from PathPlanning.ProbabilisticRoadMap import probabilistic_road_map @@ -8,5 +9,45 @@ def test1(): probabilistic_road_map.main(rng=np.random.default_rng(1233)) + +def test_collision_detects_obstacle_between_edge_samples(): + # The obstacle is less than 1 m from the segment, but more than + # 1 m from both endpoints (the old discrete samples). + obstacle_tree = KDTree([[0.5, 0.95]]) + assert probabilistic_road_map.is_collision( + 0.0, 0.0, 1.0, 0.0, 1.0, obstacle_tree + ) + assert not probabilistic_road_map.is_collision( + 0.0, 0.0, 1.0, 0.0, 0.9, obstacle_tree + ) + + +def test_collision_with_point_robot(): + on_edge = KDTree([[0.5, 0.0]]) + off_edge = KDTree([[0.5, 0.01]]) + + assert probabilistic_road_map.is_collision( + 0.0, 0.0, 1.0, 0.0, 0.0, on_edge + ) + assert not probabilistic_road_map.is_collision( + 0.0, 0.0, 1.0, 0.0, 0.0, off_edge + ) + + +def test_collision_handles_zero_length_and_endpoint_clearance(): + start_obstacle = KDTree([[0.0, 0.0]]) + outside_segment = KDTree([[2.0, 0.0]]) + + assert probabilistic_road_map.is_collision( + 0.0, 0.0, 0.0, 0.0, 0.0, start_obstacle + ) + assert not probabilistic_road_map.is_collision( + 0.0, 0.0, 0.0, 0.0, 0.0, outside_segment + ) + assert not probabilistic_road_map.is_collision( + 0.0, 0.0, 1.0, 0.0, 0.6, outside_segment + ) + + if __name__ == '__main__': conftest.run_this_test(__file__) From 0cb13d81ec83404ed5ebf74640dd595921bd9dad Mon Sep 17 00:00:00 2001 From: huxiaoyi-ovo Date: Thu, 29 Oct 2026 02:19:48 -0400 Subject: [PATCH 3/4] style(tests): normalize spacing between PRM test cases --- tests/test_probabilistic_road_map.py | 1 - 1 file changed, 1 deletion(-) diff --git a/tests/test_probabilistic_road_map.py b/tests/test_probabilistic_road_map.py index 307174d211..3005cbdfd5 100644 --- a/tests/test_probabilistic_road_map.py +++ b/tests/test_probabilistic_road_map.py @@ -9,7 +9,6 @@ def test1(): probabilistic_road_map.main(rng=np.random.default_rng(1233)) - def test_collision_detects_obstacle_between_edge_samples(): # The obstacle is less than 1 m from the segment, but more than # 1 m from both endpoints (the old discrete samples). From ebcb2dbcef9e5787e9a44890d3f60b9434680199 Mon Sep 17 00:00:00 2001 From: huxiaoyi-ovo Date: Thu, 29 Oct 2026 02:21:04 -0400 Subject: [PATCH 4/4] style(prm): separate top-level functions --- PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py | 1 + 1 file changed, 1 insertion(+) diff --git a/PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py b/PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py index f4143ad5d9..6423f57af8 100644 --- a/PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py +++ b/PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py @@ -98,6 +98,7 @@ def is_collision(sx, sy, gx, gy, rr, obstacle_kd_tree): return bool(np.any(clearance_sq <= rr ** 2)) + def generate_road_map(sample_x, sample_y, rr, obstacle_kd_tree): """ Road map generation