Fix async Control/Plan toggle orphans and before-start waypoint index

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
+- Execution: `AsyncThreadedExecuter` no longer treats intentional Control/Plan stage-off as a crashed worker — toggling those checkboxes while async exec runs used to clear the join list without stopping live peers, then vacuous `all([])` orphaned/duplicated Planner threads and left Control unable to restart cleanly
+- Common: `TrajectoryTracker.update_waypoint_by_xy` resets to waypoint 0 when ego is before path start (`closest_wp==0` and projected `s < path_s[0]`) — teleport/reset upstream of the first waypoint no longer leaves Stanley/Pure Pursuit on a stale far-path segment (max steer / wrong target speed)
 - 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/c40_execution/c45_async_threaded_executer.py

@@ -114,24 +114,14 @@ def step(
             self.create_threads()
             self.start_threads()
             return
-        elif self.threads_started and all(not t.is_alive() for t in self.threads):
-            log.warning(f"All threads are dead. Recreating and starting threads.")
-            self.stop()
-            self.create_threads()
-            self.start_threads()
-            return
-        elif (
-            self.threads_started
-            and (
-                (self.planner_thread and call_replan != self.planner_thread.is_alive())
-                or (self.controller_thread and call_control != self.controller_thread.is_alive())
-            )
-        ):  # or call_perceive != (self.perception_thread.is_alive() if self.perception_thread else False):
 
-            log.error( f"Some threads are dead: {self.planner_thread.is_alive() if self.planner_thread else 'None'}, Controller status: {self.controller_thread.is_alive() if self.controller_thread else 'None'} . Call stop() to terminate all threads.")
-            self.create_threads()
-            self.start_threads()
-            return
+        # Drop finished workers from the join list. Intentional stage-off exits
+        # (call_control / call_replan False) end the worker loop; the old code
+        # treated "flag != is_alive" as a crash, cleared self.threads without
+        # stopping live peers, then hit vacuous all([]) and orphaned / duplicated
+        # Planner threads. Prune + ensure restarts only stages that are enabled.
+        self.threads = [t for t in self.threads if t is not None and t.is_alive()]
+        self._ensure_enabled_workers(call_replan, call_control)
 
         # delta_t_exec = time.time() - self.__prev_exec_time if self.__prev_exec_time is not None else 0
         # self.__prev_exec_time = time.time()
@@ -331,25 +321,67 @@ def stop(self):
             self.perception_thread = None
             self.threads_started = False
 
+    def _ensure_enabled_workers(self, call_replan: bool, call_control: bool) -> None:
+        """Start workers for stages that were re-enabled after an intentional exit."""
+        if call_replan and (self.planner_thread is None or not self.planner_thread.is_alive()):
+            self.planner_thread = threading.Thread(
+                target=self.worker_planning, name="Planner", daemon=True
+            )
+            self.threads.append(self.planner_thread)
+            self.stopped = False
+            self.__planner_start_time = time.time()
+            self.planner_thread.start()
+            log.info("Planner thread restarted after stage enable")
+
+        if call_control and (self.controller_thread is None or not self.controller_thread.is_alive()):
+            self.controller_thread = threading.Thread(
+                target=self.worker_control, name="Controller", daemon=True
+            )
+            self.threads.append(self.controller_thread)
+            self.stopped = False
+            self.controller_thread.start()
+            log.info("Controller thread restarted after stage enable")
+
+        if (
+            not self._combined_perception_planning
+            and self.call_perceive
+            and (self.perception_thread is None or not self.perception_thread.is_alive())
+        ):
+            self.perception_thread = threading.Thread(
+                target=self.worker_perception, name="Perception", daemon=True
+            )
+            self.threads.append(self.perception_thread)
+            self.stopped = False
+            self.perception_thread.start()
+            log.info("Perception thread restarted after stage enable")
+
     def create_threads(self):
         log.info(f"Creating threads...")
-        # Make threads daemon so they exit when main thread exits
-        self.threads = []
+        # Keep any still-alive workers in the join list so a later stop() can
+        # signal them. Never drop live peers when only one stage needs a new thread.
+        self.threads = [t for t in self.threads if t is not None and t.is_alive()]
 
         if self.planner_thread is None or not self.planner_thread.is_alive():
             self.planner_thread = threading.Thread( target=self.worker_planning, name="Planner", daemon=True,  )
             self.threads.append(self.planner_thread)
             log.info(f"Planner thread created: {self.planner_thread.name}")
+        elif self.planner_thread not in self.threads:
+            self.threads.append(self.planner_thread)
 
         if self.controller_thread is None or not self.controller_thread.is_alive():
             self.controller_thread = threading.Thread(target=self.worker_control, name="Controller", daemon=True)
             self.threads.append(self.controller_thread)
             log.info(f"Controller thread created: {self.controller_thread.name}")
+        elif self.controller_thread not in self.threads:
+            self.threads.append(self.controller_thread)
 
         if not self._combined_perception_planning:
-            self.perception_thread = threading.Thread(target=self.worker_perception, name="Perception", daemon=True)
-            self.threads.append(self.perception_thread)
-            log.info(f"Perception thread created: {self.perception_thread.name}")
+            if self.perception_thread is None or not self.perception_thread.is_alive():
+                self.perception_thread = threading.Thread(target=self.worker_perception, name="Perception", daemon=True)
+                self.threads.append(self.perception_thread)
+                log.info(f"Perception thread created: {self.perception_thread.name}")
+            elif self.perception_thread not in self.threads:
+                self.threads.append(self.perception_thread)
 
         log.info(f"{len(self.threads)} threads created.")
 

avlite/c50_common/c54_trajectory_tracker.py

@@ -218,6 +218,12 @@ def update_waypoint_by_xy(self, x_current: float, y_current: float) -> None:
         elif self.path_s[closest_wp] > s_[0] and closest_wp > 0:
             self.next_wp = closest_wp
             self.current_wp = closest_wp - 1
+        else:
+            # Before path start: nearest wp is 0 and projected s < path_s[0].
+            # Leaving current_wp unchanged left Stanley/PP on a stale segment
+            # (e.g. after teleporting back upstream of the first waypoint).
+            self.current_wp = 0
+            self.next_wp = 1 if len(self.__reference_path) > 1 else 0
 
     def update_waypoint_by_xy_forward(
         self,

test/c40_execution/test_c45_async_stage_toggle.py

@@ -0,0 +1,165 @@
+"""Regression: toggling Control/Plan off must not orphan or duplicate async workers."""
+
+from __future__ import annotations
+
+import threading
+import time
+from dataclasses import dataclass, field
+from typing import Optional
+
+from avlite.c10_perception.c11_perception_model import EgoState, PerceptionModel
+from avlite.c20_planning.c21_planning_model import LocalPlan
+from avlite.c20_planning.c23_local_planning_strategy import LocalPlanningStrategy
+from avlite.c30_control.c31_control_model import ControlCommand
+from avlite.c30_control.c32_control_strategy import ControlStrategy
+from avlite.c40_execution.c41_world_bridge import WorldBridge
+from avlite.c40_execution.c45_async_threaded_executer import AsyncThreadedExecuter
+from avlite.c50_common.c51_capabilities import StackCapability
+
+
+@dataclass
+class _StubWorldBridge(WorldBridge):
+    ego_state: EgoState = field(default_factory=lambda: EgoState(x=0, y=0, theta=0, velocity=0))
+    perception_model: Optional[PerceptionModel] = None
+
+    world_capabilities = frozenset()
+    stack_capabilities = frozenset({StackCapability.LOCALIZATION})
+
+    def control_ego_state(self, cmd: ControlCommand, dt: float = 0.01):
+        pass
+
+    def get_ego_state(self) -> EgoState:
+        return self.ego_state
+
+
+class _StubLocalPlanner(LocalPlanningStrategy):
+    world_requirements = frozenset()
+    stack_requirements = frozenset()
+    stack_capabilities = frozenset({StackCapability.LOCAL_PLAN})
+
+    def __init__(self):
+        self.lap = 0
+        self._plan = LocalPlan(
+            path=[(0.0, 0.0), (1.0, 0.0), (2.0, 0.0)],
+            velocity=[1.0, 1.0, 1.0],
+        )
+
+    def replan(self, perception_model=None, sensors=None):
+        pass
+
+    def step(self, ego_state):
+        pass
+
+    def get_local_plan(self):
+        return self._plan
+
+    def reset(self):
+        pass
+
+    def __init_subclass__(cls, **kwargs):
+        pass
+
+
+class _StubController(ControlStrategy, abstract=True):
+    def control(
+        self, ego, plan=None, control_dt=None, perception_model=None, sensors=None,
+    ) -> ControlCommand:
+        return ControlCommand()
+
+    def reset(self):
+        pass
+
+
+def _make_async_executer() -> AsyncThreadedExecuter:
+    return AsyncThreadedExecuter(
+        perception_model=PerceptionModel(
+            ego_vehicle=EgoState(x=0, y=0, theta=0, velocity=0)
+        ),
+        perception=None,
+        global_planner=None,
+        local_planner=_StubLocalPlanner(),
+        controller=_StubController(),
+        world=_StubWorldBridge(),
+        control_dt=0.05,
+        replan_dt=0.05,
+    )
+
+
+def _count_named(name: str) -> int:
+    return sum(1 for t in threading.enumerate() if t.name == name and t.is_alive())
+
+
+def _step(exec_: AsyncThreadedExecuter, *, call_replan: bool, call_control: bool) -> None:
+    exec_.step(
+        call_replan=call_replan,
+        call_control=call_control,
+        call_perceive=False,
+        call_localize=False,
+        replan_dt=0.05,
+        control_dt=0.05,
+        sim_dt=0.05,
+    )
+
+
+def test_control_toggle_off_does_not_duplicate_planner():
+    """UI uncheck Control must exit the controller without orphaning/duplicating Planner."""
+    exec_ = _make_async_executer()
+    try:
+        _step(exec_, call_replan=True, call_control=True)
+        time.sleep(0.15)
+        assert _count_named("Planner") == 1
+        assert _count_named("Controller") == 1
+
+        for _ in range(3):
+            _step(exec_, call_replan=True, call_control=False)
+            time.sleep(0.12)
+
+        assert _count_named("Planner") == 1
+        assert _count_named("Controller") == 0
+        assert exec_.planner_thread is not None and exec_.planner_thread.is_alive()
+    finally:
+        exec_.stop()
+        time.sleep(0.2)
+
+
+def test_control_toggle_on_restarts_single_controller():
+    """Re-checking Control must start exactly one Controller without duplicating Planner."""
+    exec_ = _make_async_executer()
+    try:
+        _step(exec_, call_replan=True, call_control=True)
+        time.sleep(0.15)
+
+        _step(exec_, call_replan=True, call_control=False)
+        time.sleep(0.15)
+        assert _count_named("Controller") == 0
+
+        _step(exec_, call_replan=True, call_control=True)
+        time.sleep(0.15)
+
+        assert _count_named("Planner") == 1
+        assert _count_named("Controller") == 1
+    finally:
+        exec_.stop()
+        time.sleep(0.2)
+
+
+def test_plan_toggle_off_on_keeps_single_workers():
+    """Same lifecycle for the Planning stage checkbox."""
+    exec_ = _make_async_executer()
+    try:
+        _step(exec_, call_replan=True, call_control=True)
+        time.sleep(0.15)
+
+        for _ in range(2):
+            _step(exec_, call_replan=False, call_control=True)
+            time.sleep(0.12)
+        assert _count_named("Planner") == 0
+        assert _count_named("Controller") == 1
+
+        _step(exec_, call_replan=True, call_control=True)
+        time.sleep(0.15)
+        assert _count_named("Planner") == 1
+        assert _count_named("Controller") == 1
+    finally:
+        exec_.stop()
+        time.sleep(0.2)

test/c50_common/test_c54_trajectory_waypoint_update.py

@@ -39,6 +39,24 @@ def test_update_waypoint_by_wp_mid_path_advances_next():
     assert tj.next_wp == 3
 
 
+def test_update_waypoint_by_xy_before_path_start_resets_indices():
+    """Teleport/reset upstream of wp0 must not leave a stale far-path current_wp.
+
+    Stanley uses ``get_current_heading()`` / ``velocity[current_wp]``; a stale
+    index after an L-path corner commanded max steer / wrong target speed.
+    """
+    # Horizontal then vertical: heading at the end is π/2.
+    path = [(float(i), 0.0) for i in range(21)] + [(20.0, float(j)) for j in range(1, 11)]
+    tj = TrajectoryTracker(path=path, velocity=[10.0] * len(path))
+    tj.update_waypoint_by_xy(20.0, 8.0)
+    assert tj.current_wp > 20
+
+    tj.update_waypoint_by_xy(-0.5, 0.0)
+    assert tj.current_wp == 0
+    assert tj.next_wp == 1
+    assert tj.get_current_heading() == pytest.approx(0.0, abs=1e-6)
+
+
 def test_create_quintic_trajectory_sd_honors_boundary_derivatives():
     """b-vector must match A rows: value, value, 1st, 1st, 2nd, 2nd (start then end)."""
     path = [(float(i), 0.0) for i in range(40)]