Fix closed-track false goal stop, planner wp clamp, and short velocity crash

CHANGELOG.md

@@ -36,6 +36,9 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 - Visualizer: Control **Align** teleports plant ego and syncs stack PM (stack-only writes were undone by GT localization after the world/stack ego split)
 - Common: `TrajectoryTracker.update_waypoint_by_wp` / `update_to_next_waypoint` clamp `next_wp` at the path end — `%` precedence previously left `next_wp == len(path)` and crashed plot/step at the final waypoint
 - Common: `create_quintic_trajectory_sd` b-vector matches the constraint matrix (end 1st / start 2nd derivatives were swapped)
+- Execution: `GoalArrivalMonitor` seeds its rising-edge detector on the first tick — starting already inside the arrival radius (closed race lines where `goal_point == start_point`, e.g. default Yas Marina) no longer fires `GOAL_ARRIVED` / `StopExecAtGoalTask` immediately
+- Planning: `LocalPlanningStrategy.reset(wp)` clamps waypoint index — Plan UI step-back from wp 0 no longer jumps to track end via numpy negative indexing, and oversized wp no longer `IndexError`
+- Control: Stanley / PID / Pure Pursuit clamp velocity waypoint index (and zero accel on empty profiles) — short `ReferenceSpeed` vs path length no longer crashes mid-drive
 
 ## [0.5.3] - 2026-07-24
 

avlite/c20_planning/c23_local_planning_strategy.py

@@ -86,6 +86,16 @@ def set_global_plan(self, global_plan: GlobalPlan, ego_xy: Optional[tuple[float,
         log.info(f"Global plan set: ego Frenet s={s0:.2f} d={d0:.2f}. Ego xy={ref_xy}. Global plan start={global_plan.start_point}")
 
     def reset(self, wp: int = 0):
+        n = len(self.global_trajectory.path_x)
+        if n == 0:
+            self.traversed_x, self.traversed_y = [0.0], [0.0]
+            self.traversed_s, self.traversed_d = [0.0], [0.0]
+            self.location_xy = (0.0, 0.0)
+            self.location_sd = (0.0, 0.0)
+            return
+        # Clamp: negative indices would silently select the track end via numpy
+        # wrapping (Plan UI "step back" from wp 0), and oversized wp IndexError.
+        wp = max(0, min(int(wp), n - 1))
         self.traversed_x, self.traversed_y = [self.global_trajectory.path_x[wp]], [self.global_trajectory.path_y[wp]]
         self.traversed_s, self.traversed_d = [self.global_trajectory.path_s[wp]], [self.global_trajectory.path_d[wp]]
         self.location_xy = (self.traversed_x[0], self.traversed_y[0])

avlite/c30_control/c33_pid.py

@@ -86,7 +86,12 @@ def control(
         ##################################
         # Compute the velocity control PID
         ##################################
-        idx = self.tj.current_wp
+        if not self.tj.velocity:
+            log.warning("Trajectory has no velocity profile. Acceleration set to zero.")
+            cmd = ControlCommand(steer=steer, acceleration=0)
+            self.cmd = cmd
+            return cmd
+        idx = min(max(self.tj.current_wp, 0), len(self.tj.velocity) - 1)
         target_velocity = self.tj.velocity[idx]
 
         prev_cte_v = self.cte_velocity

avlite/c30_control/c34_stanley.py

@@ -86,7 +86,12 @@ def control(
         ##################################
         # Compute the velocity control PID
         ##################################
-        idx = self.tj.current_wp
+        if not self.tj.velocity:
+            log.warning("Trajectory has no velocity profile. Acceleration set to zero.")
+            cmd = ControlCommand(steer=steer, acceleration=0)
+            self.cmd = cmd
+            return cmd
+        idx = min(max(self.tj.current_wp, 0), len(self.tj.velocity) - 1)
         target_velocity = self.tj.velocity[idx]
 
         prev_cte_v = self.cte_velocity

avlite/c30_control/c35_pure_pursuit.py

@@ -180,7 +180,12 @@ def control(
         ##################################
         # Velocity PID from trajectory
         ##################################
-        idx = self.tj.current_wp
+        if not self.tj.velocity:
+            log.warning("Trajectory has no velocity profile. Acceleration set to zero.")
+            cmd = ControlCommand(steer=steer, acceleration=0)
+            self.cmd = cmd
+            return cmd
+        idx = min(max(self.tj.current_wp, 0), len(self.tj.velocity) - 1)
         target_velocity = self.tj.velocity[idx]
         acc = self.velocity_pid(ego, target_velocity)
 
@@ -256,7 +261,8 @@ def control(
         ##################################
         if self.tj is not None and self.tj.is_initialized and len(self.tj.velocity) > 0:
             self.tj.update_waypoint_by_xy(ego.x, ego.y)
-            target_velocity = self.tj.velocity[self.tj.current_wp]
+            idx = min(max(self.tj.current_wp, 0), len(self.tj.velocity) - 1)
+            target_velocity = self.tj.velocity[idx]
         else:
             target_velocity = self.cruise_velocity
 

avlite/c40_execution/c47_execution_tasks.py

@@ -12,19 +12,31 @@
 
 
 class GoalArrivalMonitor(TaskStrategy):
-    """Detect rising-edge arrival at the global goal and notify listeners."""
+    """Detect rising-edge arrival at the global goal and notify listeners.
+
+    The edge detector is seeded on the first tick (and after :meth:`reset`) so
+    starting already inside the arrival radius — normal on closed race lines
+    where ``goal_point == start_point`` — does not fire ``GOAL_ARRIVED``.
+    """
 
     schedule = TaskSchedule.EVERY_CYCLE
     arrive_radius_m: ClassVar[float] = 3.0
 
     def __init__(self) -> None:
         self._was_arrived = False
+        self._seeded = False
 
     def reset(self) -> None:
         self._was_arrived = False
+        self._seeded = False
 
     def execute(self, executer, event=None) -> None:
         arrived = self._ego_near_goal(executer, self.arrive_radius_m)
+        if not self._seeded:
+            # Seed without notifying: ego often starts at/near the goal on closed tracks.
+            self._was_arrived = arrived
+            self._seeded = True
+            return
         if arrived and not self._was_arrived:
             executer.task_runner.notify(StackEvent.GOAL_ARRIVED)
         self._was_arrived = arrived

avlite/plugins/p60_visualizer_tk/p67_stack_views.py

@@ -447,8 +447,9 @@ def step_waypoint_back(self):
         """ Step back to the previous waypoint in the local planner."""
         if not self.root.exec or not self.root.exec.local_planner:
             return
-        self.root.setting.current_wp.set(str(int(self.root.setting.current_wp.get()) - 1))
-        self.root.exec.local_planner.reset(wp=int(self.root.setting.current_wp.get()))
+        wp = max(0, int(self.root.setting.current_wp.get()) - 1)
+        self.root.setting.current_wp.set(str(wp))
+        self.root.exec.local_planner.reset(wp=wp)
         self.root.update_ui()
 
     def text_on_enter(self, event):

test/c20_planning/test_c23_local_planning_pipeline.py

@@ -1,6 +1,7 @@
 """Unit tests for the local planning pipeline and dual-role planners (c23)."""
 
 import numpy as np
+import pytest
 
 from avlite.c10_perception.c11_perception_model import EgoState, PerceptionModel
 from avlite.c20_planning.c21_planning_model import GlobalPlan, LocalBehavior, LocalPlan
@@ -135,3 +136,29 @@ def test_pipeline_step_advances_child(self):
         state = EgoState(x=5.0, y=0.0, theta=0.0, velocity=5.0)
         pipeline.step(state)
         assert pipeline.location_xy == (5.0, 0.0)
+
+
+def test_local_planner_reset_clamps_negative_and_oob_wp():
+    """Plan UI step-back from wp 0 used to pass wp=-1 and jump to track end."""
+    gp = _straight_global_plan(n=10)
+    pm = PerceptionModel()
+
+    class _P(LocalPlanningStrategy):
+        def replan(self, perception_model=None, sensors=None):
+            pass
+
+        def __init_subclass__(cls, **kwargs):
+            pass
+
+    pl = _P(gp, pm)
+    pl.reset(wp=0)
+    assert pl.global_trajectory.current_wp == 0
+    assert pl.location_sd[0] == pytest.approx(0.0)
+
+    pl.reset(wp=-1)
+    assert pl.global_trajectory.current_wp == 0
+    assert pl.location_sd[0] == pytest.approx(0.0)
+
+    pl.reset(wp=9999)
+    assert pl.global_trajectory.current_wp == len(gp.path) - 1
+    assert pl.location_xy == (gp.path[-1][0], gp.path[-1][1])

test/c30_control/test_c33_pid.py

@@ -78,3 +78,22 @@ def test_on_path_steer_is_mostly_heading_correction(self):
         ego = EgoState(x=50.0, y=0.0, theta=0.0, velocity=5.0)
         cmd = controller.control(ego)
         assert abs(cmd.steer) < 0.2
+
+    def test_short_velocity_profile_does_not_index_error(self):
+        """ReferenceSpeed shorter than ReferenceLine used to crash at high wp."""
+        path = [(float(i) * 10.0, 0.0) for i in range(20)]
+        trajectory = TrajectoryTracker(path=path, velocity=[5.0, 5.0, 5.0])
+        trajectory.update_waypoint_by_wp(15)
+        controller = StanleyController(tj=trajectory, setting=_stanley_settings())
+        ego = EgoState(x=150.0, y=0.0, theta=0.0, velocity=5.0)
+        cmd = controller.control(ego)
+        assert isinstance(cmd.steer, float)
+        assert isinstance(cmd.acceleration, float)
+
+    def test_empty_velocity_profile_commands_zero_accel(self):
+        path = [(float(i) * 10.0, 0.0) for i in range(5)]
+        trajectory = TrajectoryTracker(path=path, velocity=[])
+        controller = StanleyController(tj=trajectory, setting=_stanley_settings())
+        ego = EgoState(x=0.0, y=0.0, theta=0.0, velocity=5.0)
+        cmd = controller.control(ego)
+        assert cmd.acceleration == 0.0

test/c40_execution/test_c43_task_strategy.py

@@ -190,6 +190,29 @@ def test_goal_monitor_notifies_stop_at_goal_once():
     assert executer.stopped is False  # edge already consumed
 
 
+def test_goal_monitor_does_not_fire_when_starting_at_closed_loop_goal():
+    """Closed race lines set goal_point == start_point; default Yas start is within 3 m."""
+    monitor = GoalArrivalMonitor()
+    # Ego starts on the goal (first==last closed track).
+    executer = _FakeExecuter(x=0.0, y=0.0, goal=(0.0, 0.0))
+    notified: list = []
+    executer.task_runner = SimpleNamespace(notify=lambda e: notified.append(e))
+
+    monitor.execute(executer)
+    assert notified == []
+    assert monitor._was_arrived is True
+
+    # Leave the radius, then re-enter — that is a real arrival.
+    executer.ego_state.x = 20.0
+    monitor.execute(executer)
+    assert notified == []
+    assert monitor._was_arrived is False
+
+    executer.ego_state.x = 1.0
+    monitor.execute(executer)
+    assert notified == [StackEvent.GOAL_ARRIVED]
+
+
 def test_notify_during_step_flushes_to_on_event():
     executer = _FakeExecuter()
     runner = TaskRunner([NotifyDuringCycleTask(), StopExecAtGoalTask()], executer=executer)