769 lines
29 KiB
Python
769 lines
29 KiB
Python
# target_simulator/gui/ppi_display.py
|
||
|
||
"""
|
||
A reusable Tkinter widget that displays a Plan Position Indicator (PPI)
|
||
using Matplotlib, capable of showing both live targets and trajectory previews,
|
||
and comparing simulated vs. real-time data.
|
||
"""
|
||
|
||
import tkinter as tk
|
||
from tkinter import ttk
|
||
import math
|
||
import time
|
||
import logging
|
||
import numpy as np
|
||
import collections
|
||
import warnings
|
||
from matplotlib.figure import Figure
|
||
from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg
|
||
import matplotlib as mpl
|
||
from typing import List, Dict, Optional
|
||
|
||
from target_simulator.core.models import Target, Waypoint, ManeuverType, NM_TO_FT
|
||
|
||
# Module-level logger
|
||
logger = logging.getLogger(__name__)
|
||
|
||
|
||
class PPIDisplay(ttk.Frame):
|
||
"""
|
||
A custom widget for the PPI radar display.
|
||
"""
|
||
|
||
TRAIL_LENGTH = 100
|
||
|
||
def __init__(
|
||
self,
|
||
master,
|
||
max_range_nm: int = 100,
|
||
scan_limit_deg: int = 60,
|
||
trail_length: int = None,
|
||
):
|
||
"""
|
||
Inizializza il widget PPI.
|
||
|
||
Ingressi: master (Tk parent), max_range_nm (int), scan_limit_deg (int), trail_length (int|None)
|
||
Uscite: nessuna (side-effect: costruisce widgets e canvas)
|
||
Commento: mantiene gli artist Matplotlib per targets, trails e antenna.
|
||
"""
|
||
super().__init__(master)
|
||
self.max_range = max_range_nm
|
||
self.scan_limit_deg = scan_limit_deg
|
||
self.sim_target_artists: List[mpl.artist.Artist] = []
|
||
self.real_target_artists: List[mpl.artist.Artist] = []
|
||
self.sim_trail_artists: List[mpl.artist.Artist] = []
|
||
self.real_trail_artists: List[mpl.artist.Artist] = []
|
||
self.sim_label_artists: List[mpl.artist.Artist] = []
|
||
self.real_label_artists: List[mpl.artist.Artist] = []
|
||
# Persistent markers for finished trajectories (yellow X) - not cleared on simulation end
|
||
self.finished_trajectory_markers: List[mpl.artist.Artist] = []
|
||
self.trail_length = trail_length or self.TRAIL_LENGTH
|
||
self._trails = {
|
||
"simulated": collections.defaultdict(
|
||
lambda: collections.deque(maxlen=self.trail_length)
|
||
),
|
||
"real": collections.defaultdict(
|
||
lambda: collections.deque(maxlen=self.trail_length)
|
||
),
|
||
}
|
||
self.preview_artists: List[mpl.artist.Artist] = []
|
||
self.preview_path_artists: Dict[int, List[mpl.artist.Artist]] = {}
|
||
|
||
# Display options
|
||
self.show_sim_points_var = tk.BooleanVar(value=True)
|
||
self.show_real_points_var = tk.BooleanVar(value=True)
|
||
self.show_sim_trail_var = tk.BooleanVar(value=False)
|
||
self.show_real_trail_var = tk.BooleanVar(value=False)
|
||
self.animate_antenna_var = tk.BooleanVar(value=True)
|
||
self.display_mode_var = tk.StringVar(value="North-Up")
|
||
|
||
self.canvas = None
|
||
self._ownship_artist: Optional[mpl.patches.Polygon] = None
|
||
self.ownship_heading_deg = 0.0
|
||
self._antenna_line_artist: Optional[mpl.lines.Line2D] = None
|
||
|
||
self._create_controls()
|
||
self._create_plot()
|
||
|
||
self._real_update_timestamps = collections.deque(maxlen=10000)
|
||
self._last_update_summary_time = time.monotonic()
|
||
self._update_summary_interval_s = 1.0
|
||
|
||
def _on_display_options_changed(self, *args):
|
||
"""Handler invoked when display options (points/trails) change.
|
||
|
||
Clears current target artists and triggers a redraw so the new
|
||
display options are applied immediately.
|
||
"""
|
||
self.clear_all_targets()
|
||
if self.canvas:
|
||
self.canvas.draw()
|
||
|
||
def _on_display_mode_changed(self, *args):
|
||
"""Callback when the display mode (North-Up/Heading-Up) changes."""
|
||
self._update_plot_orientation()
|
||
if self.canvas:
|
||
self.canvas.draw_idle()
|
||
|
||
def _create_controls(self):
|
||
"""Create the top control panel containing radar controls and legend.
|
||
|
||
The panel includes 4 logical sections: Radar, Display Mode, Display
|
||
Options and Legend. Widgets are created and wired to the corresponding
|
||
callbacks and Tk variables.
|
||
"""
|
||
top_frame = ttk.Frame(self)
|
||
top_frame.pack(side=tk.TOP, fill=tk.X, padx=5, pady=(5, 2))
|
||
|
||
# Section 1: Radar Controls
|
||
radar_frame = ttk.LabelFrame(top_frame, text="Radar", padding=5)
|
||
radar_frame.pack(side=tk.LEFT, padx=(0, 5), fill=tk.Y)
|
||
|
||
range_subframe = ttk.Frame(radar_frame)
|
||
range_subframe.pack(anchor="w")
|
||
ttk.Label(range_subframe, text="Range (NM):").pack(side=tk.LEFT)
|
||
all_steps = [10, 20, 40, 80, 100, 160, 240, 320]
|
||
valid_steps = sorted(
|
||
[s for s in all_steps if s <= self.max_range]
|
||
+ ([self.max_range] if self.max_range not in all_steps else [])
|
||
)
|
||
self.range_var = tk.IntVar(value=self.max_range)
|
||
self.range_selector = ttk.Combobox(
|
||
range_subframe,
|
||
textvariable=self.range_var,
|
||
values=valid_steps,
|
||
state="readonly",
|
||
width=5,
|
||
)
|
||
self.range_selector.pack(side=tk.LEFT, padx=5)
|
||
|
||
ttk.Checkbutton(
|
||
radar_frame,
|
||
text="Animate Antenna",
|
||
variable=self.animate_antenna_var,
|
||
command=self._force_redraw,
|
||
).pack(anchor="w", pady=(4, 0))
|
||
|
||
# Section 2: Display Mode
|
||
mode_frame = ttk.LabelFrame(top_frame, text="Display Mode", padding=5)
|
||
mode_frame.pack(side=tk.LEFT, padx=5, fill=tk.Y)
|
||
ttk.Radiobutton(
|
||
mode_frame,
|
||
text="North-Up",
|
||
variable=self.display_mode_var,
|
||
value="North-Up",
|
||
command=self._on_display_mode_changed,
|
||
).pack(anchor="w")
|
||
ttk.Radiobutton(
|
||
mode_frame,
|
||
text="Heading-Up",
|
||
variable=self.display_mode_var,
|
||
value="Heading-Up",
|
||
command=self._on_display_mode_changed,
|
||
).pack(anchor="w")
|
||
|
||
# Section 3: Display Options
|
||
options_frame = ttk.LabelFrame(top_frame, text="Display Options", padding=5)
|
||
options_frame.pack(side=tk.LEFT, padx=5, fill=tk.Y)
|
||
ttk.Checkbutton(
|
||
options_frame,
|
||
text="Sim Points",
|
||
variable=self.show_sim_points_var,
|
||
command=self._on_display_options_changed,
|
||
).grid(row=0, column=0, sticky="w", padx=5)
|
||
ttk.Checkbutton(
|
||
options_frame,
|
||
text="Real Points",
|
||
variable=self.show_real_points_var,
|
||
command=self._on_display_options_changed,
|
||
).grid(row=0, column=1, sticky="w", padx=5)
|
||
ttk.Checkbutton(
|
||
options_frame,
|
||
text="Sim Trail",
|
||
variable=self.show_sim_trail_var,
|
||
command=self._on_display_options_changed,
|
||
).grid(row=1, column=0, sticky="w", padx=5)
|
||
ttk.Checkbutton(
|
||
options_frame,
|
||
text="Real Trail",
|
||
variable=self.show_real_trail_var,
|
||
command=self._on_display_options_changed,
|
||
).grid(row=1, column=1, sticky="w", padx=5)
|
||
|
||
# Spacer to push the legend to the right
|
||
spacer = ttk.Frame(top_frame)
|
||
spacer.pack(side=tk.LEFT, expand=True, fill=tk.X)
|
||
|
||
# Section 4: Legend
|
||
legend_frame = ttk.LabelFrame(top_frame, text="Legend", padding=5)
|
||
legend_frame.pack(side=tk.LEFT, padx=5, fill=tk.Y)
|
||
|
||
# Ownship
|
||
own_sw = tk.Canvas(legend_frame, width=16, height=12, highlightthickness=0)
|
||
own_sw.create_rectangle(0, 0, 16, 12, fill="cyan", outline="black")
|
||
own_sw.grid(row=0, column=0, padx=(0, 4), pady=(0, 2))
|
||
ttk.Label(legend_frame, text="Ownship").grid(row=0, column=1, sticky="w")
|
||
|
||
# Simulated
|
||
sim_sw = tk.Canvas(legend_frame, width=16, height=12, highlightthickness=0)
|
||
sim_sw.create_rectangle(0, 0, 16, 12, fill="green", outline="black")
|
||
sim_sw.grid(row=1, column=0, padx=(0, 4), pady=(0, 2))
|
||
ttk.Label(legend_frame, text="Simulated").grid(row=1, column=1, sticky="w")
|
||
|
||
# Real
|
||
real_sw = tk.Canvas(legend_frame, width=16, height=12, highlightthickness=0)
|
||
real_sw.create_rectangle(0, 0, 16, 12, fill="red", outline="black")
|
||
real_sw.grid(row=2, column=0, padx=(0, 4))
|
||
ttk.Label(legend_frame, text="Real").grid(row=2, column=1, sticky="w")
|
||
|
||
def _force_redraw(self):
|
||
"""Request an immediate (idle) redraw of the Matplotlib canvas.
|
||
|
||
This is used by UI controls that change visual options to avoid
|
||
blocking synchronous draws.
|
||
"""
|
||
if self.canvas:
|
||
self.canvas.draw_idle()
|
||
|
||
def _create_plot(self):
|
||
"""Create the Matplotlib polar Figure and initial plot artists."""
|
||
fig = Figure(figsize=(5, 5), dpi=100, facecolor="#3E3E3E")
|
||
fig.subplots_adjust(left=0.05, right=0.95, top=0.9, bottom=0.05)
|
||
self.ax = fig.add_subplot(111, projection="polar", facecolor="#2E2E2E")
|
||
self.ax.set_theta_zero_location("N")
|
||
self.ax.set_theta_direction(1)
|
||
self.ax.set_rlabel_position(90)
|
||
self.ax.set_ylim(0, self.range_var.get())
|
||
|
||
angles_deg = np.arange(0, 360, 30)
|
||
labels = [f"{(a if a <= 180 else a - 360)}°" for a in angles_deg]
|
||
self.ax.set_thetagrids(angles_deg, labels)
|
||
|
||
self.ax.tick_params(axis="x", colors="white", labelsize=8)
|
||
self.ax.tick_params(axis="y", colors="white", labelsize=8)
|
||
self.ax.grid(color="white", linestyle="--", linewidth=0.5, alpha=0.5)
|
||
self.ax.spines["polar"].set_color("white")
|
||
self.ax.set_title("PPI Display", color="white")
|
||
|
||
self._ownship_artist = mpl.patches.Polygon(
|
||
[[-1, -1]], closed=True, facecolor="cyan", edgecolor="black", zorder=10
|
||
)
|
||
self.ax.add_patch(self._ownship_artist)
|
||
|
||
(self._path_plot,) = self.ax.plot([], [], "g--", linewidth=1.5, zorder=3)
|
||
(self._start_plot,) = self.ax.plot([], [], "go", markersize=8, zorder=3)
|
||
(self._waypoints_plot,) = self.ax.plot(
|
||
[], [], "y+", markersize=10, mew=2, zorder=3
|
||
)
|
||
self.preview_artists = [self._path_plot, self._start_plot, self._waypoints_plot]
|
||
|
||
(self._scan_line_1,) = self.ax.plot([], [], "y--", linewidth=1, zorder=1)
|
||
(self._scan_line_2,) = self.ax.plot([], [], "y--", linewidth=1, zorder=1)
|
||
|
||
(self._antenna_line_artist,) = self.ax.plot(
|
||
[],
|
||
[],
|
||
color="lightgray",
|
||
linestyle="--",
|
||
linewidth=1.2,
|
||
alpha=0.85,
|
||
zorder=2,
|
||
)
|
||
logger.debug(f"Antenna artist created: {self._antenna_line_artist}")
|
||
|
||
self.canvas = FigureCanvasTkAgg(fig, master=self)
|
||
|
||
# Suppress the UserWarning from tight_layout with polar axes
|
||
with warnings.catch_warnings():
|
||
warnings.simplefilter("ignore", UserWarning)
|
||
self.canvas.draw()
|
||
|
||
self.canvas.get_tk_widget().pack(side=tk.TOP, fill=tk.BOTH, expand=True)
|
||
self.range_selector.bind("<<ComboboxSelected>>", self._on_range_selected)
|
||
self._update_plot_orientation() # Initial draw
|
||
|
||
def update_ownship_state(self, heading_deg: float):
|
||
"""Updates the ownship's visual representation on the PPI."""
|
||
if self.ownship_heading_deg != heading_deg:
|
||
self.ownship_heading_deg = heading_deg
|
||
self._update_plot_orientation()
|
||
|
||
def _update_plot_orientation(self):
|
||
"""Applies rotation to the plot or ownship icon based on display mode."""
|
||
if not self.ax or not self._ownship_artist:
|
||
return
|
||
|
||
mode = self.display_mode_var.get()
|
||
# Convention: Azimuth/Heading is 0=N, positive=CCW (Left)
|
||
# Matplotlib theta is 0=East, positive=CCW.
|
||
# With zero_location="N", theta becomes 0=North, positive=CCW.
|
||
heading_rad = np.deg2rad(self.ownship_heading_deg)
|
||
max_r = self.ax.get_ylim()[1]
|
||
|
||
# Define ownship triangle shape in polar coordinates (theta, r)
|
||
r_scale = max_r * 0.04
|
||
nose = (0, r_scale)
|
||
wing_angle = np.deg2rad(140)
|
||
left_wing = (wing_angle, r_scale * 0.8)
|
||
right_wing = (-wing_angle, r_scale * 0.8)
|
||
|
||
base_verts_polar = np.array([nose, left_wing, right_wing])
|
||
|
||
if mode == "Heading-Up":
|
||
# Rotate the entire grid by the heading angle
|
||
self.ax.set_theta_offset(np.pi / 2 - heading_rad)
|
||
|
||
# To make ownship and scan lines appear fixed, we must "counter-rotate"
|
||
# them by drawing them at an angle equal to the heading.
|
||
verts_polar = base_verts_polar.copy()
|
||
verts_polar[:, 0] += heading_rad
|
||
self._ownship_artist.set_xy(verts_polar)
|
||
|
||
limit_rad = np.deg2rad(self.scan_limit_deg)
|
||
self._scan_line_1.set_data(
|
||
[heading_rad + limit_rad, heading_rad + limit_rad], [0, max_r]
|
||
)
|
||
self._scan_line_2.set_data(
|
||
[heading_rad - limit_rad, heading_rad - limit_rad], [0, max_r]
|
||
)
|
||
|
||
else: # North-Up
|
||
# Keep grid fixed with North up
|
||
self.ax.set_theta_offset(np.pi / 2)
|
||
# Rotate ownship vertices by adding heading to theta
|
||
verts_polar = base_verts_polar.copy()
|
||
verts_polar[:, 0] += heading_rad
|
||
self._ownship_artist.set_xy(verts_polar)
|
||
# Rotate scan lines by adding heading to theta
|
||
limit_rad = np.deg2rad(self.scan_limit_deg)
|
||
self._scan_line_1.set_data(
|
||
[heading_rad + limit_rad, heading_rad + limit_rad], [0, max_r]
|
||
)
|
||
self._scan_line_2.set_data(
|
||
[heading_rad - limit_rad, heading_rad - limit_rad], [0, max_r]
|
||
)
|
||
|
||
if self.canvas:
|
||
self.canvas.draw_idle()
|
||
|
||
def clear_all_targets(self):
|
||
"""Clears all target artists from the display (except finished trajectory markers)."""
|
||
all_artists = (
|
||
self.sim_target_artists
|
||
+ self.real_target_artists
|
||
+ self.sim_trail_artists
|
||
+ self.real_trail_artists
|
||
+ self.sim_label_artists
|
||
+ self.real_label_artists
|
||
)
|
||
for artist in all_artists:
|
||
artist.remove()
|
||
self.sim_target_artists.clear()
|
||
self.real_target_artists.clear()
|
||
self.sim_trail_artists.clear()
|
||
self.real_trail_artists.clear()
|
||
self.sim_label_artists.clear()
|
||
self.real_label_artists.clear()
|
||
# NOTE: finished_trajectory_markers are NOT cleared here - they persist until new simulation
|
||
|
||
def update_simulated_targets(self, targets: List[Target]):
|
||
"""Updates and redraws only the simulated targets."""
|
||
self._update_target_category(targets, "simulated")
|
||
# Schedule a non-blocking redraw to avoid blocking the Tk event loop.
|
||
# The main view already calls `canvas.draw_idle()` once per GUI loop,
|
||
# but calling `draw_idle()` here helps ensure an update is requested
|
||
# when this method is invoked directly by UI actions.
|
||
if self.canvas:
|
||
try:
|
||
self.canvas.draw_idle()
|
||
except Exception:
|
||
# Fall back to a blocking draw if draw_idle is not available
|
||
try:
|
||
self.canvas.draw()
|
||
except Exception:
|
||
logger.exception("Failed to redraw PPI canvas")
|
||
|
||
def update_real_targets(self, targets: List[Target]):
|
||
"""Updates and redraws only the real targets."""
|
||
try:
|
||
now = time.monotonic()
|
||
self._real_update_timestamps.append(now)
|
||
if (
|
||
now - self._last_update_summary_time
|
||
) >= self._update_summary_interval_s:
|
||
self._last_update_summary_time = now
|
||
except Exception:
|
||
pass
|
||
|
||
self._update_target_category(targets, "real")
|
||
# Use non-blocking redraw to prevent long Matplotlib draw times from
|
||
# stalling the Tk event loop. This keeps the GUI refresh loop closer
|
||
# to the configured `GUI_REFRESH_RATE_MS` even under heavy load.
|
||
if self.canvas:
|
||
try:
|
||
self.canvas.draw_idle()
|
||
except Exception:
|
||
try:
|
||
self.canvas.draw()
|
||
except Exception:
|
||
logger.exception("Failed to redraw PPI canvas")
|
||
|
||
def get_real_update_rate(self, window_seconds: float = 1.0) -> float:
|
||
"""
|
||
Returns approximate PPI "real targets" update rate (updates/sec).
|
||
"""
|
||
try:
|
||
now = time.monotonic()
|
||
cutoff = now - float(window_seconds)
|
||
count = sum(1 for ts in self._real_update_timestamps if ts >= cutoff)
|
||
return count / float(window_seconds) if window_seconds > 0 else float(count)
|
||
except Exception:
|
||
return 0.0
|
||
|
||
def _update_target_category(self, new_data: List[Target], category: str):
|
||
"""
|
||
Generic helper to update targets for a specific category ('simulated' or 'real').
|
||
"""
|
||
if category == "simulated":
|
||
target_artists, trail_artists, label_artists = (
|
||
self.sim_target_artists,
|
||
self.sim_trail_artists,
|
||
self.sim_label_artists,
|
||
)
|
||
trail_data, show_points, show_trail = (
|
||
self._trails["simulated"],
|
||
self.show_sim_points_var.get(),
|
||
self.show_sim_trail_var.get(),
|
||
)
|
||
color, trail_color = "green", "limegreen"
|
||
else:
|
||
target_artists, trail_artists, label_artists = (
|
||
self.real_target_artists,
|
||
self.real_trail_artists,
|
||
self.real_label_artists,
|
||
)
|
||
trail_data, show_points, show_trail = (
|
||
self._trails["real"],
|
||
self.show_real_points_var.get(),
|
||
self.show_real_trail_var.get(),
|
||
)
|
||
color, trail_color = "red", "tomato"
|
||
|
||
for artist in target_artists + trail_artists + label_artists:
|
||
artist.remove()
|
||
target_artists.clear()
|
||
trail_artists.clear()
|
||
label_artists.clear()
|
||
|
||
if show_points or show_trail:
|
||
for t in new_data:
|
||
if t.active:
|
||
pos = (np.deg2rad(t.current_azimuth_deg), t.current_range_nm)
|
||
trail_data[t.target_id].append(pos)
|
||
|
||
if show_points:
|
||
active_targets = [t for t in new_data if t.active]
|
||
if active_targets:
|
||
self._draw_target_visuals(
|
||
active_targets, color, target_artists, label_artists
|
||
)
|
||
|
||
inactive_targets = [t for t in new_data if not t.active]
|
||
if inactive_targets and category == "simulated":
|
||
self._draw_inactive_markers(
|
||
inactive_targets, color, target_artists, label_artists
|
||
)
|
||
|
||
if show_trail:
|
||
self._draw_trails(trail_data, trail_color, trail_artists)
|
||
|
||
def _draw_inactive_markers(
|
||
self,
|
||
targets: List[Target],
|
||
color: str,
|
||
artist_list: List,
|
||
label_artist_list: List,
|
||
):
|
||
for target in targets:
|
||
try:
|
||
r_nm = target.current_range_nm
|
||
theta_rad_plot = np.deg2rad(target.current_azimuth_deg)
|
||
(dot,) = self.ax.plot(
|
||
theta_rad_plot,
|
||
r_nm,
|
||
"o",
|
||
markersize=6,
|
||
color=color,
|
||
alpha=0.6,
|
||
zorder=5,
|
||
)
|
||
artist_list.append(dot)
|
||
(x_mark,) = self.ax.plot(
|
||
theta_rad_plot,
|
||
r_nm,
|
||
marker="x",
|
||
color="yellow",
|
||
markersize=8,
|
||
markeredgewidth=0.9,
|
||
linestyle="",
|
||
zorder=6,
|
||
)
|
||
label_artist_list.append(x_mark)
|
||
# Add to persistent list so it survives simulation end
|
||
self.finished_trajectory_markers.append(x_mark)
|
||
except Exception:
|
||
pass
|
||
|
||
def _draw_target_visuals(
|
||
self,
|
||
targets: List[Target],
|
||
color: str,
|
||
artist_list: List,
|
||
label_artist_list: List,
|
||
):
|
||
vector_len_nm = self.range_var.get() / 20.0
|
||
marker_size = 8 if color == "green" else 6
|
||
|
||
for target in targets:
|
||
r_nm = target.current_range_nm
|
||
theta_rad_plot = np.deg2rad(target.current_azimuth_deg)
|
||
|
||
(dot,) = self.ax.plot(
|
||
theta_rad_plot, r_nm, "o", markersize=marker_size, color=color, zorder=5
|
||
)
|
||
artist_list.append(dot)
|
||
|
||
heading_relative_to_north_rad = np.deg2rad(target.current_heading_deg)
|
||
|
||
x_start_rel_nm = r_nm * math.sin(theta_rad_plot)
|
||
y_start_rel_nm = r_nm * math.cos(theta_rad_plot)
|
||
|
||
dx_nm = vector_len_nm * math.sin(heading_relative_to_north_rad)
|
||
dy_nm = vector_len_nm * math.cos(heading_relative_to_north_rad)
|
||
|
||
x_end_rel_nm = x_start_rel_nm + dx_nm
|
||
y_end_rel_nm = y_start_rel_nm + dy_nm
|
||
|
||
r_end_nm = math.hypot(x_end_rel_nm, y_end_rel_nm)
|
||
theta_end_rad_plot = math.atan2(x_end_rel_nm, y_end_rel_nm)
|
||
|
||
(line,) = self.ax.plot(
|
||
[theta_rad_plot, theta_end_rad_plot],
|
||
[r_nm, r_end_nm],
|
||
color=color,
|
||
linewidth=1.2,
|
||
zorder=4,
|
||
)
|
||
artist_list.append(line)
|
||
|
||
txt = self.ax.text(
|
||
theta_rad_plot,
|
||
r_nm + (vector_len_nm * 0.5),
|
||
str(target.target_id),
|
||
color="white",
|
||
fontsize=8,
|
||
ha="center",
|
||
va="bottom",
|
||
zorder=7,
|
||
)
|
||
label_artist_list.append(txt)
|
||
|
||
def _draw_trails(self, trail_data: Dict, color: str, artist_list: List):
|
||
for trail in trail_data.values():
|
||
if len(trail) > 1:
|
||
thetas, rs = zip(*trail)
|
||
(line,) = self.ax.plot(
|
||
thetas,
|
||
rs,
|
||
color=color,
|
||
linestyle="-",
|
||
linewidth=0.8,
|
||
alpha=0.7,
|
||
zorder=3,
|
||
)
|
||
artist_list.append(line)
|
||
|
||
def clear_finished_trajectory_markers(self):
|
||
"""Clears the persistent yellow X markers for finished trajectories.
|
||
Called when starting a new simulation or changing scenario.
|
||
"""
|
||
for artist in self.finished_trajectory_markers:
|
||
try:
|
||
artist.remove()
|
||
except (ValueError, AttributeError):
|
||
# Artist may have already been removed or is invalid
|
||
pass
|
||
self.finished_trajectory_markers.clear()
|
||
if self.canvas:
|
||
self.canvas.draw()
|
||
|
||
def clear_trails(self):
|
||
self._trails["simulated"].clear()
|
||
self._trails["real"].clear()
|
||
self.clear_all_targets()
|
||
self.clear_finished_trajectory_markers()
|
||
if self.canvas:
|
||
self.canvas.draw()
|
||
|
||
def _update_scan_lines(self):
|
||
"""Update scan/sector indicator lines."""
|
||
self._update_plot_orientation()
|
||
|
||
def _on_range_selected(self, event=None):
|
||
"""Adjust plot limits when the range selector value changes.
|
||
|
||
Ensures scan lines and the canvas are updated to match the new range.
|
||
"""
|
||
self.ax.set_ylim(0, self.range_var.get())
|
||
self._update_plot_orientation()
|
||
if self.canvas:
|
||
self.canvas.draw()
|
||
|
||
def clear_previews(self):
|
||
"""Clear any scenario or trajectory preview artists from the plot."""
|
||
for artist in self.preview_artists:
|
||
artist.set_data([], [])
|
||
for arts in self.preview_path_artists.values():
|
||
for a in arts:
|
||
a.remove()
|
||
self.preview_path_artists.clear()
|
||
if self.canvas:
|
||
self.canvas.draw()
|
||
|
||
def draw_scenario_preview(self, scenario):
|
||
self.clear_previews()
|
||
if scenario is None:
|
||
return
|
||
|
||
for target in scenario.get_all_targets():
|
||
try:
|
||
path, _ = Target.generate_path_from_waypoints(
|
||
target.trajectory, target.use_spline
|
||
)
|
||
if not path:
|
||
continue
|
||
|
||
path_thetas, path_rs = [], []
|
||
for point in path:
|
||
x_ft, y_ft = point[1], point[2]
|
||
r_ft = math.hypot(x_ft, y_ft)
|
||
az_rad_plot = math.atan2(x_ft, y_ft)
|
||
path_rs.append(r_ft / NM_TO_FT)
|
||
path_thetas.append(az_rad_plot)
|
||
|
||
(line_art,) = self.ax.plot(
|
||
path_thetas,
|
||
path_rs,
|
||
color="limegreen",
|
||
linestyle="--",
|
||
linewidth=1.2,
|
||
alpha=0.9,
|
||
)
|
||
(start_art,) = self.ax.plot(
|
||
[path_thetas[0]], [path_rs[0]], "go", markersize=6
|
||
)
|
||
self.preview_path_artists[target.target_id] = [line_art, start_art]
|
||
except Exception:
|
||
logger.exception(
|
||
"Failed to draw preview for target %s",
|
||
getattr(target, "target_id", "?"),
|
||
)
|
||
|
||
if self.canvas:
|
||
self.canvas.draw()
|
||
|
||
def draw_trajectory_preview(self, waypoints: List[Waypoint], use_spline: bool):
|
||
self.clear_previews()
|
||
self.clear_trails()
|
||
if not waypoints or waypoints[0].maneuver_type != ManeuverType.FLY_TO_POINT:
|
||
return
|
||
|
||
path, _ = Target.generate_path_from_waypoints(waypoints, use_spline)
|
||
if not path:
|
||
return
|
||
|
||
path_thetas, path_rs = [], []
|
||
for point in path:
|
||
x_ft, y_ft = point[1], point[2]
|
||
r_ft = math.hypot(x_ft, y_ft)
|
||
az_rad_plot = math.atan2(x_ft, y_ft)
|
||
path_rs.append(r_ft / NM_TO_FT)
|
||
path_thetas.append(az_rad_plot)
|
||
|
||
self._path_plot.set_data(path_thetas, path_rs)
|
||
|
||
wp_thetas, wp_rs = [], []
|
||
for wp in waypoints:
|
||
if wp.maneuver_type == ManeuverType.FLY_TO_POINT:
|
||
r_nm = wp.target_range_nm or 0.0
|
||
az_rad_plot = np.deg2rad(wp.target_azimuth_deg or 0.0)
|
||
wp_rs.append(r_nm)
|
||
wp_thetas.append(az_rad_plot)
|
||
self._waypoints_plot.set_data(wp_thetas, wp_rs)
|
||
|
||
start_wp = waypoints[0]
|
||
start_r = start_wp.target_range_nm or 0.0
|
||
start_theta = np.deg2rad(start_wp.target_azimuth_deg or 0.0)
|
||
self._start_plot.set_data([start_theta], [start_r])
|
||
|
||
if self.canvas:
|
||
self.canvas.draw()
|
||
|
||
def reconfigure_radar(self, max_range_nm: int, scan_limit_deg: int):
|
||
self.max_range, self.scan_limit_deg = max_range_nm, scan_limit_deg
|
||
steps = [10, 20, 40, 80, 100, 160, 240, 320]
|
||
valid_steps = sorted(
|
||
[s for s in steps if s <= max_range_nm]
|
||
+ ([max_range_nm] if max_range_nm not in steps else [])
|
||
)
|
||
self.range_selector["values"] = valid_steps
|
||
if self.range_var.get() not in valid_steps:
|
||
self.range_var.set(max_range_nm)
|
||
self._on_range_selected()
|
||
|
||
def render_antenna_line(self, az_deg: Optional[float]):
|
||
"""Directly renders the antenna line at a given absolute azimuth."""
|
||
try:
|
||
if self._antenna_line_artist is None:
|
||
logger.warning("Antenna artist is not initialized. Cannot render.")
|
||
return
|
||
|
||
if az_deg is None or not self.animate_antenna_var.get():
|
||
self._antenna_line_artist.set_visible(False)
|
||
else:
|
||
az_float = float(az_deg)
|
||
|
||
# WORKING SOLUTION confirmed by user:
|
||
# North-Up with heading=+10°:
|
||
# - Server sends ant_nav_az from +50° to -70°
|
||
# - Add 2×heading: result is +70° to -50° ✓ CORRECT
|
||
#
|
||
# Heading-Up with heading=+10°:
|
||
# - Grid is rotated by -10° (via set_theta_offset)
|
||
# - Scan lines are drawn at heading ± limit
|
||
# - Antenna needs same transformation as scan lines
|
||
#
|
||
# Solution: use SAME formula for both modes
|
||
# - North-Up: add 2×heading
|
||
# - Heading-Up: add 2×heading (counter-rotates for the rotated grid)
|
||
|
||
final_az_for_plot = az_float + (2.0 * self.ownship_heading_deg)
|
||
|
||
# Normalize to [-180, 180] range
|
||
while final_az_for_plot > 180:
|
||
final_az_for_plot -= 360
|
||
while final_az_for_plot < -180:
|
||
final_az_for_plot += 360
|
||
|
||
# Convert final angle to theta for Matplotlib (0=N, positive=CCW)
|
||
theta = np.deg2rad(final_az_for_plot)
|
||
max_r = self.ax.get_ylim()[1]
|
||
|
||
self._antenna_line_artist.set_data([theta, theta], [0, max_r])
|
||
self._antenna_line_artist.set_visible(True)
|
||
|
||
if self.canvas:
|
||
self.canvas.draw_idle()
|
||
except Exception:
|
||
logger.exception("Error rendering antenna line")
|