From 5daaac27932f4323c488c73908782f606e0b16a1 Mon Sep 17 00:00:00 2001 From: Cursor Agent Date: Thu, 6 Aug 2026 11:16:01 +0000 Subject: [PATCH] Fix pipeline velocity stage corrupting global speeds and mid-track path origin. VelocityLocalPlanner.plan_velocity now profiles a copy so LocalPlanningPipeline cannot permanently zero the shared global reference. ReferencePathPlanner emits the forward horizon from current_wp instead of the full route from s=0. Co-authored-by: Majid Khonji --- CHANGELOG.md | 2 + .../c20_planning/c26_local_path_planners.py | 10 ++- ..._local_behavioral_and_velocity_planners.py | 33 ++++++-- .../test_c23_local_planning_pipeline.py | 80 ++++++++++++++++++- 4 files changed, 117 insertions(+), 8 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 025328f..201b23f 100644 --- a/CHANGELOG.md +++ b/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 diff --git a/avlite/c20_planning/c26_local_path_planners.py b/avlite/c20_planning/c26_local_path_planners.py index 9960000..f116dce 100644 --- a/avlite/c20_planning/c26_local_path_planners.py +++ b/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 diff --git a/avlite/c20_planning/c27_local_behavioral_and_velocity_planners.py b/avlite/c20_planning/c27_local_behavioral_and_velocity_planners.py index a15a1f3..bf609f7 100644 --- a/avlite/c20_planning/c27_local_behavioral_and_velocity_planners.py +++ b/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, diff --git a/test/c20_planning/test_c23_local_planning_pipeline.py b/test/c20_planning/test_c23_local_planning_pipeline.py index bbee516..8e3dc76 100644 --- a/test/c20_planning/test_c23_local_planning_pipeline.py +++ b/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