Fix PRM collision checks to account for the full edge

PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py

@@ -69,32 +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):

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,44 @@ 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__)