Test: Add unit test to verify smoothed path respects robot_radius

Author: ShiboWork

tests/test_rrt_with_pathsmoothing_radius.py

@@ -0,0 +1,48 @@
+import conftest
+import math
+
+from PathPlanning.RRT import rrt_with_pathsmoothing as rrt_module
+
+def test_smoothed_path_safety():
+    # Define test environment
+    obstacle_list = [
+        (5, 5, 1.0),
+        (3, 6, 2.0),
+        (3, 8, 2.0),
+        (3, 10, 2.0),
+        (7, 5, 2.0),
+        (9, 5, 2.0)
+    ]
+    robot_radius = 0.5
+
+    # Disable animation for testing
+    rrt_module.show_animation = False
+
+    # Create RRT planner
+    rrt = rrt_module.RRT(
+        start=[0, 0],
+        goal=[6, 10],
+        rand_area=[-2, 15],
+        obstacle_list=obstacle_list,
+        robot_radius=robot_radius
+    )
+
+    # Run RRT
+    path = rrt.planning(animation=False)
+
+    # Smooth the path
+    smoothed = rrt_module.path_smoothing(path, max_iter=1000,
+                                         obstacle_list=obstacle_list,
+                                         robot_radius=robot_radius)
+
+    # Check if all points on the smoothed path are safely distant from obstacles
+    for x, y in smoothed:
+        for ox, oy, obs_radius in obstacle_list:
+            d = math.hypot(x - ox, y - oy)
+            min_safe_dist = obs_radius + robot_radius
+            assert d > min_safe_dist, \
+                f"Point ({x:.2f}, {y:.2f}) too close to obstacle at ({ox}, {oy})"
+
+
+if __name__ == '__main__':
+    conftest.run_this_test(__file__)