diff --git a/CHANGELOG.md b/CHANGELOG.md index 025328f..a4700fa 100644 --- a/CHANGELOG.md +++ b/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 diff --git a/avlite/c40_execution/c44_sync_executer.py b/avlite/c40_execution/c44_sync_executer.py index 7679067..6e8819b 100644 --- a/avlite/c40_execution/c44_sync_executer.py +++ b/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) diff --git a/avlite/c50_common/c54_trajectory_tracker.py b/avlite/c50_common/c54_trajectory_tracker.py index 7883e1f..bd10cd8 100644 --- a/avlite/c50_common/c54_trajectory_tracker.py +++ b/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). diff --git a/avlite/plugins/p60_visualizer_tk/p66_plot_views.py b/avlite/plugins/p60_visualizer_tk/p66_plot_views.py index 2199b7b..9fad2aa 100644 --- a/avlite/plugins/p60_visualizer_tk/p66_plot_views.py +++ b/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: diff --git a/test/c40_execution/test_c44_executer_fps.py b/test/c40_execution/test_c44_executer_fps.py index 7674596..c349ab2 100644 --- a/test/c40_execution/test_c44_executer_fps.py +++ b/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() diff --git a/test/c50_common/test_c54_closed_loop_path_s.py b/test/c50_common/test_c54_closed_loop_path_s.py index 3afbb90..beff21a 100644 --- a/test/c50_common/test_c54_closed_loop_path_s.py +++ b/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