From 6b5022895a82619bbe7f05931145dd178b809d6a Mon Sep 17 00:00:00 2001 From: Nathan Margolis Date: Wed, 12 Nov 2025 20:15:40 -0800 Subject: [PATCH 1/5] Add StanleyController for vehicle control Implement StanleyController for vehicle control using lateral and simple longitudinal strategies. --- CtrlPlanning/controller.py | 131 +++++++++++++++++++++++++++++++++++++ 1 file changed, 131 insertions(+) create mode 100644 CtrlPlanning/controller.py diff --git a/CtrlPlanning/controller.py b/CtrlPlanning/controller.py new file mode 100644 index 0000000..0940303 --- /dev/null +++ b/CtrlPlanning/controller.py @@ -0,0 +1,131 @@ +import numpy as np +from typing import Tuple + + +Array2D = np.ndarray + + +class StanleyController: + def __init__( + self, + k_e: float, # gain for cross-track error + k_yaw: float, # gain for heading error + k_v: float, # speed softening term in denominator + max_steer: float, # steering limit (radians) + k_throttle: float, # gain for throttle control + k_brake: float, # gain for brake control + speed_deadband: float # m/s deadband around target speed + ) -> None: + self.k_e: float = k_e + self.k_yaw: float = k_yaw + self.k_v: float = k_v + self.max_steer: float = max_steer + self.k_throttle: float = k_throttle + self.k_brake: float = k_brake + self.speed_deadband: float = speed_deadband + + @staticmethod + def _normalize_angle(angle: float) -> float: + """Wrap angle to [-pi, pi].""" + a: float = (angle + np.pi) % (2 * np.pi) - np.pi + return a + + def compute_control( + self, + x: float, + y: float, + yaw: float, + v: float, + path_xy: Array2D, # shape (N, 2) + v_ref: float + ) -> Tuple[float, float, float]: + """ + Stanley lateral + simple P speed control. + + Parameters + ---------- + x, y : float + Vehicle position in global frame. + yaw : float + Vehicle heading (rad, global frame). + v : float + Vehicle speed (m/s). + path_xy : np.ndarray, shape (N, 2) + Reference path waypoints [[x0, y0], [x1, y1], ...]. + v_ref : float + Target speed along path (m/s). + + Returns + ------- + delta_cmd : float + Steering command (rad), saturated to [-max_steer, max_steer]. + throttle_cmd : float + Throttle command [0, 1]. + brake_cmd : float + Brake command [0, 1]. + """ + + # --- 1. Find nearest point on the path --- + dx: Array2D = path_xy[:, 0] - x + dy: Array2D = path_xy[:, 1] - y + d2: Array2D = dx**2 + dy**2 + + nearest_idx: int = int(np.argmin(d2)) + nearest_point: Array2D = path_xy[nearest_idx] + + # --- 2. Compute path heading at nearest segment --- + if nearest_idx < len(path_xy) - 1: + next_point: Array2D = path_xy[nearest_idx + 1] + else: + # at the end, look backwards + next_point = path_xy[nearest_idx] + nearest_point = path_xy[nearest_idx - 1] + + path_dx: float = float(next_point[0] - nearest_point[0]) + path_dy: float = float(next_point[1] - nearest_point[1]) + path_yaw: float = float(np.arctan2(path_dy, path_dx)) + + # --- 3. Heading error (vehicle vs path) --- + heading_error: float = self._normalize_angle(path_yaw - yaw) + + # --- 4. Cross-track error --- + # Transform vector from vehicle -> nearest point into vehicle frame + # Vehicle frame: x forward, y to left + map_to_vehicle_R: Array2D = np.array([ + [np.cos(-yaw), -np.sin(-yaw)], + [np.sin(-yaw), np.cos(-yaw)] + ]) + + vec_to_nearest_map: Array2D = np.array([ + nearest_point[0] - x, + nearest_point[1] - y + ]) + vec_to_nearest_vehicle: Array2D = map_to_vehicle_R @ vec_to_nearest_map + cross_track_error: float = float(vec_to_nearest_vehicle[1]) # y in vehicle frame + + # --- 5. Stanley steering law (with speed softening term) --- + eps: float = 1e-3 + denom: float = v + self.k_v + eps + stanley_term: float = float(np.arctan2(self.k_e * cross_track_error, denom)) + delta_cmd: float = self.k_yaw * heading_error + stanley_term + + # Saturate steering command + delta_cmd = float(np.clip(delta_cmd, -self.max_steer, self.max_steer)) + + # --- 6. SIMPLE (i.e. placeholder) longitudinal P controller (throttle/brake) --- + speed_error: float = v_ref - v + + if abs(speed_error) < self.speed_deadband: + # within deadband: no accel, no brake + throttle_cmd: float = 0.0 + brake_cmd: float = 0.0 + elif speed_error > 0.0: + # need to accelerate + throttle_cmd = float(np.clip(self.k_throttle * speed_error, 0.0, 1.0)) + brake_cmd = 0.0 + else: + # need to decelerate + throttle_cmd = 0.0 + brake_cmd = float(np.clip(self.k_brake * (-speed_error), 0.0, 1.0)) + + return delta_cmd, throttle_cmd, brake_cmd From 328a5b720190cb6705db2ac3d8e08f8af9822b19 Mon Sep 17 00:00:00 2001 From: Dell Date: Mon, 24 Nov 2025 16:37:45 -0800 Subject: [PATCH 2/5] Add simulation and visualization for controller --- CtrlPlanning/demo.py | 154 ++++++++++++ CtrlPlanning/run_simulation.py | 336 +++++++++++++++++++++++++++ CtrlPlanning/visualize_controller.py | 154 ++++++++++++ 3 files changed, 644 insertions(+) create mode 100644 CtrlPlanning/demo.py create mode 100644 CtrlPlanning/run_simulation.py create mode 100644 CtrlPlanning/visualize_controller.py diff --git a/CtrlPlanning/demo.py b/CtrlPlanning/demo.py new file mode 100644 index 0000000..5b8f204 --- /dev/null +++ b/CtrlPlanning/demo.py @@ -0,0 +1,154 @@ +import numpy as np +import time +import matplotlib.pyplot as plt +from matplotlib.patches import Rectangle +from controller import StanleyController # 你的文件 + +# ============================================================ +# 1. HUD 可视化 +# ============================================================ +class ControlVisualizer: + def __init__(self, max_steer_rad): + self.max_steer = max_steer_rad + + plt.ion() + self.fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(7, 4)) + self.ax_steer = ax1 + self.ax_long = ax2 + + # ---- Steering bar ---- + self.ax_steer.set_title("Steering") + self.ax_steer.set_xlim(-1, 1) + self.ax_steer.set_ylim(0, 1) + self.ax_steer.axvline(0, color='black') + self.ax_steer.set_yticks([]) + self.steer_rect = Rectangle((0, 0), 0, 1, color='cyan') + self.ax_steer.add_patch(self.steer_rect) + + # ---- Longitudinal ---- + self.ax_long.set_title("Throttle / Brake") + self.ax_long.set_xlim(0, 1) + self.ax_long.set_ylim(-1, 1) + self.ax_long.axhline(0, color='black') + self.ax_long.set_xticks([]) + + self.throttle_rect = Rectangle((0.25, 0), 0.5, 0, color='green') + self.brake_rect = Rectangle((0.25, 0), 0.5, 0, color='red') + self.ax_long.add_patch(self.throttle_rect) + self.ax_long.add_patch(self.brake_rect) + + def update(self, delta, throttle, brake): + steer_norm = np.clip(delta / self.max_steer, -1, 1) + + # steering + if steer_norm >= 0: + self.steer_rect.set_x(0) + self.steer_rect.set_width(steer_norm) + else: + self.steer_rect.set_x(steer_norm) + self.steer_rect.set_width(-steer_norm) + + # throttle / brake + long_norm = throttle - brake + self.throttle_rect.set_height(max(long_norm, 0)) + self.brake_rect.set_y(min(long_norm, 0)) + self.brake_rect.set_height(abs(min(long_norm, 0))) + + self.fig.canvas.draw() + self.fig.canvas.flush_events() + +# ============================================================ +# 2. 构造 C 型赛道：直线 → 左弯 → 直线 +# ============================================================ +def generate_track_C(): + """ + Absolute coordinate path. + 然后会在每一帧转成 relative coordinate。 + """ + + # segment A: straight + A = np.array([[i, 0] for i in range(20)]) + + # segment B: left turn, radius 15m, 60 degrees arc + R = 15 + angles = np.linspace(0, np.deg2rad(60), 40) + B = np.stack([ + 20 + R * np.sin(angles), + 0 + R * (1 - np.cos(angles)) + ], axis=1) + + # segment C: straight after turn + C = np.array([[20 + R*np.sin(np.deg2rad(60)) + i, + R*(1 - np.cos(np.deg2rad(60)))] for i in range(20)]) + + return np.vstack([A, B, C]) + +# ============================================================ +# 3. 把 absolute waypoints 转到车坐标 +# ============================================================ +def to_relative(path_xy, x, y, yaw): + """ + 车辆永远是 (0,0, yaw=0)，waypoints 转到 local coordinate. + """ + dx = path_xy[:, 0] - x + dy = path_xy[:, 1] - y + cos_y = np.cos(-yaw) + sin_y = np.sin(-yaw) + + # rotation + x_local = dx * cos_y - dy * sin_y + y_local = dx * sin_y + dy * cos_y + return np.stack([x_local, y_local], axis=1) + +# ============================================================ +# 4. 简单动力学（非常简化） +# ============================================================ +def simple_dynamics(x, y, yaw, v, delta, throttle, brake, dt=0.05): + a = throttle*2.0 - brake*3.0 + v = max(0, v + a*dt) + x += v * np.cos(yaw) * dt + y += v * np.sin(yaw) * dt + yaw += v/2.5 * np.tan(delta) * dt + return x, y, yaw, v + +# ============================================================ +# 5. 主模拟程序 +# ============================================================ +def main(): + track = generate_track_C() + + controller = StanleyController( + k_e=1.0, + k_yaw=1.2, + k_v=1.0, + max_steer=np.deg2rad(25), + k_throttle=0.4, + k_brake=0.4, + speed_deadband=0.3 + ) + + vis = ControlVisualizer(max_steer_rad=controller.max_steer) + + x, y, yaw, v = 0.0, 0.0, 0.0, 0.0 + v_ref = 4.0 + + for _ in range(2000): + # 转到 relative coordinate（你的 controller 需要这个） + local_path = to_relative(track, x, y, yaw) + + delta, throttle, brake = controller.compute_control( + x=0, y=0, yaw=0, v=v, + path_xy=local_path, + v_ref=v_ref + ) + + vis.update(delta, throttle, brake) + + # 更新 absolute vehicle state + x, y, yaw, v = simple_dynamics(x, y, yaw, v, delta, throttle, brake) + + time.sleep(0.05) + + +if __name__ == "__main__": + main() diff --git a/CtrlPlanning/run_simulation.py b/CtrlPlanning/run_simulation.py new file mode 100644 index 0000000..c1ffef7 --- /dev/null +++ b/CtrlPlanning/run_simulation.py @@ -0,0 +1,336 @@ +import numpy as np +import matplotlib.pyplot as plt +import time +from controller import StanleyController +from visualize_controller import ControlVisualizer +from matplotlib.patches import Rectangle + + +def generate_short_path(): + # short path: straight then gentle right curve then straight + A = np.array([[i, 0.0] for i in range(15)], dtype=float) + angles = np.linspace(0.0, -np.deg2rad(30.0), 30) # right turn + R = 10.0 + B = np.stack([15.0 + R * np.sin(angles), R * (1 - np.cos(angles))], axis=1) + C = np.array([[15.0 + R * np.sin(angles[-1]) + i, R * (1 - np.cos(angles[-1]))] for i in range(15)], dtype=float) + return np.vstack([A, B, C]) + + + +def generate_random_path(num_points: int = 2000, + max_turn_deg: float = 120.0, + step_mean: float = 1.0, + step_std: float = 0.2, + seed: int | None = None) -> np.ndarray: + """ + Generate a long random path where the heading change between consecutive + steps does not exceed `max_turn_deg` degrees (absolute). + + Returns an array of shape (N,2) with absolute coordinates. + """ + if seed is not None: + rng = np.random.default_rng(seed) + else: + rng = np.random.default_rng() + + max_turn = np.deg2rad(max_turn_deg) + # start at origin heading along +x + headings = np.zeros(num_points) + # draw per-step heading changes limited to [-max_turn, max_turn] + # use smaller typical turns by default (clamp at max_turn) + delta_heading = rng.normal(loc=0.0, scale=np.deg2rad(10.0), size=num_points) + delta_heading = np.clip(delta_heading, -max_turn, max_turn) + headings = np.cumsum(delta_heading) + + # step lengths + steps = np.clip(rng.normal(loc=step_mean, scale=step_std, size=num_points), 0.1, 3.0) + + xs = np.cumsum(steps * np.cos(headings)) + ys = np.cumsum(steps * np.sin(headings)) + + return np.stack([xs, ys], axis=1) + + +def extend_path(path: np.ndarray, + n_add: int = 200, + max_turn_deg: float = 120.0, + step_mean: float = 1.0, + step_std: float = 0.2, + rng: np.random.Generator | None = None) -> np.ndarray: + """Append `n_add` points to `path` continuing from its last heading. + + Returns the extended path (new array). + """ + if rng is None: + rng = np.random.default_rng() + + if path.shape[0] < 2: + last_heading = 0.0 + last_x, last_y = 0.0, 0.0 + else: + last_x, last_y = path[-1] + prev_x, prev_y = path[-2] + last_heading = float(np.arctan2(last_y - prev_y, last_x - prev_x)) + + max_turn = np.deg2rad(max_turn_deg) + delta_heading = rng.normal(loc=0.0, scale=np.deg2rad(8.0), size=n_add) + delta_heading = np.clip(delta_heading, -max_turn, max_turn) + headings = last_heading + np.cumsum(delta_heading) + steps = np.clip(rng.normal(loc=step_mean, scale=step_std, size=n_add), 0.1, 3.0) + + xs = last_x + np.cumsum(steps * np.cos(headings)) + ys = last_y + np.cumsum(steps * np.sin(headings)) + + new_pts = np.stack([xs, ys], axis=1) + return np.vstack([path, new_pts]) + + +def to_relative(path_xy, x, y, yaw): + dx = path_xy[:, 0] - x + dy = path_xy[:, 1] - y + cos_y = np.cos(-yaw) + sin_y = np.sin(-yaw) + x_local = dx * cos_y - dy * sin_y + y_local = dx * sin_y + dy * cos_y + return np.stack([x_local, y_local], axis=1) + + +def simple_dynamics(x, y, yaw, v, delta, throttle, brake, dt=0.05): + # very simple bicycle-like update for demonstration + a = throttle * 2.0 - brake * 3.0 + v = max(0.0, v + a * dt) + x += v * np.cos(yaw) * dt + y += v * np.sin(yaw) * dt + # use a short wheelbase approximation + L = 2.5 + yaw += v / L * np.tan(delta) * dt + return x, y, yaw, v + + +def run(short=True, hud_mode: str = 'inset', autoscale: bool = True, num_steps: int | None = None): + # choose path: generate a random path (short or long) + if short: + path = generate_random_path(num_points=300, max_turn_deg=120.0, seed=42) + else: + path = generate_random_path(num_points=3000, max_turn_deg=120.0, seed=42) + + controller = StanleyController( + k_e=1.0, + k_yaw=1.2, + k_v=1.0, + max_steer=np.deg2rad(25.0), + k_throttle=0.4, + k_brake=0.4, + speed_deadband=0.2, + ) + + # initial vehicle state (absolute coordinates) + x, y, yaw, v = 0.0, 0.0, 0.0, 0.0 + v_ref = 4.0 + + # prepare plots: 2D path + HUD + plt.ion() + fig, ax = plt.subplots(figsize=(7, 5)) + path_line, = ax.plot(path[:, 0], path[:, 1], '-k', linewidth=1.0, label='path') + traj_line, = ax.plot([], [], '-r', linewidth=1.5, label='trajectory') + veh_point, = ax.plot([], [], 'bo', label='vehicle') + heading_arrow = None + # marker for the waypoint Stanley used for this step + waypoint_marker, = ax.plot([], [], marker='o', ms=10, mec='lime', mfc='none', linestyle='') + + ax.set_aspect('equal', adjustable='box') + ax.set_xlabel('X (m)') + ax.set_ylabel('Y (m)') + ax.set_title('Stanley Controller - 2D Simulation') + ax.legend() + ax.grid(True) + + # step counter text (top-left, axes fraction) + step_text = ax.text(0.01, 0.98, 'Step: 0', transform=ax.transAxes, va='top') + + # HUD: either separate window (ControlVisualizer) or inset axes inside main figure + hud = None + steer_rect = None + throttle_rect = None + brake_rect = None + hud_ax = None + step_text_hud = None + + # if the user clicks the figure (e.g., to zoom/pan), suspend autoscaling + user_zoomed = False + def _on_click(event): + nonlocal user_zoomed + user_zoomed = True + fig.canvas.mpl_connect('button_press_event', _on_click) + + if hud_mode == 'separate': + # independent figure (may open separate window) + hud = ControlVisualizer(max_steer_rad=controller.max_steer) + else: + # inset HUD inside main figure (top-right corner) + # coordinates: [left, bottom, width, height] in figure fraction + hud_ax = fig.add_axes([0.68, 0.68, 0.28, 0.28]) + hud_ax.set_title('HUD') + hud_ax.axis('off') + # use normalized axes coordinates for patches (0..1) to make updates reliable + hud_ax.set_xlim(0.0, 1.0) + hud_ax.set_ylim(0.0, 1.0) + # steering horizontal bar at top area (non-overlapping) + steer_rect = Rectangle((0.1, 0.75), 0.0, 0.15, color='cyan', transform=hud_ax.transAxes) + hud_ax.add_patch(steer_rect) + + # throttle (left) and brake (right) vertical bars in middle area + throttle_rect = Rectangle((0.08, 0.1), 0.2, 0.0, color='green', transform=hud_ax.transAxes) + brake_rect = Rectangle((0.72, 0.1), 0.2, 0.0, color='red', transform=hud_ax.transAxes) + hud_ax.add_patch(throttle_rect) + hud_ax.add_patch(brake_rect) + # step counter inside HUD + step_text_hud = hud_ax.text(0.02, 0.92, 'Step: 0', transform=hud_ax.transAxes, va='top') + + traj_x = [] + traj_y = [] + + step = 0 + # main loop: run for `num_steps` iterations, or forever if num_steps is None + while (num_steps is None) or (step < num_steps): + # controller expects path in vehicle coordinates (vehicle at origin, yaw=0) + local_path = to_relative(path, x, y, yaw) + + delta, throttle, brake = controller.compute_control( + x=0.0, y=0.0, yaw=0.0, v=v, + path_xy=local_path, + v_ref=v_ref + ) + + # update HUD bars so user can see steering/throttle/brake + if hud_mode == 'separate' and hud is not None: + try: + hud.update(delta, throttle, brake) + except Exception: + pass + elif hud_mode == 'inset' and hud_ax is not None: + # invert steering sign so bar matches visualization direction + steer_norm = np.clip(-delta / (controller.max_steer + 1e-6), -1.0, 1.0) + + # steer bar: center region, draw to the right (positive) or left (negative) + center_x = 0.5 + half_span = 0.4 + if steer_norm >= 0: + # set xy and width in axes fraction + steer_rect.set_xy((center_x, 0.75)) + steer_rect.set_width(half_span * steer_norm) + steer_rect.set_height(0.15) + else: + steer_rect.set_xy((center_x + half_span * steer_norm, 0.75)) + steer_rect.set_width(-half_span * steer_norm) + steer_rect.set_height(0.15) + + # throttle_brake: throttle grows upward from base y=0.1, brake grows upward from base y=0.1 on right side + long_norm = np.clip(throttle - brake, -1.0, 1.0) + base_y = 0.1 + max_h = 0.6 + # throttle at left patch + if long_norm >= 0: + throttle_rect.set_xy((0.08, base_y)) + throttle_rect.set_width(0.2) + throttle_rect.set_height(max_h * long_norm) + # clear brake + brake_rect.set_xy((0.72, base_y)) + brake_rect.set_width(0.2) + brake_rect.set_height(0.0) + else: + throttle_rect.set_xy((0.08, base_y)) + throttle_rect.set_width(0.2) + throttle_rect.set_height(0.0) + # brake shows proportionally on the right patch + h = max_h * (-long_norm) + brake_rect.set_xy((0.72, base_y)) + brake_rect.set_width(0.2) + brake_rect.set_height(h) + + x, y, yaw, v = simple_dynamics(x, y, yaw, v, delta, throttle, brake, dt=0.05) + + traj_x.append(x) + traj_y.append(y) + + # extend path dynamically when vehicle approaches the end + nearest_idx_abs = int(np.argmin((path[:, 0] - x)**2 + (path[:, 1] - y)**2)) + if nearest_idx_abs >= len(path) - 40: + # append more points + path = extend_path(path, n_add=800, max_turn_deg=120.0, step_mean=1.0, step_std=0.2) + # update the drawn path line + path_line.set_data(path[:, 0], path[:, 1]) + # determine which waypoint the Stanley controller actually used for nearest_point + # The controller uses nearest_idx, except when nearest_idx == last index, it uses previous point + if nearest_idx_abs < len(path) - 1: + idx_to_highlight = nearest_idx_abs + else: + idx_to_highlight = max(0, nearest_idx_abs - 1) + try: + wp_x, wp_y = float(path[idx_to_highlight, 0]), float(path[idx_to_highlight, 1]) + waypoint_marker.set_data([wp_x], [wp_y]) + except Exception: + waypoint_marker.set_data([], []) + + # update plots every 2 steps to keep interactive responsive + if step % 2 == 0: + traj_line.set_data(traj_x, traj_y) + veh_point.set_data([x], [y]) + # update step counter text + try: + step_text.set_text(f"Step: {step}") + except Exception: + pass + if step_text_hud is not None: + try: + step_text_hud.set_text(f"Step: {step}") + except Exception: + pass + + # draw heading arrow (remove previous) + if heading_arrow is not None: + try: + heading_arrow.remove() + except Exception: + pass + dx = 0.8 * np.cos(yaw) + dy = 0.8 * np.sin(yaw) + heading_arrow = ax.arrow(x, y, dx, dy, head_width=0.2, head_length=0.2, fc='b', ec='b') + + # autoscale view to include trajectory and path (unless user zoomed or autoscale disabled) + if autoscale and not user_zoomed: + all_x = np.concatenate([path[:, 0], np.array(traj_x)]) + all_y = np.concatenate([path[:, 1], np.array(traj_y)]) + xmin, xmax = np.min(all_x) - 2.0, np.max(all_x) + 2.0 + ymin, ymax = np.min(all_y) - 2.0, np.max(all_y) + 2.0 + ax.set_xlim(xmin, xmax) + ax.set_ylim(ymin, ymax) + + fig.canvas.draw() + fig.canvas.flush_events() + + # update HUD draw as well (in case backend needs flushing) + if hud_mode == 'separate' and hud is not None: + try: + hud.fig.canvas.draw() + hud.fig.canvas.flush_events() + except Exception: + pass + elif hud_mode == 'inset' and hud_ax is not None: + try: + hud_ax.figure.canvas.draw() + hud_ax.figure.canvas.flush_events() + except Exception: + pass + + time.sleep(0.02) + step += 1 + + # keep the final plot open + plt.ioff() + plt.show() + + +if __name__ == '__main__': + # run indefinitely by default when executed directly + run(short=True, hud_mode='inset', autoscale=True, num_steps=None) diff --git a/CtrlPlanning/visualize_controller.py b/CtrlPlanning/visualize_controller.py new file mode 100644 index 0000000..8d7aebb --- /dev/null +++ b/CtrlPlanning/visualize_controller.py @@ -0,0 +1,154 @@ +import numpy as np +from controller import StanleyController # 这里按你实际文件名改 +import matplotlib.pyplot as plt +from matplotlib.patches import Rectangle + + +class ControlVisualizer: + """ + 简单 HUD：一条转向条 + 一条油门/刹车条 + """ + def __init__(self, max_steer_rad: float): + self.max_steer = max_steer_rad + + # 初始化图像 + plt.ion() + self.fig, (self.ax_steer, self.ax_long) = plt.subplots( + 2, 1, figsize=(6, 4), gridspec_kw={'height_ratios': [1, 1]} + ) + self.fig.suptitle("Stanley Controller HUD") + + # ---- 转向条 (水平) ---- + self.ax_steer.set_title("Steering") + self.ax_steer.set_xlim(-1.0, 1.0) + self.ax_steer.set_ylim(0, 1) + self.ax_steer.axvline(0.0, color='black', linestyle='--', linewidth=1) # 中线 + self.ax_steer.set_yticks([]) + self.ax_steer.set_xticks([-1, -0.5, 0, 0.5, 1]) + self.ax_steer.set_xlabel("Left (-1) 0 (+1) Right") + + # 我们用一个 rectangle 来表示当前转向 + self.steer_rect = Rectangle( + (0, 0), 0, 1, alpha=0.6 + ) + self.ax_steer.add_patch(self.steer_rect) + + # ---- 油门/刹车条 (垂直) ---- + self.ax_long.set_title("Throttle / Brake") + self.ax_long.set_xlim(0, 1) + self.ax_long.set_ylim(-1.0, 1.0) + self.ax_long.axhline(0.0, color='black', linestyle='--', linewidth=1) # 分界线 + self.ax_long.set_xticks([]) + self.ax_long.set_yticks([-1, -0.5, 0, 0.5, 1]) + self.ax_long.set_ylabel("Brake (-1) 0 (+1) Throttle") + + # 这里用两块 rect：上面油门，下面刹车 + self.throttle_rect = Rectangle( + (0.25, 0), 0.5, 0, alpha=0.6 # 从 0 往上长 + ) + self.brake_rect = Rectangle( + (0.25, 0), 0.5, 0, alpha=0.6 # 从 0 往下长 + ) + self.ax_long.add_patch(self.throttle_rect) + self.ax_long.add_patch(self.brake_rect) + + plt.tight_layout() + + def update(self, delta_cmd: float, throttle_cmd: float, brake_cmd: float): + """ + 更新 HUD 显示 + + Parameters + ---------- + delta_cmd : 转向角 [rad], 已经被 Stanley 饱和在 [-max_steer, +max_steer] + throttle_cmd : [0, 1] + brake_cmd : [0, 1] + """ + + # ---- 1. 归一化转向：[-1, 1] ---- + steer_norm = np.clip(delta_cmd / (self.max_steer + 1e-6), -1.0, 1.0) + + # rectangle 是从左边界开始的，我们让它从 0 伸到 steer_norm + if steer_norm >= 0: + self.steer_rect.set_x(0.0) + self.steer_rect.set_width(steer_norm) + else: + self.steer_rect.set_x(steer_norm) + self.steer_rect.set_width(-steer_norm) + + # ---- 2. 归一化纵向：油门为 +, 刹车为 - ---- + # long_norm ∈ [-1, 1] + long_norm = np.clip(throttle_cmd - brake_cmd, -1.0, 1.0) + + # throttle_rect：从 0 向上 + if long_norm > 0: + self.throttle_rect.set_y(0.0) + self.throttle_rect.set_height(long_norm) + else: + self.throttle_rect.set_y(0.0) + self.throttle_rect.set_height(0.0) + + # brake_rect：从 0 向下（负方向） + if long_norm < 0: + self.brake_rect.set_y(long_norm) + self.brake_rect.set_height(-long_norm) + else: + self.brake_rect.set_y(0.0) + self.brake_rect.set_height(0.0) + + # ---- 3. 刷新图像 ---- + self.fig.canvas.draw() + self.fig.canvas.flush_events() + + +def main_demo(): + """ + 简单 demo：随机给 controller 喂点数据 + HUD 更新 + 实际用的时候，你把这里换成你真实仿真/跑车循环就行 + """ + import time + + # 初始化你的 StanleyController（参数自己填） + controller = StanleyController( + k_e=1.0, + k_yaw=1.0, + k_v=1.0, + max_steer=np.deg2rad(25.0), # 例子：最大 25° + k_throttle=0.3, + k_brake=0.5, + speed_deadband=0.2, + ) + + vis = ControlVisualizer(max_steer_rad=controller.max_steer) + + # 假装有一个循环（例如仿真每 0.05 s） + x, y, yaw, v = 0.0, 0.0, 0.0, 0.0 + t = 0.0 + path_xy = np.array([[i, 0.0] for i in range(100)], dtype=float) # 一条直线 + + while True: + v_ref = 5.0 # m/s，目标速度 + # 计算控制量 + delta_cmd, throttle_cmd, brake_cmd = controller.compute_control( + x=x, + y=y, + yaw=yaw, + v=v, + path_xy=path_xy, + v_ref=v_ref + ) + + # 更新 HUD + vis.update(delta_cmd, throttle_cmd, brake_cmd) + + # 这里随便更新一下车辆状态（你自己会有更真实的动力学） + v += (throttle_cmd - brake_cmd) * 0.1 + x += v * np.cos(yaw) * 0.05 + y += v * np.sin(yaw) * 0.05 + + t += 0.05 + time.sleep(0.05) + + +if __name__ == "__main__": + main_demo() From 3ff44500c37fe50f7830781c6f8eb197f0cb62d8 Mon Sep 17 00:00:00 2001 From: "Ray Zou (Clutch)" Date: Mon, 24 Nov 2025 19:24:38 -0800 Subject: [PATCH 3/5] Clean up --- CtrlPlanning/demo.py | 154 ------------------------------------------- 1 file changed, 154 deletions(-) delete mode 100644 CtrlPlanning/demo.py diff --git a/CtrlPlanning/demo.py b/CtrlPlanning/demo.py deleted file mode 100644 index 5b8f204..0000000 --- a/CtrlPlanning/demo.py +++ /dev/null @@ -1,154 +0,0 @@ -import numpy as np -import time -import matplotlib.pyplot as plt -from matplotlib.patches import Rectangle -from controller import StanleyController # 你的文件 - -# ============================================================ -# 1. HUD 可视化 -# ============================================================ -class ControlVisualizer: - def __init__(self, max_steer_rad): - self.max_steer = max_steer_rad - - plt.ion() - self.fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(7, 4)) - self.ax_steer = ax1 - self.ax_long = ax2 - - # ---- Steering bar ---- - self.ax_steer.set_title("Steering") - self.ax_steer.set_xlim(-1, 1) - self.ax_steer.set_ylim(0, 1) - self.ax_steer.axvline(0, color='black') - self.ax_steer.set_yticks([]) - self.steer_rect = Rectangle((0, 0), 0, 1, color='cyan') - self.ax_steer.add_patch(self.steer_rect) - - # ---- Longitudinal ---- - self.ax_long.set_title("Throttle / Brake") - self.ax_long.set_xlim(0, 1) - self.ax_long.set_ylim(-1, 1) - self.ax_long.axhline(0, color='black') - self.ax_long.set_xticks([]) - - self.throttle_rect = Rectangle((0.25, 0), 0.5, 0, color='green') - self.brake_rect = Rectangle((0.25, 0), 0.5, 0, color='red') - self.ax_long.add_patch(self.throttle_rect) - self.ax_long.add_patch(self.brake_rect) - - def update(self, delta, throttle, brake): - steer_norm = np.clip(delta / self.max_steer, -1, 1) - - # steering - if steer_norm >= 0: - self.steer_rect.set_x(0) - self.steer_rect.set_width(steer_norm) - else: - self.steer_rect.set_x(steer_norm) - self.steer_rect.set_width(-steer_norm) - - # throttle / brake - long_norm = throttle - brake - self.throttle_rect.set_height(max(long_norm, 0)) - self.brake_rect.set_y(min(long_norm, 0)) - self.brake_rect.set_height(abs(min(long_norm, 0))) - - self.fig.canvas.draw() - self.fig.canvas.flush_events() - -# ============================================================ -# 2. 构造 C 型赛道：直线 → 左弯 → 直线 -# ============================================================ -def generate_track_C(): - """ - Absolute coordinate path. - 然后会在每一帧转成 relative coordinate。 - """ - - # segment A: straight - A = np.array([[i, 0] for i in range(20)]) - - # segment B: left turn, radius 15m, 60 degrees arc - R = 15 - angles = np.linspace(0, np.deg2rad(60), 40) - B = np.stack([ - 20 + R * np.sin(angles), - 0 + R * (1 - np.cos(angles)) - ], axis=1) - - # segment C: straight after turn - C = np.array([[20 + R*np.sin(np.deg2rad(60)) + i, - R*(1 - np.cos(np.deg2rad(60)))] for i in range(20)]) - - return np.vstack([A, B, C]) - -# ============================================================ -# 3. 把 absolute waypoints 转到车坐标 -# ============================================================ -def to_relative(path_xy, x, y, yaw): - """ - 车辆永远是 (0,0, yaw=0)，waypoints 转到 local coordinate. - """ - dx = path_xy[:, 0] - x - dy = path_xy[:, 1] - y - cos_y = np.cos(-yaw) - sin_y = np.sin(-yaw) - - # rotation - x_local = dx * cos_y - dy * sin_y - y_local = dx * sin_y + dy * cos_y - return np.stack([x_local, y_local], axis=1) - -# ============================================================ -# 4. 简单动力学（非常简化） -# ============================================================ -def simple_dynamics(x, y, yaw, v, delta, throttle, brake, dt=0.05): - a = throttle*2.0 - brake*3.0 - v = max(0, v + a*dt) - x += v * np.cos(yaw) * dt - y += v * np.sin(yaw) * dt - yaw += v/2.5 * np.tan(delta) * dt - return x, y, yaw, v - -# ============================================================ -# 5. 主模拟程序 -# ============================================================ -def main(): - track = generate_track_C() - - controller = StanleyController( - k_e=1.0, - k_yaw=1.2, - k_v=1.0, - max_steer=np.deg2rad(25), - k_throttle=0.4, - k_brake=0.4, - speed_deadband=0.3 - ) - - vis = ControlVisualizer(max_steer_rad=controller.max_steer) - - x, y, yaw, v = 0.0, 0.0, 0.0, 0.0 - v_ref = 4.0 - - for _ in range(2000): - # 转到 relative coordinate（你的 controller 需要这个） - local_path = to_relative(track, x, y, yaw) - - delta, throttle, brake = controller.compute_control( - x=0, y=0, yaw=0, v=v, - path_xy=local_path, - v_ref=v_ref - ) - - vis.update(delta, throttle, brake) - - # 更新 absolute vehicle state - x, y, yaw, v = simple_dynamics(x, y, yaw, v, delta, throttle, brake) - - time.sleep(0.05) - - -if __name__ == "__main__": - main() From daf36aa3babfd80156b38136afec4eafeeb7ec75 Mon Sep 17 00:00:00 2001 From: "Ray Zou (Clutch)" Date: Mon, 24 Nov 2025 19:25:57 -0800 Subject: [PATCH 4/5] Enhance run function with seed and debug options for deterministic paths --- CtrlPlanning/run_simulation.py | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/CtrlPlanning/run_simulation.py b/CtrlPlanning/run_simulation.py index c1ffef7..6c0575b 100644 --- a/CtrlPlanning/run_simulation.py +++ b/CtrlPlanning/run_simulation.py @@ -107,12 +107,14 @@ def simple_dynamics(x, y, yaw, v, delta, throttle, brake, dt=0.05): return x, y, yaw, v -def run(short=True, hud_mode: str = 'inset', autoscale: bool = True, num_steps: int | None = None): +def run(short=True, hud_mode: str = 'inset', autoscale: bool = True, num_steps: int | None = None, + seed: int | None = 42, debug: bool = False): # choose path: generate a random path (short or long) + # pass through `seed` so runs can be deterministic when desired if short: - path = generate_random_path(num_points=300, max_turn_deg=120.0, seed=42) + path = generate_random_path(num_points=300, max_turn_deg=120.0, seed=seed) else: - path = generate_random_path(num_points=3000, max_turn_deg=120.0, seed=42) + path = generate_random_path(num_points=3000, max_turn_deg=120.0, seed=seed) controller = StanleyController( k_e=1.0, @@ -202,6 +204,9 @@ def _on_click(event): v_ref=v_ref ) + if debug: + print(f"step={step} delta={delta:.4f} throttle={throttle:.3f} brake={brake:.3f} v={v:.3f}") + # update HUD bars so user can see steering/throttle/brake if hud_mode == 'separate' and hud is not None: try: @@ -333,4 +338,4 @@ def _on_click(event): if __name__ == '__main__': # run indefinitely by default when executed directly - run(short=True, hud_mode='inset', autoscale=True, num_steps=None) + run(short=True, hud_mode='inset', autoscale=True, num_steps=None, seed=43, debug=True) From b9d935880beff0e78c59a77f353b8e50f9ba327c Mon Sep 17 00:00:00 2001 From: "Ray Zou (Clutch)" Date: Mon, 1 Dec 2025 20:53:59 -0800 Subject: [PATCH 5/5] Add race track path gen --- CtrlPlanning/run_simulation2.py | 492 ++++++++++++++++++++++++++++++++ 1 file changed, 492 insertions(+) create mode 100644 CtrlPlanning/run_simulation2.py diff --git a/CtrlPlanning/run_simulation2.py b/CtrlPlanning/run_simulation2.py new file mode 100644 index 0000000..89b2ae1 --- /dev/null +++ b/CtrlPlanning/run_simulation2.py @@ -0,0 +1,492 @@ +import numpy as np +import matplotlib.pyplot as plt +import time +from controller import StanleyController +from visualize_controller import ControlVisualizer +from matplotlib.patches import Rectangle + + +def generate_short_path(): + # short path: straight then gentle right curve then straight + A = np.array([[i, 0.0] for i in range(15)], dtype=float) + angles = np.linspace(0.0, -np.deg2rad(30.0), 30) # right turn + R = 10.0 + B = np.stack([15.0 + R * np.sin(angles), R * (1 - np.cos(angles))], axis=1) + C = np.array([[15.0 + R * np.sin(angles[-1]) + i, R * (1 - np.cos(angles[-1]))] for i in range(15)], dtype=float) + return np.vstack([A, B, C]) + + + +def generate_random_path(num_points: int = 2000, + max_turn_deg: float = 120.0, + turn_std_deg: float = 10.0, + step_mean: float = 1.0, + step_std: float = 0.2, + seed: int | None = None) -> np.ndarray: + """ + Generate a long random path where the heading change between consecutive + steps does not exceed `max_turn_deg` degrees (absolute). + + Returns an array of shape (N,2) with absolute coordinates. + """ + if seed is not None: + rng = np.random.default_rng(seed) + else: + rng = np.random.default_rng() + + max_turn = np.deg2rad(max_turn_deg) + # start at origin heading along +x + headings = np.zeros(num_points) + # draw per-step heading changes limited to [-max_turn, max_turn] + # use smaller typical turns by default (clamp at max_turn) + delta_heading = rng.normal(loc=0.0, scale=np.deg2rad(turn_std_deg), size=num_points) + delta_heading = np.clip(delta_heading, -max_turn, max_turn) + headings = np.cumsum(delta_heading) + + # step lengths + steps = np.clip(rng.normal(loc=step_mean, scale=step_std, size=num_points), 0.1, 3.0) + + xs = np.cumsum(steps * np.cos(headings)) + ys = np.cumsum(steps * np.sin(headings)) + + return np.stack([xs, ys], axis=1) + + +def generate_racetrack_path(num_steps: int = 500, + step_mean: float = 1.0, + n_segments: int | None = None, + straight_len_mu: float = 20.0, + straight_len_std: float = 5.0, + arc_angle_mu_deg: float = 60.0, + arc_angle_std_deg: float = 15.0, + arc_radius_mu: float = 15.0, + arc_radius_std: float = 5.0, + seed: int | None = None) -> np.ndarray: + """ + Generate a racetrack-like path composed of straight segments and circular arcs. + + - `num_steps` approximates how many points the path should contain (used to size segments). + - The generator alternates straights and arcs to create distinct turns that resemble a track. + """ + if seed is not None: + rng = np.random.default_rng(seed) + else: + rng = np.random.default_rng() + + if n_segments is None: + n_segments = max(4, int(num_steps / 80)) + + pts = [] + x, y, heading = 0.0, 0.0, 0.0 + + for seg in range(n_segments): + # straight segment + straight_len = max(5.0, rng.normal(loc=straight_len_mu, scale=straight_len_std)) + n_st = max(1, int(straight_len / step_mean)) + xs = x + np.cumsum(np.full(n_st, step_mean) * np.cos(heading)) + ys = y + np.cumsum(np.full(n_st, step_mean) * np.sin(heading)) + seg_pts = np.stack([xs, ys], axis=1) + pts.append(seg_pts) + x, y = seg_pts[-1] + + # arc (turn) + angle_deg = max(15.0, rng.normal(loc=arc_angle_mu_deg, scale=arc_angle_std_deg)) + # alternate left/right turns to look like a track (with some randomness) + turn_dir = -1 if (seg % 2 == 0) else 1 + if rng.random() < 0.15: + turn_dir *= -1 + angle = np.deg2rad(angle_deg) * turn_dir + radius = max(4.0, rng.normal(loc=arc_radius_mu, scale=arc_radius_std)) + + arc_length = abs(angle) * radius + n_arc = max(2, int(arc_length / step_mean)) + # center of the arc + # center is located at (x, y) plus a vector perpendicular to heading + cx = x - radius * np.sin(heading) * turn_dir + cy = y + radius * np.cos(heading) * turn_dir + + # compute arc points + start_angle = np.arctan2(y - cy, x - cx) + arc_thetas = np.linspace(start_angle, start_angle + angle, n_arc + 1)[1:] + ax_pts = cx + radius * np.cos(arc_thetas) + ay_pts = cy + radius * np.sin(arc_thetas) + arc_points = np.stack([ax_pts, ay_pts], axis=1) + pts.append(arc_points) + + # update state to end of arc + x, y = arc_points[-1] + heading = (heading + angle) % (2 * np.pi) + + all_pts = np.vstack(pts) + # resample so spacing is approximately step_mean + dxy = np.diff(all_pts, axis=0) + dists = np.hypot(dxy[:, 0], dxy[:, 1]) + cum = np.concatenate([[0.0], np.cumsum(dists)]) + total_len = cum[-1] + n_target = max(2, int(total_len / step_mean)) + new_s = np.linspace(0.0, total_len, n_target) + new_x = np.interp(new_s, cum, all_pts[:, 0]) + new_y = np.interp(new_s, cum, all_pts[:, 1]) + return np.stack([new_x, new_y], axis=1) + + +def extend_path(path: np.ndarray, + n_add: int = 200, + max_turn_deg: float = 120.0, + turn_std_deg: float = 8.0, + step_mean: float = 1.0, + step_std: float = 0.2, + rng: np.random.Generator | None = None) -> np.ndarray: + """Append `n_add` points to `path` continuing from its last heading. + + Returns the extended path (new array). + """ + if rng is None: + rng = np.random.default_rng() + + if path.shape[0] < 2: + last_heading = 0.0 + last_x, last_y = 0.0, 0.0 + else: + last_x, last_y = path[-1] + prev_x, prev_y = path[-2] + last_heading = float(np.arctan2(last_y - prev_y, last_x - prev_x)) + + max_turn = np.deg2rad(max_turn_deg) + delta_heading = rng.normal(loc=0.0, scale=np.deg2rad(turn_std_deg), size=n_add) + delta_heading = np.clip(delta_heading, -max_turn, max_turn) + headings = last_heading + np.cumsum(delta_heading) + steps = np.clip(rng.normal(loc=step_mean, scale=step_std, size=n_add), 0.1, 3.0) + + xs = last_x + np.cumsum(steps * np.cos(headings)) + ys = last_y + np.cumsum(steps * np.sin(headings)) + + new_pts = np.stack([xs, ys], axis=1) + return np.vstack([path, new_pts]) + + +def to_relative(path_xy, x, y, yaw): + dx = path_xy[:, 0] - x + dy = path_xy[:, 1] - y + cos_y = np.cos(-yaw) + sin_y = np.sin(-yaw) + x_local = dx * cos_y - dy * sin_y + y_local = dx * sin_y + dy * cos_y + return np.stack([x_local, y_local], axis=1) + + +def simple_dynamics(x, y, yaw, v, delta, throttle, brake, dt=0.05): + # very simple bicycle-like update for demonstration + a = throttle * 2.0 - brake * 3.0 + v = max(0.0, v + a * dt) + x += v * np.cos(yaw) * dt + y += v * np.sin(yaw) * dt + # use a short wheelbase approximation + L = 2.5 + yaw += v / L * np.tan(delta) * dt + return x, y, yaw, v + + +def run(short=True, + hud_mode: str = 'inset', + autoscale: bool = True, + num_steps: int | None = None, + seed: int | None = 42, + debug: bool = False, + path_mode: str = 'random', + start_state: tuple[float, float, float, float] | None = None, + path_turn_std_deg: float = 10.0): + """ + Run the 2D Stanley simulation. + + Parameters added: + - path_mode: 'random' (default) or 'straight'. If 'straight', a straight-line + path starting at the global origin along +X is used. + - start_state: optional tuple (x, y, yaw, v) to set the vehicle initial state. + If omitted, defaults to (0,0,0,0). + """ + + # choose path: random or straight + if path_mode == 'straight': + # straight line starting at global origin along +X + if num_steps is not None: + # create slightly longer path than steps with a small safety buffer + length = max(int(num_steps * 1.1) + 50, 50) + else: + length = 400 if not short else 150 + path = np.array([[i, 0.0] for i in range(length)], dtype=float) + elif path_mode == 'race': + # racetrack-like path composed of straights + circular arcs + if num_steps is None: + num_steps = 500 + path = generate_racetrack_path(num_steps=num_steps, step_mean=1.0, seed=seed) + else: + # pass through `seed` so runs can be deterministic when desired + if num_steps is not None: + # generate one waypoint per step plus buffer (handles variable step sizes) + num_points = max(int(num_steps * 1.2) + 100, 100) + else: + num_points = 300 if short else 3000 + + path = generate_random_path(num_points=num_points, + max_turn_deg=120.0, + turn_std_deg=path_turn_std_deg, + seed=seed) + + controller = StanleyController( + k_e=0.5, + k_yaw=0.5, + k_v=1.0, + max_steer=np.deg2rad(45.0), + k_throttle=0.4, + k_brake=0.4, + speed_deadband=0.2, + ) + + # initial vehicle state (absolute coordinates) + if start_state is None: + x, y, yaw, v = 0.0, 0.0, 0.0, 0.0 + else: + x, y, yaw, v = start_state + v_ref = 4.0 + + # prepare plots: 2D path + HUD + plt.ion() + fig, ax = plt.subplots(figsize=(7, 5)) + path_line, = ax.plot(path[:, 0], path[:, 1], '-k', linewidth=1.0, label='path') + traj_line, = ax.plot([], [], '-r', linewidth=1.5, label='trajectory') + veh_point, = ax.plot([], [], 'bo', label='vehicle') + heading_arrow = None + # marker for the waypoint Stanley used for this step + waypoint_marker, = ax.plot([], [], marker='o', ms=10, mec='lime', mfc='none', linestyle='') + + ax.set_aspect('equal', adjustable='box') + ax.set_xlabel('X (m)') + ax.set_ylabel('Y (m)') + ax.set_title('Stanley Controller - 2D Simulation') + ax.legend() + ax.grid(True) + + # step counter text (top-left, axes fraction) + step_text = ax.text(0.01, 0.98, 'Step: 0', transform=ax.transAxes, va='top') + + # HUD: either separate window (ControlVisualizer) or inset axes inside main figure + hud = None + steer_rect = None + throttle_rect = None + brake_rect = None + hud_ax = None + step_text_hud = None + + # if the user clicks the figure (e.g., to zoom/pan), suspend autoscaling + user_zoomed = False + def _on_click(event): + nonlocal user_zoomed + user_zoomed = True + fig.canvas.mpl_connect('button_press_event', _on_click) + + if hud_mode == 'separate': + # independent figure (may open separate window) + hud = ControlVisualizer(max_steer_rad=controller.max_steer) + else: + # inset HUD inside main figure (top-right corner) + # coordinates: [left, bottom, width, height] in figure fraction + hud_ax = fig.add_axes([0.68, 0.68, 0.28, 0.28]) + hud_ax.set_title('HUD') + hud_ax.axis('off') + # use normalized axes coordinates for patches (0..1) to make updates reliable + hud_ax.set_xlim(0.0, 1.0) + hud_ax.set_ylim(0.0, 1.0) + # steering horizontal bar at top area (non-overlapping) + steer_rect = Rectangle((0.1, 0.75), 0.0, 0.15, color='cyan', transform=hud_ax.transAxes) + hud_ax.add_patch(steer_rect) + + # throttle (left) and brake (right) vertical bars in middle area + throttle_rect = Rectangle((0.08, 0.1), 0.2, 0.0, color='green', transform=hud_ax.transAxes) + brake_rect = Rectangle((0.72, 0.1), 0.2, 0.0, color='red', transform=hud_ax.transAxes) + hud_ax.add_patch(throttle_rect) + hud_ax.add_patch(brake_rect) + # step counter inside HUD + step_text_hud = hud_ax.text(0.02, 0.92, 'Step: 0', transform=hud_ax.transAxes, va='top') + + traj_x = [] + traj_y = [] + + # vehicle state for visualization: current steering angle (rad) + steer_angle = 0.0 + # simulation timestep (keep in sync with simple_dynamics dt) + sim_dt = 0.05 + + # markers to show the two points used to compute path heading + heading_pt1_marker, = ax.plot([], [], marker='x', ms=8, mec='orange', mfc='none', linestyle='') + heading_pt2_marker, = ax.plot([], [], marker='x', ms=8, mec='magenta', mfc='none', linestyle='') + + step = 0 + # main loop: run for `num_steps` iterations, or forever if num_steps is None + while (num_steps is None) or (step < num_steps): + # controller expects path in vehicle coordinates (vehicle at origin, yaw=0) + local_path = to_relative(path, x, y, yaw) + + delta, throttle, brake = controller.compute_control( + x=0.0, y=0.0, yaw=0.0, v=v, + path_xy=local_path, + v_ref=v_ref + ) + + # apply commanded steering as a delta to the current steering angle + # Interpretation: controller returns a steering *change* (delta), + # so we update the vehicle's current steer_angle by adding it. + # Then saturate to controller limits so the vehicle can't exceed max_steer. + steer_angle += float(delta) + steer_angle = float(np.clip(steer_angle, -controller.max_steer, controller.max_steer)) + + if debug: + print(f"step={step} delta={delta:.4f} steer_angle={steer_angle:.4f} throttle={throttle:.3f} brake={brake:.3f} v={v:.3f}") + + # update HUD bars so user can see steering/throttle/brake + if hud_mode == 'separate' and hud is not None: + try: + # pass vehicle's current steering angle (not raw controller delta) + hud.update(steer_angle, throttle, brake) + except Exception: + pass + elif hud_mode == 'inset' and hud_ax is not None: + # invert steering sign so bar matches visualization direction + steer_norm = np.clip(-steer_angle / (controller.max_steer + 1e-6), -1.0, 1.0) + + # steer bar: center region, draw to the right (positive) or left (negative) + center_x = 0.5 + half_span = 0.4 + if steer_norm >= 0: + # set xy and width in axes fraction + steer_rect.set_xy((center_x, 0.75)) + steer_rect.set_width(half_span * steer_norm) + steer_rect.set_height(0.15) + else: + steer_rect.set_xy((center_x + half_span * steer_norm, 0.75)) + steer_rect.set_width(-half_span * steer_norm) + steer_rect.set_height(0.15) + + # throttle_brake: throttle grows upward from base y=0.1, brake grows upward from base y=0.1 on right side + long_norm = np.clip(throttle - brake, -1.0, 1.0) + base_y = 0.1 + max_h = 0.6 + # throttle at left patch + if long_norm >= 0: + throttle_rect.set_xy((0.08, base_y)) + throttle_rect.set_width(0.2) + throttle_rect.set_height(max_h * long_norm) + # clear brake + brake_rect.set_xy((0.72, base_y)) + brake_rect.set_width(0.2) + brake_rect.set_height(0.0) + else: + throttle_rect.set_xy((0.08, base_y)) + throttle_rect.set_width(0.2) + throttle_rect.set_height(0.0) + # brake shows proportionally on the right patch + h = max_h * (-long_norm) + brake_rect.set_xy((0.72, base_y)) + brake_rect.set_width(0.2) + brake_rect.set_height(h) + + # use the controller's raw steering command for the vehicle update + x, y, yaw, v = simple_dynamics(x, y, yaw, v, steer_angle, throttle, brake, dt=sim_dt) + + traj_x.append(x) + traj_y.append(y) + + # extend path dynamically when vehicle approaches the end + nearest_idx_abs = int(np.argmin((path[:, 0] - x)**2 + (path[:, 1] - y)**2)) + if nearest_idx_abs >= len(path) - 40: + # append more points + path = extend_path(path, n_add=800, max_turn_deg=120.0, turn_std_deg=path_turn_std_deg, step_mean=1.0, step_std=0.2) + # update the drawn path line + path_line.set_data(path[:, 0], path[:, 1]) + # determine which waypoint indices are used to compute the path heading + # matches logic in `StanleyController.compute_control`: heading uses + # (nearest_idx, nearest_idx+1) except at end where it uses (nearest_idx-1, nearest_idx) + if nearest_idx_abs < len(path) - 1: + i1, i2 = nearest_idx_abs, nearest_idx_abs + 1 + else: + i1, i2 = max(0, nearest_idx_abs - 1), nearest_idx_abs + + # highlight the nominal nearest waypoint (used for cross-track) + try: + wp_x, wp_y = float(path[nearest_idx_abs, 0]), float(path[nearest_idx_abs, 1]) + waypoint_marker.set_data([wp_x], [wp_y]) + except Exception: + waypoint_marker.set_data([], []) + + # highlight the two points used to compute path heading + try: + p1x, p1y = float(path[i1, 0]), float(path[i1, 1]) + p2x, p2y = float(path[i2, 0]), float(path[i2, 1]) + heading_pt1_marker.set_data([p1x], [p1y]) + heading_pt2_marker.set_data([p2x], [p2y]) + except Exception: + heading_pt1_marker.set_data([], []) + heading_pt2_marker.set_data([], []) + + # update plots every 2 steps to keep interactive responsive + if step % 2 == 0: + traj_line.set_data(traj_x, traj_y) + veh_point.set_data([x], [y]) + # update step counter text + try: + step_text.set_text(f"Step: {step}") + except Exception: + pass + if step_text_hud is not None: + try: + step_text_hud.set_text(f"Step: {step}") + except Exception: + pass + + # draw heading arrow (remove previous) + if heading_arrow is not None: + try: + heading_arrow.remove() + except Exception: + pass + dx = 0.8 * np.cos(yaw) + dy = 0.8 * np.sin(yaw) + heading_arrow = ax.arrow(x, y, dx, dy, head_width=0.2, head_length=0.2, fc='b', ec='b') + + # autoscale view to include trajectory and path (unless user zoomed or autoscale disabled) + if autoscale and not user_zoomed: + all_x = np.concatenate([path[:, 0], np.array(traj_x)]) + all_y = np.concatenate([path[:, 1], np.array(traj_y)]) + xmin, xmax = np.min(all_x) - 2.0, np.max(all_x) + 2.0 + ymin, ymax = np.min(all_y) - 2.0, np.max(all_y) + 2.0 + ax.set_xlim(xmin, xmax) + ax.set_ylim(ymin, ymax) + + fig.canvas.draw() + fig.canvas.flush_events() + + # update HUD draw as well (in case backend needs flushing) + if hud_mode == 'separate' and hud is not None: + try: + hud.fig.canvas.draw() + hud.fig.canvas.flush_events() + except Exception: + pass + elif hud_mode == 'inset' and hud_ax is not None: + try: + hud_ax.figure.canvas.draw() + hud_ax.figure.canvas.flush_events() + except Exception: + pass + + time.sleep(0.02) + step += 1 + + # keep the final plot open + plt.ioff() + plt.show() + + +if __name__ == '__main__': + # run indefinitely by default when executed directly + run(short=True, hud_mode='inset', autoscale=True, num_steps=1000, seed=43, debug=True, path_mode='race', path_turn_std_deg=15)