Fix target-point arclength interpolation in RRT path smoothing

PathPlanning/RRT/rrt_with_pathsmoothing.py

@@ -30,25 +30,19 @@ def get_path_length(path):
 
 
 def get_target_point(path, targetL):
-    le = 0
-    ti = 0
-    lastPairLen = 0
+    """Return the point and containing segment at distance targetL along path."""
+    remaining_length = targetL
     for i in range(len(path) - 1):
         dx = path[i + 1][0] - path[i][0]
         dy = path[i + 1][1] - path[i][1]
-        d = math.hypot(dx, dy)
-        le += d
-        if le >= targetL:
-            ti = i - 1
-            lastPairLen = d
-            break
-
-    partRatio = (le - targetL) / lastPairLen
-
-    x = path[ti][0] + (path[ti + 1][0] - path[ti][0]) * partRatio
-    y = path[ti][1] + (path[ti + 1][1] - path[ti][1]) * partRatio
-
-    return [x, y, ti]
+        segment_length = math.hypot(dx, dy)
+        if segment_length == 0:
+            continue
+        if remaining_length <= segment_length:
+            ratio = remaining_length / segment_length
+            return [path[i][0] + ratio * dx, path[i][1] + ratio * dy, i]
+        remaining_length -= segment_length
+    return [path[-1][0], path[-1][1], len(path) - 2]
 
 
 def is_point_collision(x, y, obstacle_list, robot_radius):

tests/test_rrt_with_pathsmoothing_radius.py

@@ -1,8 +1,26 @@
 import conftest
 import math
+import pytest
 
 from PathPlanning.RRT import rrt_with_pathsmoothing as rrt_module
 
+
+@pytest.mark.parametrize("distance, expected", [
+    (0.0, [0.0, 0.0, 0]),
+    (5.0, [5.0, 0.0, 0]),
+    (10.0, [10.0, 0.0, 0]),
+    (15.0, [10.0, 5.0, 1]),
+    (20.0, [10.0, 10.0, 1]),
+])
+def test_target_point_matches_path_arclength(distance, expected):
+    path = [[0.0, 0.0], [10.0, 0.0], [10.0, 10.0]]
+    assert rrt_module.get_target_point(path, distance) == expected
+
+
+def test_target_point_skips_zero_length_segments():
+    path = [[0.0, 0.0], [0.0, 0.0], [10.0, 0.0], [10.0, 10.0]]
+    assert rrt_module.get_target_point(path, 5.0) == [5.0, 0.0, 1]
+
 def test_smoothed_path_safety():
     # Define test environment
     obstacle_list = [