From c875374c1b52f82665c60d47a68f443f58efe1ec Mon Sep 17 00:00:00 2001 From: Cursor Agent Date: Wed, 5 Aug 2026 11:12:52 +0000 Subject: [PATCH] Fix closed-track false goal stop, planner wp clamp, and short velocity crash. GoalArrivalMonitor seeded its rising edge after the first in-radius sample, so Yas-style closed lines (goal==start) fired GOAL_ARRIVED/StopExecAtGoal immediately. LocalPlanningStrategy.reset now clamps wp (Plan step-back from 0 no longer jumps to track end). Controllers clamp velocity index when ReferenceSpeed is short. Co-authored-by: Majid Khonji --- CHANGELOG.md | 3 +++ .../c23_local_planning_strategy.py | 10 +++++++ avlite/c30_control/c33_pid.py | 7 ++++- avlite/c30_control/c34_stanley.py | 7 ++++- avlite/c30_control/c35_pure_pursuit.py | 10 +++++-- avlite/c40_execution/c47_execution_tasks.py | 14 +++++++++- .../p60_visualizer_tk/p67_stack_views.py | 5 ++-- .../test_c23_local_planning_pipeline.py | 27 +++++++++++++++++++ test/c30_control/test_c33_pid.py | 19 +++++++++++++ test/c40_execution/test_c43_task_strategy.py | 23 ++++++++++++++++ 10 files changed, 118 insertions(+), 7 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 025328f..982024d 100644 --- a/CHANGELOG.md +++ b/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 diff --git a/avlite/c20_planning/c23_local_planning_strategy.py b/avlite/c20_planning/c23_local_planning_strategy.py index 6dd48a6..9360c1f 100644 --- a/avlite/c20_planning/c23_local_planning_strategy.py +++ b/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]) diff --git a/avlite/c30_control/c33_pid.py b/avlite/c30_control/c33_pid.py index baeef2e..6b2b082 100644 --- a/avlite/c30_control/c33_pid.py +++ b/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 diff --git a/avlite/c30_control/c34_stanley.py b/avlite/c30_control/c34_stanley.py index aaf34b8..789c0fb 100644 --- a/avlite/c30_control/c34_stanley.py +++ b/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 diff --git a/avlite/c30_control/c35_pure_pursuit.py b/avlite/c30_control/c35_pure_pursuit.py index c037f1c..1e3dbc1 100644 --- a/avlite/c30_control/c35_pure_pursuit.py +++ b/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 diff --git a/avlite/c40_execution/c47_execution_tasks.py b/avlite/c40_execution/c47_execution_tasks.py index d6a9eef..f2b7b7b 100644 --- a/avlite/c40_execution/c47_execution_tasks.py +++ b/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 diff --git a/avlite/plugins/p60_visualizer_tk/p67_stack_views.py b/avlite/plugins/p60_visualizer_tk/p67_stack_views.py index 4a58001..2fde4cd 100644 --- a/avlite/plugins/p60_visualizer_tk/p67_stack_views.py +++ b/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): diff --git a/test/c20_planning/test_c23_local_planning_pipeline.py b/test/c20_planning/test_c23_local_planning_pipeline.py index bbee516..8ddaca2 100644 --- a/test/c20_planning/test_c23_local_planning_pipeline.py +++ b/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]) diff --git a/test/c30_control/test_c33_pid.py b/test/c30_control/test_c33_pid.py index 7ea5475..45a86bd 100644 --- a/test/c30_control/test_c33_pid.py +++ b/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 diff --git a/test/c40_execution/test_c43_task_strategy.py b/test/c40_execution/test_c43_task_strategy.py index 7a4a205..35a2d9a 100644 --- a/test/c40_execution/test_c43_task_strategy.py +++ b/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)