Fix pipeline velocity corrupting global speeds and mid-track path origin

CHANGELOG.md

@@ -25,6 +25,8 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 - Docs: plugin registry field tables list every field with a required column (README, Overview, Plugin Development)
 
 ### Fixed
+- Planning: `VelocityLocalPlanner.plan_velocity` profiles a copy of the incoming trajectory — `LocalPlanningPipeline` no longer permanently zeros the shared global reference speeds when the velocity stage brakes for an obstacle
+- Planning: `ReferencePathPlanner.plan_path` emits the forward horizon from `current_wp` instead of the full route from s=0 — mid-track pipeline replans no longer hand control a path that starts at the origin
 - Common: `TrajectoryTracker` initializes `path_s` from cumulative arc-length instead of re-projecting the reference through KD-tree Frenet conversion — closed tracks with `first==last` (e.g. bundled Yas Marina race line) no longer get non-monotonic `path_s` with `path_s[-1] == 0`
 - Common: Frenet XY→SD picks the better adjacent segment around the nearest waypoint (and SD→XY brackets by arc-length) — on-path points after corners no longer pick up a huge false CTE from the previous segment
 - Common / Planning: lattice sampling, replan end-of-track gates, and race lap detection use `TrajectoryTracker.track_end_s` (`path_s[-1]`) instead of the stale `path_s[-2]` workaround — avoids `IndexError` on 1-point paths and restores the final closed-track segment after the cumulative `path_s` fix

avlite/c20_planning/c26_local_path_planners.py

@@ -17,6 +17,7 @@
 )
 from avlite.c20_planning.c29_settings import PlanningSettings, PlanningSettingsSchema
 from avlite.c50_common.c51_capabilities import StackCapability
+from avlite.c50_common.c54_trajectory_tracker import slice_trajectory_horizon
 
 log = logging.getLogger(__name__)
 
@@ -52,8 +53,13 @@ def replan(
         pass
 
     def plan_path(self, plan: LocalPlan) -> LocalPlan:
-        plan.path = list(self.global_trajectory.path)
-        plan.velocity = list(self.global_trajectory.velocity)
+        # Forward horizon from the tracked waypoint — copying the full route from
+        # s=0 left mid-track ego with a local plan that starts at the origin
+        # (current_wp=0), so velocity profiling and Stanley/PID aimed at the
+        # wrong segment.
+        sliced = slice_trajectory_horizon(self.global_trajectory, max_points=0)
+        plan.path = list(sliced.path)
+        plan.velocity = list(sliced.velocity)
         # Leave trajectory unset so a downstream velocity stage builds (and may
         # mutate) a fresh tracker rather than the shared global one.
         plan.trajectory = None

avlite/c20_planning/c27_local_behavioral_and_velocity_planners.py

@@ -103,15 +103,38 @@ def replan(
         self._local_trajectory = local_tj
 
     def plan_velocity(self, plan: LocalPlan) -> LocalPlan:
-        """Velocity stage: profile the incoming plan's trajectory in place."""
+        """Velocity stage: profile a copy of the incoming plan's trajectory.
+
+        ``LocalPlanningPipeline`` may wrap the live global tracker via
+        ``LocalPlan.from_trajectory``. Profiling must not mutate that shared
+        object — otherwise a braking pass permanently zeros the global
+        reference speeds and the ego never resumes cruise after the obstacle
+        clears.
+        """
         tj = plan.as_trajectory()
         if tj is not None:
-            ref_velocity = np.asarray(tj.velocity, dtype=float)
-            self.profile_trajectory(tj, ref_velocity=ref_velocity)
-            plan.velocity = list(tj.velocity)
-            plan.trajectory = tj
+            local = self._copy_trajectory_for_profiling(tj)
+            ref_velocity = np.asarray(local.velocity, dtype=float)
+            self.profile_trajectory(local, ref_velocity=ref_velocity)
+            plan.path = list(local.path)
+            plan.velocity = list(local.velocity)
+            plan.trajectory = local
         return plan
 
+    @staticmethod
+    def _copy_trajectory_for_profiling(tj: TrajectoryTracker) -> TrajectoryTracker:
+        """Deep-copy path/velocity/waypoints so speed-match cannot alias ``tj``."""
+        velocity = list(tj.velocity) if tj.velocity is not None else None
+        local = TrajectoryTracker(path=list(tj.path), velocity=velocity)
+        local.current_wp = tj.current_wp
+        local.next_wp = tj.next_wp
+        local.name = getattr(tj, "name", local.name) or "Local Trajectory"
+        if getattr(tj, "ref_left_boundary_d", None) is not None:
+            local.ref_left_boundary_d = list(tj.ref_left_boundary_d)
+        if getattr(tj, "ref_right_boundary_d", None) is not None:
+            local.ref_right_boundary_d = list(tj.ref_right_boundary_d)
+        return local
+
     def apply_speed_match(
         self,
         trajectory: TrajectoryTracker,

test/c20_planning/test_c23_local_planning_pipeline.py

@@ -1,8 +1,10 @@
 """Unit tests for the local planning pipeline and dual-role planners (c23)."""
 
+import math
+
 import numpy as np
 
-from avlite.c10_perception.c11_perception_model import EgoState, PerceptionModel
+from avlite.c10_perception.c11_perception_model import AgentState, EgoState, PerceptionModel
 from avlite.c20_planning.c21_planning_model import GlobalPlan, LocalBehavior, LocalPlan
 from avlite.c20_planning.c23_local_planning_strategy import (
     LocalBehavioralPlanningStrategy,
@@ -11,6 +13,7 @@
     LocalPlanningStrategy,
     LocalVelocityPlanningStrategy,
 )
+from avlite.c20_planning.c26_local_path_planners import ReferencePathPlanner
 from avlite.c20_planning.c27_local_behavioral_and_velocity_planners import (
     CruiseBehavioralPlanner,
     VelocityLocalPlanner,
@@ -78,6 +81,8 @@ def test_velocity_stage_profiles_in_place(self):
         out = planner.plan_velocity(plan)
         assert out.trajectory is not None
         assert len(out.velocity) == len(out.trajectory.velocity)
+        # Must not alias the shared global tracker (pipeline hands that in).
+        assert out.trajectory is not global_plan.trajectory
 
     def test_path_stage_fills_geometry(self):
         global_plan = _straight_global_plan()
@@ -88,6 +93,15 @@ def test_path_stage_fills_geometry(self):
         assert out.trajectory is not None
         assert len(out.path) > 0
 
+    def test_reference_path_stage_starts_at_current_wp(self):
+        global_plan = _straight_global_plan(x_end=100.0, n=21)
+        global_plan.trajectory.update_waypoint_by_xy(50.0, 0.0)
+        pm = PerceptionModel(ego_vehicle=EgoState(x=50.0, y=0.0, theta=0.0, velocity=5.0))
+        planner = ReferencePathPlanner(global_plan=global_plan, env=pm)
+        out = planner.plan_path(LocalPlan())
+        assert abs(out.path[0][0] - 50.0) < 1e-6
+        assert out.trajectory is None
+
 
 class TestLocalPlanningPipeline:
     def _pipeline(self, path="GreedyLatticePlanner", behavioral="", velocity=""):
@@ -135,3 +149,67 @@ 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_velocity_stage_does_not_corrupt_global_speeds(self):
+        """Empty path stage + velocity must not permanently zero the global plan."""
+        setting = PlanningSettingsSchema()
+        setting.c23_behavioral_strategy = ""
+        setting.c23_path_strategy = ""
+        setting.c23_velocity_strategy = "VelocityLocalPlanner"
+        global_plan = _straight_global_plan(x_end=100.0, n=40, velocity=10.0)
+        ref = list(global_plan.trajectory.velocity)
+        pm = PerceptionModel(
+            ego_vehicle=EgoState(x=0.0, y=0.0, theta=0.0, velocity=10.0, length=4.5, width=2.0)
+        )
+        pm.agent_vehicles = [
+            AgentState(x=50.0, y=0.0, theta=0.0, velocity=0.0, length=4.0, width=2.0)
+        ]
+        pipeline = LocalPlanningPipeline(global_plan=global_plan, env=pm, setting=setting)
+
+        pipeline.replan()
+        local_after_hit = pipeline.get_local_plan().as_trajectory()
+        assert local_after_hit is not None
+        assert min(local_after_hit.velocity) < 10.0
+        assert list(global_plan.trajectory.velocity) == ref
+
+        pm.agent_vehicles = []
+        pipeline.replan()
+        assert list(global_plan.trajectory.velocity) == ref
+        local_after_clear = pipeline.get_local_plan().as_trajectory()
+        assert local_after_clear is not None
+        assert min(local_after_clear.velocity) == 10.0
+
+    def test_reference_path_midroute_local_plan_near_ego(self):
+        """Mid-track ReferencePath + velocity must not start the local plan at s=0."""
+        path = [(float(i), 0.0) for i in range(50)] + [(50.0, float(i)) for i in range(1, 51)]
+        n = len(path)
+        vel = [10.0] * n
+        tj = TrajectoryTracker(path=path, velocity=list(vel))
+        tj.ref_left_boundary_d = [3.0] * n
+        tj.ref_right_boundary_d = [-3.0] * n
+        global_plan = GlobalPlan(
+            start_point=path[0],
+            goal_point=path[-1],
+            path=path,
+            velocity=list(vel),
+            trajectory=tj,
+            left_boundary_d=[3.0] * n,
+            right_boundary_d=[-3.0] * n,
+        )
+        ego = EgoState(x=50.0, y=25.0, theta=math.pi / 2, velocity=10.0, length=4.5, width=2.0)
+        pm = PerceptionModel(ego_vehicle=ego)
+        global_plan.trajectory.update_waypoint_by_xy(50.0, 25.0)
+
+        setting = PlanningSettingsSchema()
+        setting.c23_behavioral_strategy = ""
+        setting.c23_path_strategy = "ReferencePathPlanner"
+        setting.c23_velocity_strategy = "VelocityLocalPlanner"
+        pipeline = LocalPlanningPipeline(global_plan=global_plan, env=pm, setting=setting)
+        pipeline.step(ego)
+        pipeline.replan()
+
+        local = pipeline.get_local_plan().as_trajectory()
+        assert local is not None
+        assert abs(local.path[0][0] - 50.0) < 1e-6
+        assert abs(local.path[0][1] - 25.0) < 1.5
+        assert list(global_plan.trajectory.velocity) == vel