Fix Voronoi roadmap collision checks across the full edge

PathPlanning/VoronoiRoadMap/voronoi_road_map.py

@@ -42,32 +42,33 @@ def planning(self, sx, sy, gx, gy, ox, oy, robot_radius):
         return rx, ry
 
     def is_collision(self, 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 >= self.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 swept edge lies within this ball around its midpoint.
+        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_info(self, node_x, node_y, rr, obstacle_tree):
         """

tests/test_voronoi_road_map_collision.py

@@ -0,0 +1,46 @@
+import conftest  # Add root path to sys.path
+from scipy.spatial import cKDTree
+
+from PathPlanning.VoronoiRoadMap import voronoi_road_map as m
+
+
+def test_edge_collision_detects_obstacle_between_samples():
+    planner = m.VoronoiRoadMapPlanner()
+    # The endpoints are each more than 1 m from the obstacle, but
+    # the segment passes within 0.95 m of it.
+    obstacle_tree = cKDTree([[0.5, 0.95]])
+
+    assert planner.is_collision(0.0, 0.0, 1.0, 0.0, 1.0, obstacle_tree)
+    assert not planner.is_collision(0.0, 0.0, 1.0, 0.0, 0.9, obstacle_tree)
+
+
+def test_point_robot_detects_obstacles_on_the_segment():
+    planner = m.VoronoiRoadMapPlanner()
+    on_edge = cKDTree([[0.5, 0.0]])
+    off_edge = cKDTree([[0.5, 0.01]])
+
+    assert planner.is_collision(0.0, 0.0, 1.0, 0.0, 0.0, on_edge)
+    assert not planner.is_collision(0.0, 0.0, 1.0, 0.0, 0.0, off_edge)
+
+
+def test_zero_length_edge_checks_its_single_position():
+    planner = m.VoronoiRoadMapPlanner()
+    at_start = cKDTree([[0.0, 0.0]])
+    far_away = cKDTree([[2.0, 0.0]])
+
+    assert planner.is_collision(0.0, 0.0, 0.0, 0.0, 0.0, at_start)
+    assert not planner.is_collision(0.0, 0.0, 0.0, 0.0, 0.0, far_away)
+
+
+def test_edge_limit_and_obstacle_past_endpoint():
+    planner = m.VoronoiRoadMapPlanner()
+    far_away = cKDTree([[2.0, 0.0]])
+
+    assert not planner.is_collision(0.0, 0.0, 1.0, 0.0, 0.6, far_away)
+    assert planner.is_collision(
+        0.0, 0.0, planner.MAX_EDGE_LEN, 0.0, 0.0, far_away
+    )
+
+
+if __name__ == '__main__':
+    conftest.run_this_test(__file__)