From 58fd4548483da641d4bb7f5f022e20779258fac0 Mon Sep 17 00:00:00 2001
From: Autrin <autrinhakimi@gmail.com>
Date: Sun, 1 Nov 2026 17:52:10 -0700
Subject: [PATCH] Fix Pure Pursuit target search at course endpoint

---
 PathTracking/pure_pursuit/pure_pursuit.py     |  9 +++--
 .../pure_pursuit_tracking_main.rst            |  3 ++
 tests/test_pure_pursuit.py                    | 40 ++++++++++++++++++-
 3 files changed, 47 insertions(+), 5 deletions(-)

diff --git a/PathTracking/pure_pursuit/pure_pursuit.py b/PathTracking/pure_pursuit/pure_pursuit.py
index 48453927ab..8bd7828095 100644
--- a/PathTracking/pure_pursuit/pure_pursuit.py
+++ b/PathTracking/pure_pursuit/pure_pursuit.py
@@ -93,6 +93,7 @@ def __init__(self, cx, cy):
         self.old_nearest_point_index = None
 
     def search_target_index(self, state):
+        """Return the target waypoint index and look-ahead distance in meters."""
 
         # To speed up nearest point search, doing it at only first time.
         if self.old_nearest_point_index is None:
@@ -106,12 +107,12 @@ def search_target_index(self, state):
             ind = self.old_nearest_point_index
             distance_this_index = state.calc_distance(self.cx[ind],
                                                       self.cy[ind])
-            while True:
+            while ind + 1 < len(self.cx):
                 distance_next_index = state.calc_distance(self.cx[ind + 1],
                                                           self.cy[ind + 1])
                 if distance_this_index < distance_next_index:
                     break
-                ind = ind + 1 if (ind + 1) < len(self.cx) else ind
+                ind += 1
                 distance_this_index = distance_next_index
             self.old_nearest_point_index = ind
 
@@ -231,13 +232,13 @@ def plot_wheel(x, y, yaw, steer=0.0, color=color):
     # Plot wheels (darker color for front wheels)
     front_color = 'darkblue'
     rear_color = color
-    
+
     # Plot four wheels
     # Front left
     plot_wheel(x + LENGTH/4 * c - WIDTH/2 * s,
               y + LENGTH/4 * s + WIDTH/2 * c,
               yaw, steer, front_color)
-    # Front right  
+    # Front right
     plot_wheel(x + LENGTH/4 * c + WIDTH/2 * s,
               y + LENGTH/4 * s - WIDTH/2 * c,
               yaw, steer, front_color)
diff --git a/docs/modules/6_path_tracking/pure_pursuit_tracking/pure_pursuit_tracking_main.rst b/docs/modules/6_path_tracking/pure_pursuit_tracking/pure_pursuit_tracking_main.rst
index ff6749bbbe..7ffd37e6e2 100644
--- a/docs/modules/6_path_tracking/pure_pursuit_tracking/pure_pursuit_tracking_main.rst
+++ b/docs/modules/6_path_tracking/pure_pursuit_tracking/pure_pursuit_tracking_main.rst
@@ -9,6 +9,9 @@ speed control.
 The red line is a target course, the green cross means the target point
 for pure pursuit control, the blue line is the tracking.
 
+If the look-ahead search reaches the end of the course,
+the final waypoint is used as the target.
+
 Code Link
 ~~~~~~~~~~~~~~~
 
diff --git a/tests/test_pure_pursuit.py b/tests/test_pure_pursuit.py
index 89c1829514..9d5cd75c55 100644
--- a/tests/test_pure_pursuit.py
+++ b/tests/test_pure_pursuit.py
@@ -1,6 +1,6 @@
 import conftest  # Add root path to sys.path
 from PathTracking.pure_pursuit import pure_pursuit as m
-
+import pytest
 
 def test1():
     m.show_animation = False
@@ -11,5 +11,43 @@ def test_backward():
     m.is_reverse_mode = True
     m.main()
 
+@pytest.mark.parametrize(
+    "course_x, rear_x_positions, expected_indices",
+    [
+        ([0.0], [0.0, 0.0], [0, 0]),
+        ([0.0, 10.0, 20.0], [20.0, 20.0], [2, 2]),
+        (
+            [0.0, 10.0, 20.0],
+            [0.0, 10.0, 20.0, 25.0],
+            [1, 2, 2, 2],
+        ),
+        ([0.0, 10.0, 20.0], [0.0, 0.0], [1, 1]),
+    ],
+    ids=[
+        "single-waypoint",
+        "repeated-search-at-final-waypoint",
+        "advance-to-and-beyond-final-waypoint",
+        "repeated-search-at-interior-waypoint",
+    ],
+)
+def test_search_target_index_handles_path_end(
+    course_x, rear_x_positions, expected_indices
+):
+    course = m.TargetCourse(course_x, [0.0] * len(course_x))
+
+    for rear_x, expected_index in zip(
+        rear_x_positions, expected_indices
+    ):
+        state = m.State(
+            x=rear_x + m.WB / 2,
+            y=0.0,
+            yaw=0.0,
+            v=0.0,
+        )
+
+        target_index, _ = course.search_target_index(state)
+
+        assert target_index == expected_index
+
 if __name__ == '__main__':
     conftest.run_this_test(__file__)