Fix Sync ZOH after Control off and Frenet SD orientation

CHANGELOG.md

@@ -25,6 +25,9 @@ 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: `SyncExecuter` clears the held ZOH control command when `call_control=False` — unchecking Control no longer keeps integrating the last accel/steer into the plant
+- Common: `TrajectoryTracker.convert_sd_orientation_to_xy_orientation` uses the path tangent at query `s` (same segment bracketing as `convert_sd_to_xy`) instead of tracker `current_wp`/`next_wp` — Frenet teleport/spawn no longer points along the wrong corridor
+- Visualizer: Frenet-view ego orient drag converts orientation at the click pose (`teleport_s`/`teleport_d`), not the mouse-tip cursor
 - 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/c44_sync_executer.py

@@ -110,6 +110,13 @@ def step(
             )
         )
 
+        # Control disabled: drop any held ZOH command so simulate cannot keep
+        # integrating the last accel/steer after the UI unchecks Control.
+        # (Paced control with call_control=True still holds _last_cmd between
+        # recomputes — that is intentional zero-order hold.)
+        if not call_control:
+            self._last_cmd = None
+
         sensors = (
             self.world.get_sensor_frame()
             if (do_localize or do_perceive or do_replan or do_control)

avlite/c50_common/c54_trajectory_tracker.py

@@ -680,14 +680,30 @@ def convert_sd_orientation_to_xy_orientation(self, s: float, d: float, theta:flo
         Convert Frenet coordinates (s, d) and orientation theta to Cartesian coordinates (x, y) and orientation.
         :param s: Frenet s coordinate
         :param d: Frenet d coordinate
-        :param theta: Orientation in radians
+        :param theta: Orientation in radians relative to the path tangent at ``s``
         :return: Tuple of (x, y, orientation)
         """
         x, y = self.convert_sd_to_xy(s, d)
-        theta = theta + math.atan2(self.path_y[self.next_wp] - self.path_y[self.current_wp],
-            self.path_x[self.next_wp] - self.path_x[self.current_wp],)
-
-        return x, y, theta
+        # Path heading must come from the segment that brackets ``s`` (same as
+        # convert_sd_to_xy). Using tracker current_wp/next_wp is wrong whenever the
+        # query pose is not on the currently tracked segment — Frenet teleport/spawn
+        # then points the ego/NPC along the wrong corridor.
+        n = len(self.__path_s_array)
+        if n < 2:
+            heading = float(self.path_heading[0]) if len(self.path_heading) else 0.0
+        else:
+            idx = int(np.searchsorted(self.__path_s_array, s))
+            if idx <= 0:
+                prev_wp, next_wp = 0, 1
+            elif idx >= n:
+                prev_wp, next_wp = n - 2, n - 1
+            else:
+                prev_wp, next_wp = idx - 1, idx
+            heading = math.atan2(
+                self.path_y[next_wp] - self.path_y[prev_wp],
+                self.path_x[next_wp] - self.path_x[prev_wp],
+            )
+        return x, y, theta + heading
 
     def _frenet_on_segment(self, point, prev_wp: int, next_wp: int) -> tuple[float, float, float]:
         """Project ``point`` onto segment (prev_wp, next_wp).

avlite/plugins/p60_visualizer_tk/p66_plot_views.py

@@ -361,8 +361,12 @@ def on_mouse_move(self, event):
                 if self.left_mouse_button_pressed:
                     teleport_orientation = np.arctan2(y - self.teleport_d, x - self.teleport_s)
                     self.local_plot.show_vehicle_orientation_ax2(s=self.teleport_s, d=self.teleport_d, theta=teleport_orientation) 
-                    x_,y_, theta = self.root.exec.local_planner.global_plan.trajectory.convert_sd_orientation_to_xy_orientation(x,y,teleport_orientation)
-                    self.root.teleport_ego(self.teleport_x, self.teleport_y,theta)
+                    # Use the click pose (teleport_s/d), not the drag-tip cursor (x,y),
+                    # so world heading is path-tangent at the ego, not at the mouse tip.
+                    _, _, theta = self.root.exec.local_planner.global_plan.trajectory.convert_sd_orientation_to_xy_orientation(
+                        self.teleport_s, self.teleport_d, teleport_orientation
+                    )
+                    self.root.teleport_ego(self.teleport_x, self.teleport_y, theta)
                     self.root.exec.local_planner.step(state=self.root.exec.world.get_ego_state())       
                     self.root.update_ui()
                 elif self.right_mouse_button_pressed and not self.spawn_in_ax1:

test/c40_execution/test_c44_executer_fps.py

@@ -263,6 +263,26 @@ def test_simulate_holds_last_cmd_without_control_recompute(self):
         assert exec_.controller.calls == 1
         assert exec_.world.sim_calls == 5
 
+    def test_call_control_false_drops_held_zoh_command(self):
+        """Unchecking Control must stop plant actuation (not keep integrating last cmd)."""
+        exec_, _ = _make_sync_executer()
+        exec_.step(
+            control_dt=0.01, sim_dt=0.01, replan_dt=99, localization_dt=99,
+            call_replan=False, call_perceive=False, call_localize=False,
+            call_control=True, pace_control=True,
+        )
+        assert exec_.world.sim_calls == 1
+        assert exec_._last_cmd is not None
+        for _ in range(5):
+            exec_.step(
+                control_dt=0.01, sim_dt=0.01, replan_dt=99, localization_dt=99,
+                call_replan=False, call_perceive=False, call_localize=False,
+                call_control=False, pace_control=True,
+            )
+        assert exec_.controller.calls == 1
+        assert exec_.world.sim_calls == 1
+        assert exec_._last_cmd is None
+
     def test_free_run_sim_matches_real_clock(self):
         """When pace_sim is off, sim and real must advance by the same wall intervals."""
         exec_, _ = _make_sync_executer()

test/c50_common/test_c54_closed_loop_path_s.py

@@ -3,6 +3,7 @@
 from __future__ import annotations
 
 import json
+import math
 from pathlib import Path
 
 import numpy as np
@@ -71,3 +72,19 @@ def test_track_end_s_matches_final_arc_length_including_short_paths():
     assert abs(tj.track_end_s - tj.path_s[-1]) < 1e-9
     # Stale [-2] workaround is one segment short of the true lap length.
     assert tj.path_s[-2] < tj.track_end_s - 1.0
+
+
+def test_sd_orientation_uses_heading_at_s_not_tracker_wp():
+    """Frenet teleport/spawn must add path tangent at query s, not current_wp."""
+    path = [(0.0, 0.0), (10.0, 0.0), (10.0, 10.0)]
+    tj = TrajectoryTracker(path=path, velocity=[1.0] * len(path))
+    assert tj.current_wp == 0
+    assert tj.next_wp == 1
+    # Vertical leg at s=15; relative theta=0 → world heading π/2.
+    _, _, theta = tj.convert_sd_orientation_to_xy_orientation(15.0, 0.0, 0.0)
+    assert abs(theta - math.pi / 2) < 1e-6
+    # At final waypoint current==next (atan2(0,0) was previously 0).
+    tj.update_waypoint_by_wp(2)
+    assert tj.current_wp == tj.next_wp
+    _, _, theta2 = tj.convert_sd_orientation_to_xy_orientation(15.0, 0.0, 0.1)
+    assert abs(theta2 - (math.pi / 2 + 0.1)) < 1e-6