From 7040dd232271b520a43f398be57b7d5b856da0d8 Mon Sep 17 00:00:00 2001 From: Cursor Agent Date: Fri, 7 Aug 2026 11:09:10 +0000 Subject: [PATCH] =?UTF-8?q?Fix=20sensors=E2=86=92control=20skipped=20when?= =?UTF-8?q?=20local=20planner=20is=20empty.?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit FollowTheGapController declares no LOCAL_PLAN requirement and docs advertise end-to-end sensors→control, but sync/async executers and Control Step gated on local_planner and never recomputed commands. Allow control with plan=None. Co-authored-by: Majid Khonji --- CHANGELOG.md | 1 + .../c40_execution/c42_execution_strategy.py | 9 +- avlite/c40_execution/c44_sync_executer.py | 1 - .../c45_async_threaded_executer.py | 2 +- .../p60_visualizer_tk/p67_stack_views.py | 10 +- .../test_c44_control_without_planner.py | 120 ++++++++++++++++++ 6 files changed, 136 insertions(+), 7 deletions(-) create mode 100644 test/c40_execution/test_c44_control_without_planner.py diff --git a/CHANGELOG.md b/CHANGELOG.md index 025328f..156701a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -25,6 +25,7 @@ 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 +- Execution / Visualizer: control no longer hard-requires a local planner — sensors→control stacks (`FollowTheGapController` with empty `c40_local_planner`) now recompute and apply commands in sync/async executers and Control **Step** - 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/c40_execution/c42_execution_strategy.py b/avlite/c40_execution/c42_execution_strategy.py index 0ab6517..455f5ba 100644 --- a/avlite/c40_execution/c42_execution_strategy.py +++ b/avlite/c40_execution/c42_execution_strategy.py @@ -338,13 +338,16 @@ def _replan_step(self, sensors: SensorFrame) -> None: def _control_step(self, sim_dt: float, sensors: SensorFrame) -> None: """Recompute control command into ``_last_cmd`` (no world integrate). - Uses the caller-supplied tick snapshot. + Uses the caller-supplied tick snapshot. Controllers that do not require + a plan (e.g. sensors→control Follow-the-Gap) run with ``plan=None``. """ - if not self.controller or not self.local_planner: + if not self.controller: return if not self._can_actuate(): return - local_plan = self.local_planner.get_local_plan() + local_plan = ( + self.local_planner.get_local_plan() if self.local_planner is not None else None + ) cmd = self.controller.control( self.pm.ego_vehicle, local_plan, control_dt=sim_dt, perception_model=self.pm, sensors=sensors, diff --git a/avlite/c40_execution/c44_sync_executer.py b/avlite/c40_execution/c44_sync_executer.py index 7679067..f428bf1 100644 --- a/avlite/c40_execution/c44_sync_executer.py +++ b/avlite/c40_execution/c44_sync_executer.py @@ -103,7 +103,6 @@ def step( do_control = ( call_control and self.controller is not None - and self.local_planner is not None and ( (not pace_control) or (self.elapsed_sim_time - self.__controller_last_time >= control_dt) diff --git a/avlite/c40_execution/c45_async_threaded_executer.py b/avlite/c40_execution/c45_async_threaded_executer.py index 4257f45..7abd377 100644 --- a/avlite/c40_execution/c45_async_threaded_executer.py +++ b/avlite/c40_execution/c45_async_threaded_executer.py @@ -242,7 +242,7 @@ def worker_control(self): with self.lock_world: if is_world_stack_capability_enabled(StackCapability.LOCALIZATION): self.pm.ego_vehicle.copy_from(self.world.get_ego_state()) - if self.controller and self.local_planner: + if self.controller: self._control_step(self.sim_dt, self.world.get_sensor_frame()) # Free-run: sim and real share the same wall interval (start-of-iter stamps). diff --git a/avlite/plugins/p60_visualizer_tk/p67_stack_views.py b/avlite/plugins/p60_visualizer_tk/p67_stack_views.py index 4a58001..3dcb6e3 100644 --- a/avlite/plugins/p60_visualizer_tk/p67_stack_views.py +++ b/avlite/plugins/p60_visualizer_tk/p67_stack_views.py @@ -610,11 +610,17 @@ def update_data(self): self.controller_dropdown_menu["values"] = ("",) + tuple(ControlStrategy.registry.keys()) def step_control(self): - if not self.root.exec or not self.root.exec.controller or not self.root.exec.local_planner: + if not self.root.exec or not self.root.exec.controller: return + # Sensors→control stacks (e.g. FollowTheGap) may omit the local planner. + plan = ( + self.root.exec.local_planner.get_local_plan() + if self.root.exec.local_planner is not None + else None + ) cmd = self.root.exec.controller.control( self.root.exec.ego_state, - self.root.exec.local_planner.get_local_plan(), + plan, control_dt=self.root.setting.sim_dt.get(), perception_model=self.root.exec.pm, sensors=self.root.exec.world.get_sensor_frame(), diff --git a/test/c40_execution/test_c44_control_without_planner.py b/test/c40_execution/test_c44_control_without_planner.py new file mode 100644 index 0000000..ec859ee --- /dev/null +++ b/test/c40_execution/test_c44_control_without_planner.py @@ -0,0 +1,120 @@ +"""Sensors→control must run when no local planner is assembled.""" + +from __future__ import annotations + +from dataclasses import dataclass, field +from types import SimpleNamespace +from typing import Optional + +import numpy as np +import pytest + +from avlite.c10_perception.c11_perception_model import EGO_AGENT_ID, EgoState, PerceptionModel +from avlite.c30_control.c31_control_model import AckermannControlCommand +from avlite.c30_control.c35_pure_pursuit import FollowTheGapController +from avlite.c30_control.c39_settings import ControlSettings +from avlite.c40_execution.c41_world_bridge import WorldBridge +from avlite.c40_execution.c44_sync_executer import SyncExecuter +from avlite.c40_execution.c49_settings import ExecutionSettings +from avlite.c50_common.c51_capabilities import StackCapability +from avlite.c50_common.c52_world_sensor_datatypes import SensorFrame +from avlite.c60_apps.c62_factory import executor_factory + + +@dataclass +class _PlantWorld(WorldBridge): + """Minimal plant that records applied commands and serves LiDAR.""" + + ego_state: EgoState = field( + default_factory=lambda: EgoState(x=0.0, y=0.0, theta=0.0, velocity=0.0) + ) + perception_model: Optional[PerceptionModel] = None + world_capabilities = frozenset() + stack_capabilities = frozenset({StackCapability.LOCALIZATION}) + applied: list = field(default_factory=list) + + def control_ego_state(self, cmd, dt: Optional[float] = 0.01): + self.applied.append(cmd) + # Crude integrate so factory smoke can observe motion. + self.ego_state.velocity = max(0.0, self.ego_state.velocity + float(cmd.acceleration) * float(dt or 0.01)) + self.ego_state.x += self.ego_state.velocity * float(dt or 0.01) + + def get_sensor_frame(self, agent_id: int = EGO_AGENT_ID) -> SensorFrame: + # Forward cone of returns so Follow-the-Gap can pick a gap. + angles = np.linspace(-0.8, 0.8, 40) + r = 12.0 + pts = np.column_stack( + [ + self.ego_state.x + r * np.cos(self.ego_state.theta + angles), + self.ego_state.y + r * np.sin(self.ego_state.theta + angles), + np.zeros_like(angles), + np.zeros_like(angles), + ] + ).astype(np.float32) + return SensorFrame(lidar=pts) + + +def test_sync_control_runs_without_local_planner(): + """Hard gate on local_planner previously skipped sensors→control entirely.""" + seen: dict = {} + + def control(ego, plan=None, control_dt=None, perception_model=None, sensors=None): + seen["plan"] = plan + seen["sensors"] = sensors + return AckermannControlCommand(steer=0.1, acceleration=1.0) + + world = _PlantWorld() + exec_ = SyncExecuter( + perception_model=PerceptionModel(ego_vehicle=EgoState(x=0.0, y=0.0)), + world=world, + perception=None, + localization=None, + global_planner=None, + local_planner=None, + controller=SimpleNamespace( + world_requirements=frozenset(), + stack_requirements=frozenset({StackCapability.LOCALIZATION}), + stack_capabilities=frozenset({StackCapability.CONTROL}), + control=control, + reset=lambda: None, + ), + ) + + exec_.step(sim_dt=0.05, control_dt=0.0, pace_control=True, pace_sim=True) + + assert "sensors" in seen + assert seen["plan"] is None + assert exec_._last_cmd is not None + assert exec_._last_cmd.acceleration == pytest.approx(1.0) + assert len(world.applied) == 1 + + +def test_follow_the_gap_factory_stack_actuates_without_local_planner(): + """Documented sensors→control composition must move the plant.""" + # Keep world GT localization so _can_actuate passes with no localization module. + prev_caps = ExecutionSettings.c41_world_stack_capabilities + prev_cruise = ControlSettings.c35_cruise_velocity + try: + ExecutionSettings.c41_world_stack_capabilities = None # allow defaults / all + ControlSettings.c35_cruise_velocity = 5.0 + ex = executor_factory( + local_planner_strategy_name="", + controller_strategy_name="FollowTheGapController", + global_planner_strategy_name="", + perception_strategy_name="", + localization_strategy_name="", + load_plugins=False, + ) + assert ex.local_planner is None + assert isinstance(ex.controller, FollowTheGapController) + + x0 = ex.world.get_ego_state().x + for _ in range(25): + ex.step(sim_dt=0.05, control_dt=0.0, replan_dt=1e9, perception_dt=1e9) + + assert ex._last_cmd is not None + assert ex._last_cmd.acceleration != 0.0 or ex._last_cmd.steer != 0.0 + assert ex.world.get_ego_state().x != pytest.approx(x0, abs=1e-6) + finally: + ExecutionSettings.c41_world_stack_capabilities = prev_caps + ControlSettings.c35_cruise_velocity = prev_cruise