From 1575c1b38a6a78e30ccd2973eb66e210c9d94662 Mon Sep 17 00:00:00 2001 From: VALLONGOL Date: Wed, 15 Oct 2025 10:14:58 +0200 Subject: [PATCH] refactoring di preview e simulazione usando traiettorie calcolate --- Temp/spline_preview.csv | 199 +++++---- settings.json | 47 +- target_simulator/core/command_builder.py | 7 +- target_simulator/core/models.py | 400 ++++++++--------- target_simulator/core/simulation_engine.py | 50 +-- target_simulator/gui/ppi_display.py | 416 +++++------------- .../gui/trajectory_editor_window.py | 317 +++++-------- 7 files changed, 569 insertions(+), 867 deletions(-) diff --git a/Temp/spline_preview.csv b/Temp/spline_preview.csv index 8ff5dfb..35dafef 100644 --- a/Temp/spline_preview.csv +++ b/Temp/spline_preview.csv @@ -1,101 +1,100 @@ x_nm,y_nm -0.17452406437283513,9.998476951563912 -0.2859422147817867,10.279638104623308 -0.4421393353133215,10.609933583832394 -0.636432547474389,10.985187585712282 -0.8621389727719386,11.401224306784082 -1.1125757327129193,11.853867943568899 -1.3810599488042814,12.338942692587844 -1.6609087425529734,12.852272750362022 -1.9454392354659444,13.389682313412546 -2.227968549050145,13.946995578260525 -2.5018138048125236,14.520036741427065 -2.7602921242600305,15.104629999433275 -2.996720628899614,15.696599548800267 -3.2044164402382243,16.291769586049142 -3.3766966797828104,16.885964307701016 -3.5068784690403216,17.475007910277 -3.588278929517707,18.054724590298196 -3.6142151827219164,18.620938544285714 -3.578004350159902,19.169473968760666 -3.472963553338606,19.696155060244156 -3.4729635533386065,19.69615506024416 -3.272762644392875,20.217383756990923 -2.964843068011936,20.750866222507394 -2.5631793345343286,21.294382991192247 -2.081745954298589,21.845714597444157 -1.5345174376432555,22.402641575661793 -0.9354682949068648,22.962944460243826 -0.2985730364279542,23.52440378558893 --0.36219382745493856,24.084800086095772 --1.032857786403277,24.641913896163032 --1.699444330078522,25.193525750189377 --2.347978948142139,25.737416182573483 --2.964487130255587,26.271365727714013 --3.5349943660803307,26.793154920009652 --4.045526145277833,27.30056429385906 --4.482107957509558,27.79137438366092 --4.830765292436963,28.263365723813894 --5.077523639721516,28.714318848716662 --5.208408489024677,29.14201429276789 --5.2094453300079095,29.544232590366253 --5.2094453300079095,29.544232590366242 --5.087215806911271,29.93160758132983 --4.8634013589893685,30.31504050771131 --4.54667716842549,30.69271300726126 --4.145718417402923,31.062806717730222 --3.6692002881049564,31.42350327686877 --3.1257979627148766,31.772984322427458 --2.5241866234159707,32.10943149215684 --1.873041452391528,32.43102642380748 --1.1810376318248348,32.735950755129934 --0.4568503438991798,33.02238612387475 -0.29084522920215017,33.28851416779252 -1.0533739052958677,33.53251652463377 -1.822060502198683,33.75257483214907 -2.5882298377273103,33.94687072808898 -3.343206729698462,34.113585850204075 -4.078315995928851,34.25090183624488 -4.784882454235189,34.35700032396197 -5.454230922434187,34.430062951105924 -6.077686218342558,34.46827135542727 -6.077686218342562,34.468271355427284 -6.6950615437258465,34.47971290071672 -7.3476044195818755,34.472832854810775 -8.028791142575807,34.44634971152879 -8.732098009372796,34.3989819646901 -9.451001316637997,34.32944810811404 -10.178977361036566,34.23646663561996 -10.909502439233654,34.11875604102719 -11.636052847894423,33.97503481815507 -12.352104883684024,33.80402146082294 -13.051134843267612,33.60443446285013 -13.726619023310347,33.37499231805599 -14.372033720477376,33.114413520259845 -14.98085523143386,32.82141656328105 -15.546559852844958,32.49471994093893 -16.062623881375814,32.13304214705283 -16.522523613691593,31.735101675442085 -16.919735346457447,31.29961701992603 -17.247735376338525,30.825306674324008 -17.499999999999996,30.310889132455358 -17.499999999999996,30.310889132455355 -17.667951456825268,29.722378176057024 -17.752649951768674,29.034858901953363 -17.76202872985173,28.26065677659477 -17.704021036095945,27.412097266431687 -17.586560115522854,26.501505837914515 -17.417579213153953,25.541207957493693 -17.205011574010783,24.54352909161962 -16.956790443114837,23.52079470674273 -16.68084906548765,22.485330269313444 -16.385120686150735,21.449461245782175 -16.0775385501256,20.425513102599343 -15.766035902433776,19.42581130621538 -15.45854598809677,18.462681323080695 -15.1630020521361,17.5484486196457 -14.88733733957329,16.69543866236083 -14.639485095429851,15.9159769176765 -14.427378564727302,15.222388852043133 -14.258950992487165,14.626999931911136 -14.142135623730947,14.142135623730947 +1.7364817766693033,9.84807753012208 +1.8196915585801245,9.997286852043835 +1.909698355298528,10.164705993254746 +2.006314644389864,10.349354914685591 +2.1093529034194827,10.550253577267155 +2.218625609952734,10.766421941930222 +2.3339452415549684,10.99687996960557 +2.4551242757915364,11.240647621223985 +2.581975190227787,11.496744857716246 +2.714310462429072,11.764191640013136 +2.851942569960741,12.04200792904544 +2.9946839903881437,12.329213685743936 +3.14234720127663,12.624828871039403 +3.2947446801915516,12.927873445862636 +3.4516889046982575,13.237367371144403 +3.612992352362098,13.552330607815493 +3.7784675007484245,13.871783116806688 +3.947926827422586,14.19474485904877 +4.121182809949932,14.520235795472518 +4.298047925895815,14.847275887008719 +4.478334652825583,15.17488509458815 +4.661855468304586,15.502083379141595 +4.848422849898178,15.827890701599843 +5.037849275171705,16.151327022893664 +5.2299472216905185,16.471412303953848 +5.424529167019969,16.787166505711173 +5.621407588725407,17.09760958909643 +5.8203949643721815,17.401761515040384 +6.021303771525644,17.698642244473834 +6.223946487751144,17.987271738327554 +6.4281355906140325,18.26666995753233 +6.6336835576796584,18.535856863018942 +6.840402866513372,18.79385241571817 +6.840402866513374,18.79385241571817 +7.064412663195302,19.043619595904882 +7.319986102939524,19.28896147814137 +7.604166677012322,19.530158164789555 +7.913997876679983,19.767489758211376 +8.246523193208787,20.001236360768743 +8.598786117865023,20.231678074823588 +8.96783014191497,20.459095002737843 +9.350698756624908,20.683767246873423 +9.744435453261131,20.90597490959226 +10.146083723089918,21.12599809325628 +10.552687057377549,21.3441169002274 +10.961288947390312,21.560611432867567 +11.368932884394491,21.775761793538678 +11.772662359656369,21.989848084602684 +12.169520864442228,22.203150408421493 +12.556551890018351,22.41594886735704 +12.930798927651024,22.62852356377125 +13.289305468606532,22.84115460002604 +13.629115004151155,23.054122078483353 +13.947271025551181,23.267706101505095 +14.24081702407289,23.482186771453204 +14.506796490982568,23.6978441906896 +14.742252917546494,23.91495846157622 +14.944229795030958,24.133809686474972 +15.109770614702246,24.354677967747794 +15.23591886782663,24.57784340775661 +15.319718045670406,24.80358610886334 +15.358211639499848,25.032186173429917 +15.348443140581244,25.26392370381826 +15.28745604018088,25.499078802390304 +15.172293829565035,25.737931571507964 +15.0,25.980762113533167 +14.999999999999998,25.98076211353316 +14.76007873005941,26.230939963143115 +14.447076186168827,26.491050501357915 +14.065598841372388,26.760197600053854 +13.62025316871424,27.037485131107207 +13.11564564123853,27.322016966394273 +12.556382731989396,27.612896977791323 +11.947070914010986,27.909229037174644 +11.292316660347446,28.210117016420522 +10.596726444042913,28.514664787405238 +9.86490673814154,28.82197622200508 +9.101464015687464,29.13115519209633 +8.311004749724834,29.441305569555276 +7.498135413297791,29.751531226258194 +6.66746247945048,30.060936034081376 +5.823592421227048,30.368623864901103 +4.971131711671633,30.67369859059366 +4.114686823828384,30.97526408303533 +3.2588642307414455,31.272424214102394 +2.4082704054549575,31.564282855671145 +1.5675118210130687,31.84994387961786 +0.7411949504599216,32.12851115781883 +-0.06607373316034426,32.39908856215034 +-0.8496877568035757,32.660779964488654 +-1.6050406474256338,32.91268923671007 +-2.3275259319823753,33.15392025069089 +-3.0125371374296517,33.38357687830738 +-3.6554677907233284,33.600762991435815 +-4.2517114188192515,33.80458246195249 +-4.796661548673278,33.99413916173369 +-5.285711707241269,34.1685369626557 +-5.714255421479081,34.326879736594805 +-6.077686218342556,34.46827135542728 diff --git a/settings.json b/settings.json index 0dbc493..e101638 100644 --- a/settings.json +++ b/settings.json @@ -3,7 +3,7 @@ "scan_limit": 60, "max_range": 100, "geometry": "1200x1024+463+195", - "last_selected_scenario": "scenario3", + "last_selected_scenario": "scenario1", "connection": { "target": { "type": "tftp", @@ -47,10 +47,10 @@ }, { "maneuver_type": "Fly for Duration", + "duration_s": 100.0, + "target_altitude_ft": 10000.0, "target_velocity_fps": 1670.9318999999994, - "target_heading_deg": 10.0, - "duration_s": 300.0, - "target_altitude_ft": 10000.0 + "target_heading_deg": 10.0 }, { "maneuver_type": "Fly to Point", @@ -59,7 +59,8 @@ "target_range_nm": 25.0, "target_azimuth_deg": -20.0 } - ] + ], + "use_spline": false } ] }, @@ -150,7 +151,7 @@ "target_azimuth_deg": -10.0 } ], - "use_spline": true + "use_spline": false } ] }, @@ -164,29 +165,43 @@ "trajectory": [ { "maneuver_type": "Fly to Point", - "target_velocity_fps": 506.343, - "target_heading_deg": 180.0, "duration_s": 10.0, "target_altitude_ft": 10000.0, - "target_range_nm": 20.0, + "target_velocity_fps": 506.343, + "target_heading_deg": 180.0, + "target_range_nm": 5.0, "target_azimuth_deg": 0.0 }, { "maneuver_type": "Fly for Duration", - "target_velocity_fps": 506.343, - "target_heading_deg": 90.0, "duration_s": 20.0, - "target_altitude_ft": 10000.0 + "target_altitude_ft": 10000.0, + "target_velocity_fps": 506.343, + "target_heading_deg": 90.0 }, { "maneuver_type": "Fly for Duration", - "target_velocity_fps": 506.343, - "target_heading_deg": 180.0, "duration_s": 10.0, - "target_altitude_ft": 10000.0 + "target_altitude_ft": 10000.0, + "target_velocity_fps": 506.343, + "target_heading_deg": 180.0 + }, + { + "maneuver_type": "Fly for Duration", + "duration_s": 20.0, + "target_altitude_ft": 10000.0, + "target_velocity_fps": 506.343, + "target_heading_deg": 90.0 + }, + { + "maneuver_type": "Fly for Duration", + "duration_s": 30.0, + "target_altitude_ft": 10000.0, + "target_velocity_fps": 506.343, + "target_heading_deg": -180.0 } ], - "use_spline": false + "use_spline": true } ] } diff --git a/target_simulator/core/command_builder.py b/target_simulator/core/command_builder.py index 7a59941..c558417 100644 --- a/target_simulator/core/command_builder.py +++ b/target_simulator/core/command_builder.py @@ -14,14 +14,15 @@ logger = get_logger(__name__) def build_tgtinit(target: Target) -> str: """ Builds the 'tgtinit' command from a target's initial state. + After the refactor, the target object's state is pre-calculated + by reset_simulation(), so we can read kinematics directly. """ params = [ target.target_id, f"{target.current_range_nm:.2f}", f"{target.current_azimuth_deg:.2f}", - # Velocity and heading for tgtinit are taken from the first waypoint - f"{(target.trajectory[0].target_velocity_fps or 0.0) if target.trajectory else 0.0:.2f}", - f"{(target.trajectory[0].target_heading_deg or 0.0) if target.trajectory else 0.0:.2f}", + f"{target.current_velocity_fps:.2f}", + f"{target.current_heading_deg:.2f}", f"{target.current_altitude_ft:.2f}" ] qualifiers = ["/s" if target.active else "/-s", "/t" if target.traceable else "/-t"] diff --git a/target_simulator/core/models.py b/target_simulator/core/models.py index 8203342..f1c285d 100644 --- a/target_simulator/core/models.py +++ b/target_simulator/core/models.py @@ -7,7 +7,7 @@ from __future__ import annotations import math from enum import Enum from dataclasses import dataclass, field, fields -from typing import Dict, List, Optional +from typing import Dict, List, Optional, Tuple # --- Constants --- MIN_TARGET_ID = 0 @@ -25,28 +25,23 @@ class Waypoint: """Represents a segment of a target's trajectory, defining a maneuver.""" maneuver_type: ManeuverType = ManeuverType.FLY_FOR_DURATION - # Parameters used by FLY_FOR_DURATION and as final state for FLY_TO_POINT + # Parameters used by both maneuver types + duration_s: Optional[float] = 10.0 + target_altitude_ft: Optional[float] = 10000.0 + + # Final state parameters for FLY_TO_POINT and dynamic for FLY_FOR_DURATION target_velocity_fps: Optional[float] = None target_heading_deg: Optional[float] = None - # Parameters used by both - duration_s: Optional[float] = 10.0 - target_altitude_ft: Optional[float] = 10000.0 - - # Parameters used by FLY_TO_POINT + # Position parameters for FLY_TO_POINT target_range_nm: Optional[float] = None target_azimuth_deg: Optional[float] = None - # Internal state for interpolation, set when waypoint becomes active - _start_velocity_fps: float = field(init=False, repr=False, default=0.0) - _start_heading_deg: float = field(init=False, repr=False, default=0.0) - _start_altitude_ft: float = field(init=False, repr=False, default=0.0) - _calculated_duration_s: Optional[float] = field(init=False, default=None, repr=False) - def to_dict(self) -> Dict: + """Serializes the waypoint to a dictionary.""" data = {"maneuver_type": self.maneuver_type.value} - # Use dataclasses.fields to iterate reliably for f in fields(self): + # Exclude private fields and the already-handled maneuver_type if not f.name.startswith('_') and f.name != "maneuver_type": val = getattr(self, f.name) if val is not None: @@ -55,13 +50,12 @@ class Waypoint: @dataclass class Target: - _spline_path: Optional[List[List[float]]] = field(init=False, default=None, repr=False) """Represents a dynamic 3D target with a programmable trajectory.""" target_id: int trajectory: List[Waypoint] = field(default_factory=list) active: bool = True traceable: bool = True - use_spline: bool = False # Se True, la traiettoria viene interpretata come spline + use_spline: bool = False # --- Current simulation state (dynamic) --- current_range_nm: float = field(init=False, default=0.0) @@ -71,247 +65,211 @@ class Target: current_heading_deg: float = field(init=False, default=0.0) current_pitch_deg: float = field(init=False, default=0.0) + # Internal Cartesian position in feet _pos_x_ft: float = field(init=False, repr=False, default=0.0) _pos_y_ft: float = field(init=False, repr=False, default=0.0) _pos_z_ft: float = field(init=False, repr=False, default=0.0) + # Simulation time and pre-calculated path _sim_time_s: float = field(init=False, default=0.0) - _current_waypoint_index: int = field(init=False, default=-1) - _time_in_waypoint_s: float = field(init=False, default=0.0) + _total_duration_s: float = field(init=False, default=0.0) + # Path format: List of (timestamp_s, x_ft, y_ft, z_ft) + _path: List[Tuple[float, float, float, float]] = field(init=False, repr=False, default_factory=list) def __post_init__(self): + """Validates ID and initializes the simulation state.""" if not (MIN_TARGET_ID <= self.target_id <= MAX_TARGET_ID): raise ValueError(f"Target ID must be between {MIN_TARGET_ID} and {MAX_TARGET_ID}.") self.reset_simulation() def reset_simulation(self): + """ + Resets the simulation time and pre-calculates the entire trajectory path + based on the waypoints. This is the core of the refactored logic. + """ self._sim_time_s = 0.0 - self._time_in_waypoint_s = 0.0 - self._current_waypoint_index = -1 + self._generate_path() - if self.use_spline and self.trajectory: + if self._path: + # Set initial state from the first point of the generated path + initial_pos = self._path[0] + self._pos_x_ft = initial_pos[1] + self._pos_y_ft = initial_pos[2] + self._pos_z_ft = initial_pos[3] + + # Calculate initial heading and velocity from the first segment + if len(self._path) > 1: + next_pos = self._path[1] + dx = next_pos[1] - self._pos_x_ft + dy = next_pos[2] - self._pos_y_ft + dz = next_pos[3] - self._pos_z_ft + dt = next_pos[0] - initial_pos[0] + + dist_3d = math.sqrt(dx**2 + dy**2 + dz**2) + dist_2d = math.sqrt(dx**2 + dy**2) + + self.current_heading_deg = math.degrees(math.atan2(dx, dy)) % 360 if dist_2d > 0.1 else 0.0 + self.current_pitch_deg = math.degrees(math.atan2(dz, dist_2d)) if dist_2d > 0.1 else 0.0 + self.current_velocity_fps = dist_3d / dt if dt > 0 else 0.0 + else: + self.current_heading_deg = 0.0 + self.current_pitch_deg = 0.0 + self.current_velocity_fps = 0.0 + else: + # No trajectory, reset to origin + self._pos_x_ft = self._pos_y_ft = self._pos_z_ft = 0.0 + self.current_velocity_fps = 0.0 + self.current_heading_deg = 0.0 + self.current_pitch_deg = 0.0 + + self._update_current_polar_coords() + self.active = bool(self.trajectory) # A target is active if it has a trajectory + + def _generate_path(self): + """Instance method that uses the static path generator.""" + self._path, self._total_duration_s = Target.generate_path_from_waypoints(self.trajectory, self.use_spline) + + @staticmethod + def generate_path_from_waypoints(waypoints: List[Waypoint], use_spline: bool) -> Tuple[List[Tuple[float, ...]], float]: + """ + Generates a time-stamped, Cartesian path from a list of waypoints. + This is a static method so it can be used for previews without a Target instance. + """ + path = [] + total_duration_s = 0.0 + if not waypoints: + return path, total_duration_s + + first_wp = waypoints[0] + if first_wp.maneuver_type != ManeuverType.FLY_TO_POINT: + return path, total_duration_s + + keyframes = [] + current_time = 0.0 + + range_ft = (first_wp.target_range_nm or 0.0) * NM_TO_FT + az_rad = math.radians(first_wp.target_azimuth_deg or 0.0) + current_x = range_ft * math.sin(az_rad) + current_y = range_ft * math.cos(az_rad) + current_z = first_wp.target_altitude_ft or 0.0 + + current_heading = first_wp.target_heading_deg or 0.0 + current_velocity = first_wp.target_velocity_fps or 0.0 + + keyframes.append((current_time, current_x, current_y, current_z)) + + for i, wp in enumerate(waypoints): + if i == 0: + continue + + duration = wp.duration_s or 0.0 + + if wp.maneuver_type == ManeuverType.FLY_TO_POINT: + target_range_ft = (wp.target_range_nm or 0.0) * NM_TO_FT + target_az_rad = math.radians(wp.target_azimuth_deg or 0.0) + current_x = target_range_ft * math.sin(target_az_rad) + current_y = target_range_ft * math.cos(target_az_rad) + current_z = wp.target_altitude_ft or current_z + + elif wp.maneuver_type == ManeuverType.FLY_FOR_DURATION: + velocity_to_use = wp.target_velocity_fps if wp.target_velocity_fps is not None else current_velocity + heading_to_use = wp.target_heading_deg if wp.target_heading_deg is not None else current_heading + + heading_rad = math.radians(heading_to_use) + dist_moved = velocity_to_use * duration + + current_x += dist_moved * math.sin(heading_rad) + current_y += dist_moved * math.cos(heading_rad) + current_z = wp.target_altitude_ft or current_z + + current_heading = heading_to_use + current_velocity = velocity_to_use + + total_duration_s += duration + keyframes.append((total_duration_s, current_x, current_y, current_z)) + + if len(keyframes) < 2: + return keyframes, total_duration_s + + if use_spline and len(keyframes) >= 4: from target_simulator.utils.spline import catmull_rom_spline - # Convert all waypoints to cartesian points in feet - pts = [] - for wp in self.trajectory: - if wp.maneuver_type == ManeuverType.FLY_TO_POINT and wp.target_range_nm is not None and wp.target_azimuth_deg is not None: - range_ft = (wp.target_range_nm or 0.0) * NM_TO_FT - az_rad = math.radians(wp.target_azimuth_deg or 0.0) - x = range_ft * math.sin(az_rad) - y = range_ft * math.cos(az_rad) - z = wp.target_altitude_ft or 0.0 - pts.append([x, y, z]) - - if not pts: - self._pos_x_ft = self._pos_y_ft = self._pos_z_ft = 0.0 - self.current_velocity_fps = 0.0 - self.current_heading_deg = 0.0 - self._spline_path = None - return - - # Generate spline path from points in feet - self._spline_path = catmull_rom_spline(pts, num_points=200) - self._spline_index = 0 - - # Set initial position from the first point of the spline - initial_pos = self._spline_path[0] - self._pos_x_ft, self._pos_y_ft, self._pos_z_ft = initial_pos[0], initial_pos[1], initial_pos[2] + points_to_spline = [kf[1:] for kf in keyframes] + num_spline_points = int(total_duration_s * 10) - # Calculate total duration from waypoints - total_duration = sum(wp.duration_s or 0.0 for wp in self.trajectory if wp.duration_s) + splined_points = catmull_rom_spline(points_to_spline, num_points=max(num_spline_points, 100)) - # Create a time mapping for each point in the spline path - if self._spline_path is not None and len(self._spline_path) > 1: - self._spline_point_times = [i * (total_duration / (len(self._spline_path) - 1)) for i in range(len(self._spline_path))] - else: - self._spline_point_times = [0.0] - - self._update_current_polar_coords() - + for i, point in enumerate(splined_points): + time = (i / (len(splined_points) - 1)) * total_duration_s if len(splined_points) > 1 else 0.0 + path.append((time, point[0], point[1], point[2])) else: - # Waypoint-based simulation reset - if not self.trajectory or self.trajectory[0].maneuver_type != ManeuverType.FLY_TO_POINT: - self._pos_x_ft = self._pos_y_ft = self._pos_z_ft = 0.0 - self.current_velocity_fps = 0.0 - self.current_heading_deg = 0.0 - self.current_pitch_deg = 0.0 - else: - first_wp = self.trajectory[0] - self.current_range_nm = first_wp.target_range_nm or 0.0 - self.current_azimuth_deg = first_wp.target_azimuth_deg or 0.0 - self.current_altitude_ft = first_wp.target_altitude_ft or 0.0 - self.current_velocity_fps = first_wp.target_velocity_fps or 0.0 - self.current_heading_deg = first_wp.target_heading_deg or 0.0 - self.current_pitch_deg = 0.0 - - range_ft = self.current_range_nm * NM_TO_FT - az_rad = math.radians(self.current_azimuth_deg) - self._pos_x_ft = range_ft * math.sin(az_rad) - self._pos_y_ft = range_ft * math.cos(az_rad) - self._pos_z_ft = self.current_altitude_ft - - self._activate_next_waypoint() - - self._update_current_polar_coords() - - def _activate_next_waypoint(self): - """Pre-calculates and activates the next waypoint in the trajectory.""" - self._current_waypoint_index += 1 - self._time_in_waypoint_s = 0.0 + path = keyframes - if self._current_waypoint_index >= len(self.trajectory): - self._current_waypoint_index = -1; return - - wp = self.trajectory[self._current_waypoint_index] - wp._start_velocity_fps = self.current_velocity_fps - wp._start_heading_deg = self.current_heading_deg - wp._start_altitude_ft = self.current_altitude_ft - - if wp.maneuver_type == ManeuverType.FLY_TO_POINT: - target_range_ft = (wp.target_range_nm or 0.0) * NM_TO_FT - target_az_rad = math.radians(wp.target_azimuth_deg or 0.0) - target_x = target_range_ft * math.sin(target_az_rad) - target_y = target_range_ft * math.cos(target_az_rad) - target_z = wp.target_altitude_ft or self._pos_z_ft - - dx, dy, dz = target_x - self._pos_x_ft, target_y - self._pos_y_ft, target_z - self._pos_z_ft - dist_3d = math.sqrt(dx**2 + dy**2 + dz**2) - dist_2d = math.sqrt(dx**2 + dy**2) - - self.current_heading_deg = math.degrees(math.atan2(dx, dy)) % 360 - self.current_pitch_deg = math.degrees(math.atan2(dz, dist_2d)) if dist_2d > 1 else 90.0 if dz > 0 else -90.0 - - if wp.duration_s and wp.duration_s > 0: - self.current_velocity_fps = dist_3d / wp.duration_s - wp._calculated_duration_s = wp.duration_s - else: - self.current_velocity_fps = 0 - wp._calculated_duration_s = 999999 - elif wp.maneuver_type == ManeuverType.FLY_FOR_DURATION: - # Aggiorna la posizione iniziale per il prossimo segmento - heading_rad = math.radians(wp.target_heading_deg or self.current_heading_deg) - pitch_rad = math.radians(self.current_pitch_deg) - dist_moved = (wp.target_velocity_fps or self.current_velocity_fps) * (wp.duration_s or 0.0) - dist_2d = dist_moved * math.cos(pitch_rad) - self._pos_x_ft += dist_2d * math.sin(heading_rad) - self._pos_y_ft += dist_2d * math.cos(heading_rad) - self._pos_z_ft += dist_moved * math.sin(pitch_rad) - self._update_current_polar_coords() - self.current_heading_deg = wp.target_heading_deg or self.current_heading_deg - self.current_velocity_fps = wp.target_velocity_fps or self.current_velocity_fps - wp._calculated_duration_s = wp.duration_s or 0.0 - else: - self.current_pitch_deg = 0 + return path, total_duration_s def update_state(self, delta_time_s: float): - if not self.active: + # ... (questo metodo rimane invariato) + if not self.active or not self._path: return - if self.use_spline and self._spline_path: - self._sim_time_s += delta_time_s - - total_duration = self._spline_point_times[-1] - if total_duration <= 0 or self._sim_time_s >= total_duration: - # Simulation finished, stay at the last point - self._spline_index = len(self._spline_path) - 1 + self._sim_time_s += delta_time_s + current_sim_time = min(self._sim_time_s, self._total_duration_s) + + if current_sim_time >= self._total_duration_s: + final_pos = self._path[-1] + self._pos_x_ft, self._pos_y_ft, self._pos_z_ft = final_pos[1], final_pos[2], final_pos[3] + self.current_velocity_fps = 0.0 + if self._sim_time_s >= self._total_duration_s: self.active = False - self.current_velocity_fps = 0.0 - else: - # Find the current index in the spline path based on time - progress = self._sim_time_s / total_duration - self._spline_index = int(progress * (len(self._spline_path) - 1)) - - pt = self._spline_path[self._spline_index] - - # Determine previous point for calculating heading/pitch - prev_pt = self._spline_path[self._spline_index - 1] if self._spline_index > 0 else pt - - # Update position (path is already in feet) - self._pos_x_ft, self._pos_y_ft, self._pos_z_ft = pt[0], pt[1], pt[2] - - # Update kinematics - dx = self._pos_x_ft - prev_pt[0] - dy = self._pos_y_ft - prev_pt[1] - dz = self._pos_z_ft - prev_pt[2] - - dist_moved_since_last_tick = math.sqrt(dx**2 + dy**2 + dz**2) - dist_2d = math.sqrt(dx**2 + dy**2) - - if dist_2d > 0.1: # Avoid division by zero or noise - self.current_heading_deg = math.degrees(math.atan2(dx, dy)) % 360 - self.current_pitch_deg = math.degrees(math.atan2(dz, dist_2d)) - - if delta_time_s > 0: - self.current_velocity_fps = dist_moved_since_last_tick / delta_time_s - - self._update_current_polar_coords() - - # --- DEBUG LOG --- - #import logging - #logging.getLogger("target_simulator.spline_debug").info( - # f"SplinePos: t={self._sim_time_s:.2f}s idx={self._spline_index} x={self._pos_x_ft:.2f} y={self._pos_y_ft:.2f} z={self._pos_z_ft:.2f}") - # --- DEBUG LOG --- - #import logging - #logging.getLogger("target_simulator.spline_debug").info( - # f"SplinePos: t={self._sim_time_s:.2f}s idx={self._spline_index} x={self._pos_x_ft:.2f} y={self._pos_y_ft:.2f} z={self._pos_z_ft:.2f}") else: - # Logica classica - self._sim_time_s += delta_time_s - self._time_in_waypoint_s += delta_time_s - if self._current_waypoint_index == -1: - # Traiettoria terminata: disattiva target - self.active = False - self.current_velocity_fps = 0.0 - return - wp = self.trajectory[self._current_waypoint_index] - duration = wp._calculated_duration_s if wp._calculated_duration_s is not None else wp.duration_s or 0.0 - if self._time_in_waypoint_s >= duration: - # Finalize state to match waypoint target - if wp.maneuver_type == ManeuverType.FLY_FOR_DURATION: - self.current_velocity_fps = wp.target_velocity_fps or self.current_velocity_fps - self.current_heading_deg = wp.target_heading_deg or self.current_heading_deg - self.current_altitude_ft = wp.target_altitude_ft or self.current_altitude_ft - self._activate_next_waypoint() - if self._current_waypoint_index == -1: - # Traiettoria terminata: disattiva target - self.active = False - self.current_velocity_fps = 0.0 - return - wp = self.trajectory[self._current_waypoint_index] # Get new active waypoint - duration = wp._calculated_duration_s if wp._calculated_duration_s is not None else wp.duration_s or 0.0 - progress = self._time_in_waypoint_s / duration if duration > 0 else 1.0 - if wp.maneuver_type == ManeuverType.FLY_FOR_DURATION: - # Interpolate kinematics during the maneuver - self.current_velocity_fps = wp._start_velocity_fps + ((wp.target_velocity_fps or wp._start_velocity_fps) - wp._start_velocity_fps) * progress - self.current_altitude_ft = wp._start_altitude_ft + ((wp.target_altitude_ft or wp._start_altitude_ft) - wp._start_altitude_ft) * progress - delta_heading = ((wp.target_heading_deg or wp._start_heading_deg) - wp._start_heading_deg + 180) % 360 - 180 - self.current_heading_deg = (wp._start_heading_deg + delta_heading * progress) % 360 - # Update position using current 3D vector - heading_rad = math.radians(self.current_heading_deg) - pitch_rad = math.radians(self.current_pitch_deg) - dist_moved = self.current_velocity_fps * delta_time_s - dist_2d = dist_moved * math.cos(pitch_rad) - self._pos_x_ft += dist_2d * math.sin(heading_rad) - self._pos_y_ft += dist_2d * math.cos(heading_rad) - self._pos_z_ft += dist_moved * math.sin(pitch_rad) - self._update_current_polar_coords() + p1 = self._path[0] + p2 = self._path[-1] + for i in range(len(self._path) - 1): + if self._path[i][0] <= current_sim_time <= self._path[i+1][0]: + p1 = self._path[i] + p2 = self._path[i+1] + break + + segment_duration = p2[0] - p1[0] + progress = (current_sim_time - p1[0]) / segment_duration if segment_duration > 0 else 1.0 + + prev_x, prev_y, prev_z = self._pos_x_ft, self._pos_y_ft, self._pos_z_ft + + self._pos_x_ft = p1[1] + (p2[1] - p1[1]) * progress + self._pos_y_ft = p1[2] + (p2[2] - p1[2]) * progress + self._pos_z_ft = p1[3] + (p2[3] - p1[3]) * progress + + dx = self._pos_x_ft - prev_x + dy = self._pos_y_ft - prev_y + dz = self._pos_z_ft - prev_z + + dist_moved_3d = math.sqrt(dx**2 + dy**2 + dz**2) + dist_moved_2d = math.sqrt(dx**2 + dy**2) + + if delta_time_s > 0: + self.current_velocity_fps = dist_moved_3d / delta_time_s + + if dist_moved_2d > 0.1: + self.current_heading_deg = math.degrees(math.atan2(dx, dy)) % 360 + self.current_pitch_deg = math.degrees(math.atan2(dz, dist_moved_2d)) + + self._update_current_polar_coords() def _update_current_polar_coords(self): - """Recalculates current polar coordinates from Cartesian ones.""" + # ... (questo metodo rimane invariato) self.current_range_nm = math.sqrt(self._pos_x_ft**2 + self._pos_y_ft**2) / NM_TO_FT - self.current_azimuth_deg = math.degrees(math.atan2(self._pos_x_ft, self._pos_y_ft)) + self.current_azimuth_deg = math.degrees(math.atan2(self._pos_x_ft, self._pos_y_ft)) % 360 self.current_altitude_ft = self._pos_z_ft def to_dict(self) -> Dict: - data = { + # ... (questo metodo rimane invariato) + return { "target_id": self.target_id, "active": self.active, "traceable": self.traceable, "trajectory": [wp.to_dict() for wp in self.trajectory], "use_spline": self.use_spline } - return data class Scenario: def __init__(self, name: str = "Untitled Scenario"): @@ -341,6 +299,10 @@ class Scenario: for target in self.targets.values(): target.update_state(delta_time_s) + def is_finished(self) -> bool: + """Checks if all targets in the scenario have finished their trajectory.""" + return all(not target.active for target in self.targets.values()) + def to_dict(self) -> Dict: return { "name": self.name, diff --git a/target_simulator/core/simulation_engine.py b/target_simulator/core/simulation_engine.py index bb9032e..f172196 100644 --- a/target_simulator/core/simulation_engine.py +++ b/target_simulator/core/simulation_engine.py @@ -20,8 +20,6 @@ TICK_RATE_HZ = 10.0 TICK_INTERVAL_S = 1.0 / TICK_RATE_HZ class SimulationEngine(threading.Thread): - # ...existing code... - def __init__(self, communicator: Optional[CommunicatorInterface], update_queue: Optional[Queue]): super().__init__(daemon=True, name="SimulationEngineThread") self.logger = get_logger(__name__) @@ -40,7 +38,7 @@ class SimulationEngine(threading.Thread): def load_scenario(self, scenario: Scenario): """Loads a new scenario into the engine and resets its simulation state.""" - self.logger.info(f"Loading scenario '{scenario.name}' (target_{scenario.get_all_targets()[0].target_id if scenario.get_all_targets() else 'N/A'}) into simulation engine.") + self.logger.info(f"Loading scenario '{scenario.name}' into simulation engine.") self.scenario = scenario self.scenario.reset_simulation() @@ -65,6 +63,7 @@ class SimulationEngine(threading.Thread): if not self.scenario: time.sleep(TICK_INTERVAL_S) continue + if self._is_paused: time.sleep(TICK_INTERVAL_S) continue @@ -80,39 +79,36 @@ class SimulationEngine(threading.Thread): updated_targets = self.scenario.get_all_targets() # --- Check for simulation end --- - if hasattr(self.scenario, 'is_finished') and self.scenario.is_finished(): - self.logger.info("Simulation finished: auto-stopping thread.") + if self.scenario.is_finished(): + self.logger.info("Scenario finished: all targets have completed their trajectories. Stopping engine.") self._stop_event.set() if self.update_queue: try: self.update_queue.put_nowait('SIMULATION_FINISHED') except Exception: - pass + pass # Ignore if queue is full on the last message break # --- Communication Step (conditional) --- if self.communicator and self.communicator.is_open: if current_time - self._last_update_time >= self.update_interval_s: self._last_update_time = current_time - - # --- Batch commands for communicators that support it (TFTP) --- - if hasattr(self.communicator, 'send_commands'): - commands_to_send = [] - for target in updated_targets: - if target.active: - commands_to_send.append(command_builder.build_tgtset_from_target_state(target)) - if commands_to_send: + commands_to_send = [] + for target in updated_targets: + if target.active: # Only send updates for active targets + commands_to_send.append(command_builder.build_tgtset_from_target_state(target)) + + if commands_to_send: + # Batch commands for communicators that support it (TFTP) + if hasattr(self.communicator, 'send_commands'): self.communicator.send_commands(commands_to_send) - # --- Send commands individually for other communicators (Serial) --- - else: - for target in updated_targets: - if not target.active: - continue - command = command_builder.build_tgtset_from_target_state(target) - if hasattr(self.communicator, 'send_command'): - self.communicator.send_command(command) - elif hasattr(self.communicator, '_send_single_command'): - self.communicator._send_single_command(command) + # Send commands individually for others (Serial) + else: + for command in commands_to_send: + if hasattr(self.communicator, 'send_command'): + self.communicator.send_command(command) + elif hasattr(self.communicator, '_send_single_command'): + self.communicator._send_single_command(command) # --- GUI Update Step (conditional) --- if self.update_queue: @@ -123,6 +119,9 @@ class SimulationEngine(threading.Thread): # --- Loop Control --- time.sleep(TICK_INTERVAL_S) + + self._is_running_event.clear() + self.logger.info("Simulation engine thread stopped.") def pause(self): """Pauses the simulation loop.""" @@ -134,9 +133,6 @@ class SimulationEngine(threading.Thread): self.logger.info("Simulation resumed.") self._is_paused = False - self._is_running_event.clear() - self.logger.info("Simulation engine thread stopped.") - def stop(self): """Signals the simulation thread to stop and waits for it to terminate.""" self.logger.info("Stop signal received for simulation thread.") diff --git a/target_simulator/gui/ppi_display.py b/target_simulator/gui/ppi_display.py index 2db54c6..e5c736e 100644 --- a/target_simulator/gui/ppi_display.py +++ b/target_simulator/gui/ppi_display.py @@ -9,24 +9,14 @@ import tkinter as tk from tkinter import ttk import math import numpy as np -import copy from matplotlib.figure import Figure from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg -from target_simulator.core.models import NM_TO_FT # Use absolute imports -from target_simulator.core.models import Target, Waypoint, ManeuverType -from typing import List, Optional +from target_simulator.core.models import Target, Waypoint, ManeuverType, NM_TO_FT +from typing import List class PPIDisplay(ttk.Frame): - def clear_previews(self): - """Clears all preview artists from the plot.""" - for artist in self.preview_artists: - artist.set_data([], []) - self.canvas.draw() - def _on_motion(self, event): - # Stub: implementazione futura per tooltip/mouseover - pass """A custom, reusable widget for the PPI radar display.""" def __init__(self, master, max_range_nm: int = 100, scan_limit_deg: int = 60): @@ -34,9 +24,11 @@ class PPIDisplay(ttk.Frame): self.max_range = max_range_nm self.scan_limit_deg = scan_limit_deg + # Artists for dynamic target display self.target_artists = [] self.active_targets: List[Target] = [] - self._target_dots = [] + + # Artists for trajectory preview display self.preview_artists = [] self._create_controls() @@ -48,21 +40,14 @@ class PPIDisplay(ttk.Frame): self.controls_frame.pack(side=tk.TOP, fill=tk.X, padx=5, pady=5) ttk.Label(self.controls_frame, text="Range (NM):").pack(side=tk.LEFT) - - # Create a list of valid range steps up to the theoretical max_range - all_steps = [10, 20, 40, 80, 100, 160] - valid_steps = sorted([s for s in all_steps if s <= self.max_range]) - if not valid_steps: - valid_steps = [self.max_range] - # Ensure the initial max range is in the list if not a standard step - if self.max_range not in valid_steps: + all_steps = [10, 20, 40, 80, 100, 160, 240, 320] + valid_steps = sorted([s for s in all_steps if s <= self.max_range]) + if not valid_steps or self.max_range not in valid_steps: valid_steps.append(self.max_range) valid_steps.sort() - - # The initial value for the combobox is the max_range passed to the constructor - self.range_var = tk.IntVar(value=self.max_range) + self.range_var = tk.IntVar(value=self.max_range) self.range_selector = ttk.Combobox( self.controls_frame, textvariable=self.range_var, values=valid_steps, state="readonly", width=5 @@ -73,7 +58,7 @@ class PPIDisplay(ttk.Frame): def _create_plot(self): """Initializes the Matplotlib polar plot.""" fig = Figure(figsize=(5, 5), dpi=100, facecolor='#3E3E3E') - fig.subplots_adjust(left=0.05, right=0.95, top=0.85, bottom=0.05) + 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') @@ -83,302 +68,135 @@ class PPIDisplay(ttk.Frame): self.ax.set_ylim(0, self.range_var.get()) angles = np.arange(0, 360, 30) - labels = [f"{angle}°" if angle == 0 else f"+{angle}°" if angle < 180 else "±180°" if angle == 180 else f"-{360 - angle}°" for angle in angles] + labels = [f"{angle}°" for angle in angles] self.ax.set_thetagrids(angles, 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', y=1.08) + self.ax.set_title("PPI Display", color='white') - # --- Artists for drawing --- - self._start_plot, = self.ax.plot([], [], 'go', markersize=8) - self._waypoints_plot, = self.ax.plot([], [], 'y+', markersize=10, mew=2, linestyle='None') - self._path_plot, = self.ax.plot([], [], 'g--', linewidth=1.5) - self._preview_artist, = self.ax.plot([], [], color="orange", linestyle="--", linewidth=2, alpha=0.7) - self._heading_artist, = self.ax.plot([], [], color='red', linewidth=1, alpha=0.8) - self.preview_artists = [self._start_plot, self._waypoints_plot, self._path_plot, self._preview_artist, self._heading_artist] + # --- Artists for drawing previews --- + self._path_plot, = self.ax.plot([], [], 'g--', linewidth=1.5, label="Path") + self._start_plot, = self.ax.plot([], [], 'go', markersize=8, label="Start") + self._waypoints_plot, = self.ax.plot([], [], 'y+', markersize=10, mew=2, label="Waypoints") + self.preview_artists = [self._path_plot, self._start_plot, self._waypoints_plot] - # --- NEW: Create artists for scan lines --- + # --- Artists for scan lines --- limit_rad = np.deg2rad(self.scan_limit_deg) self._scan_line_1, = self.ax.plot([limit_rad, limit_rad], [0, self.max_range], color='yellow', linestyle='--', linewidth=1) self._scan_line_2, = self.ax.plot([-limit_rad, -limit_rad], [0, self.max_range], color='yellow', linestyle='--', linewidth=1) - self._tooltip_label = None - self.canvas = FigureCanvasTkAgg(fig, master=self) self.canvas.draw() self.canvas.get_tk_widget().pack(side=tk.TOP, fill=tk.BOTH, expand=True) - self.canvas.mpl_connect('motion_notify_event', self._on_motion) - # --- NEW: Initial draw of scan lines --- self._update_scan_lines() def _update_scan_lines(self): - """Updates the length of the scan sector lines to match the current range.""" + """Updates the length and position of the scan sector lines.""" current_range_max = self.ax.get_ylim()[1] - self._scan_line_1.set_ydata([0, current_range_max]) - self._scan_line_2.set_ydata([0, current_range_max]) + limit_rad = np.deg2rad(self.scan_limit_deg) + + self._scan_line_1.set_data([limit_rad, limit_rad], [0, current_range_max]) + self._scan_line_2.set_data([-limit_rad, -limit_rad], [0, current_range_max]) def _on_range_selected(self, event=None): """Handles the selection of a new range.""" new_range = self.range_var.get() self.ax.set_ylim(0, new_range) - - # --- NEW: Update scan lines on zoom change --- self._update_scan_lines() - - self.update_targets(self.active_targets) - self.canvas.draw() + self.canvas.draw_idle() + + def clear_previews(self): + """Clears all preview-related artists from the plot.""" + for artist in self.preview_artists: + artist.set_data([], []) + self.canvas.draw_idle() - # ... il resto dei metodi rimane invariato ... - def update_targets(self, targets: List[Target]): - # (This method is unchanged) + """Updates the display with the current state of active targets.""" self.active_targets = [t for t in targets if t.active] - for artist in self.target_artists: artist.remove() - self.target_artists.clear() - self._target_dots.clear() - vector_len = self.range_var.get() / 25 - for target in self.active_targets: - r = target.current_range_nm - theta = np.deg2rad(target.current_azimuth_deg) - dot, = self.ax.plot(theta, r, 'o', markersize=5, color='red') - self.target_artists.append(dot) - self._target_dots.append((dot, target)) - x1, y1 = r * np.sin(theta), r * np.cos(theta) - h_rad = np.deg2rad(target.current_heading_deg) - dx, dy = vector_len * np.sin(h_rad), vector_len * np.cos(h_rad) - x2, y2 = x1 + dx, y1 + dy - r2, th2 = np.sqrt(x2**2 + y2**2), np.arctan2(x2, y2) - line, = self.ax.plot([theta, th2], [r, r2], color='red', linewidth=1.2) - self.target_artists.append(line) - self.canvas.draw() - def draw_trajectory_preview(self, trajectory): - """ - Simulates and draws a trajectory preview. - Accepts either a list of Waypoint or a Target object. - """ - # Pulisci tutto prima di ridisegnare - self.clear_previews() - self._waypoints_plot.set_data([], []) - self._start_plot.set_data([], []) - if hasattr(self, '_preview_artist'): - self._preview_artist.set_data([], []) - # Rimuovi eventuali artisti extra creati manualmente (puntini/linee) - if hasattr(self, '_preview_extra_artists'): - for a in self._preview_extra_artists: - try: - a.remove() - except Exception: - pass - self._preview_extra_artists.clear() - else: - self._preview_extra_artists = [] - # Forza la pulizia della canvas - self.canvas.draw() - self._on_range_selected() + # Clear previous target artists + for artist in self.target_artists: + artist.remove() + self.target_artists.clear() - # Determine input type - if isinstance(trajectory, list): - self._spline_preview_active = False - waypoints = trajectory - if not waypoints: - self.canvas.draw() - return - if all(getattr(wp, 'maneuver_type', None) != ManeuverType.FLY_TO_POINT for wp in waypoints): - self.canvas.draw() - return - # Classic preview (polyline) SOLO se spline non attiva - # (la preview spline cancella la classica) - # Costruisci la lista dei punti da visualizzare - points = [] - # Stato corrente: range/azimuth - curr_r = None - curr_theta = None - for i, wp in enumerate(waypoints): - if getattr(wp, 'maneuver_type', None) == ManeuverType.FLY_TO_POINT: - curr_r = getattr(wp, 'target_range_nm', 0) - curr_theta = math.radians(getattr(wp, 'target_azimuth_deg', 0)) - points.append((curr_theta, curr_r, wp)) - elif getattr(wp, 'maneuver_type', None) == ManeuverType.FLY_FOR_DURATION: - # Se non c'è punto iniziale, ignora - if curr_r is None or curr_theta is None: - continue - vel_fps = getattr(wp, 'target_velocity_fps', 0) - vel_nmps = vel_fps / NM_TO_FT if vel_fps else 0 - duration = getattr(wp, 'duration_s', 0) - heading_deg = getattr(wp, 'target_heading_deg', 0) - heading_rad = math.radians(heading_deg) - # Calcola delta x/y in coordinate polari - dr = vel_nmps * duration - theta1 = curr_theta + heading_rad - r1 = curr_r + dr - curr_r = r1 - curr_theta = theta1 - points.append((curr_theta, curr_r, wp)) - thetas = [p[0] for p in points] - rs = [p[1] for p in points] - if len(thetas) == 1: - # Mostra solo il punto iniziale con stile simulazione - self._preview_artist.set_data([], []) - self._waypoints_plot.set_data([], []) - self._start_plot.set_data([], []) - wp0 = waypoints[0] - start_theta = thetas[0] - start_r = rs[0] - self._preview_extra_artists = [] - # Puntino rosso - dot, = self.ax.plot([start_theta], [start_r], 'o', markersize=5, color='red') - self._preview_extra_artists.append(dot) - # Linea di heading corta - heading_deg = getattr(wp0, 'target_heading_deg', None) - if heading_deg is not None: - h_rad = math.radians(heading_deg) - vector_len = self.max_range / 25 - x1, y1 = start_r * math.sin(start_theta), start_r * math.cos(start_theta) - dx, dy = vector_len * math.sin(h_rad), vector_len * math.cos(h_rad) - x2, y2 = x1 + dx, y1 + dy - r2, th2 = math.sqrt(x2**2 + y2**2), math.atan2(x2, y2) - line, = self.ax.plot([start_theta, th2], [start_r, r2], color='red', linewidth=1.2) - self._preview_extra_artists.append(line) - self.canvas.draw() - return - if len(thetas) < 2: - self.canvas.draw() - return - # Verifica se la preview è per spline: se sì, non disegnare la linea classica - if hasattr(self, '_spline_preview_active') and self._spline_preview_active: - self._preview_artist.set_data([], []) - else: - self._preview_artist.set_data(thetas, rs) - self._waypoints_plot.set_data(thetas, rs) - start_theta = thetas[0] - start_r = rs[0] - self._start_plot.set_data([start_theta], [start_r]) - # Heading dal primo waypoint - wp0 = points[0][2] - heading_deg = getattr(wp0, 'target_heading_deg', None) - if heading_deg is not None: - h_rad = math.radians(heading_deg) - vector_len = self.max_range / 25 - x1, y1 = start_r * math.sin(start_theta), start_r * math.cos(start_theta) - dx, dy = vector_len * math.sin(h_rad), vector_len * math.cos(h_rad) - x2, y2 = x1 + dx, y1 + dy - r2, th2 = math.sqrt(x2**2 + y2**2), math.atan2(x2, y2) - if hasattr(self, '_heading_artist'): - self._heading_artist.remove() - self._heading_artist, = self.ax.plot([start_theta, th2], [start_r, r2], color='red', linewidth=1.2) - # Heading per ogni punto finale FLY_FOR_DURATION - for i, (theta, r, wp) in enumerate(points): - if i == 0: - continue - if getattr(wp, 'maneuver_type', None) == ManeuverType.FLY_FOR_DURATION: - heading_deg = getattr(wp, 'target_heading_deg', None) - if heading_deg is not None: - h_rad = math.radians(heading_deg) - vector_len = self.max_range / 25 - x1, y1 = r * math.sin(theta), r * math.cos(theta) - dx, dy = vector_len * math.sin(h_rad), vector_len * math.cos(h_rad) - x2, y2 = x1 + dx, y1 + dy - r2, th2 = math.sqrt(x2**2 + y2**2), math.atan2(x2, y2) - self.ax.plot([theta, th2], [r, r2], color='red', linewidth=1.2) - else: - # Assume Target object with trajectory and use_spline - waypoints = getattr(trajectory, "trajectory", []) - if len(waypoints) < 2: - self.canvas.draw() - return - if getattr(trajectory, "use_spline", False): - self._spline_preview_active = True - from target_simulator.utils.spline import catmull_rom_spline - # Costruisci la lista dei punti: waypoint espliciti + punti finali calcolati per FLY_FOR_DURATION - points = [] - curr_r = None - curr_theta = None - for i, wp in enumerate(waypoints): - if getattr(wp, 'maneuver_type', None) == ManeuverType.FLY_TO_POINT: - curr_r = getattr(wp, 'target_range_nm', 0) - curr_theta = math.radians(getattr(wp, 'target_azimuth_deg', 0)) - x_nm = curr_r * math.sin(curr_theta) - y_nm = curr_r * math.cos(curr_theta) - points.append((x_nm, y_nm)) - elif getattr(wp, 'maneuver_type', None) == ManeuverType.FLY_FOR_DURATION: - # Se non c'è punto iniziale, ignora - if curr_r is None or curr_theta is None: - continue - vel_fps = getattr(wp, 'target_velocity_fps', 0) - vel_nmps = vel_fps / NM_TO_FT if vel_fps else 0 - duration = getattr(wp, 'duration_s', 0) - heading_deg = getattr(wp, 'target_heading_deg', 0) - heading_rad = math.radians(heading_deg) - dr = vel_nmps * duration - theta1 = curr_theta + heading_rad - r1 = curr_r + dr - x_nm = r1 * math.sin(theta1) - y_nm = r1 * math.cos(theta1) - points.append((x_nm, y_nm)) - curr_r = r1 - curr_theta = theta1 - if len(points) < 4: - spline_pts = points - else: - try: - spline_pts = catmull_rom_spline(points) - # DEBUG: salva i punti spline in un file CSV - import csv, os - temp_path = os.path.join(os.path.dirname(__file__), '..', '..', 'Temp', 'spline_preview.csv') - with open(temp_path, 'w', newline='') as csvfile: - writer = csv.writer(csvfile) - writer.writerow(['x_nm', 'y_nm']) - for pt in spline_pts: - writer.writerow([pt[0], pt[1]]) - except Exception: - spline_pts = points - # Converte i punti spline da x/y a (theta, r) in NM - spline_thetas = [] - spline_rs = [] - for x_nm, y_nm in spline_pts: - r_nm = (x_nm**2 + y_nm**2)**0.5 - theta = math.atan2(x_nm, y_nm) - spline_thetas.append(theta) - spline_rs.append(r_nm) - self._preview_artist.set_data(spline_thetas, spline_rs) - wp_thetas = [] - wp_rs = [] - for x_nm, y_nm in points: - r_nm = (x_nm**2 + y_nm**2)**0.5 - theta = math.atan2(x_nm, y_nm) - wp_thetas.append(theta) - wp_rs.append(r_nm) - self._waypoints_plot.set_data(wp_thetas, wp_rs) - start_x_nm, start_y_nm = points[0] - start_r_nm = (start_x_nm**2 + start_y_nm**2)**0.5 - start_theta = math.atan2(start_x_nm, start_y_nm) - self._start_plot.set_data([start_theta], [start_r_nm]) - if hasattr(self, '_heading_artist'): - self._heading_artist.remove() - if len(points) > 1: - dx = points[1][0] - points[0][0] - dy = points[1][1] - points[0][1] - norm = (dx**2 + dy**2)**0.5 - if norm > 0: - heading_len = 3 - hx_nm = start_x_nm + heading_len * dx / norm - hy_nm = start_y_nm + heading_len * dy / norm - h_r_nm = (hx_nm**2 + hy_nm**2)**0.5 - h_theta = math.atan2(hx_nm, hy_nm) - self._heading_artist, = self.ax.plot([start_theta, h_theta], [start_r_nm, h_r_nm], color='red', linewidth=1, alpha=0.8) - else: - self._spline_preview_active = False - xs = [wp.x for wp in waypoints] - ys = [wp.y for wp in waypoints] - self._preview_artist, = self.ax.plot(xs, ys, color="orange", linestyle="--", linewidth=2, alpha=0.7) - # ...rimosso blocco duplicato/non valido... - self._update_scan_lines() - self.canvas.draw() + vector_len_nm = self.range_var.get() / 20.0 # Length of heading vector + + for target in self.active_targets: + # Target's polar coordinates + r_nm = target.current_range_nm + theta_rad = np.deg2rad(target.current_azimuth_deg) + + # Draw the target dot + dot, = self.ax.plot(theta_rad, r_nm, 'o', markersize=6, color='red') + self.target_artists.append(dot) + + # Draw the heading vector + heading_rad = np.deg2rad(target.current_heading_deg) + + # Convert to Cartesian to calculate endpoint, then back to polar + x_nm = r_nm * np.sin(theta_rad) + y_nm = r_nm * np.cos(theta_rad) + dx_nm = vector_len_nm * np.sin(heading_rad) + dy_nm = vector_len_nm * np.cos(heading_rad) + + r2_nm = math.sqrt((x_nm + dx_nm)**2 + (y_nm + dy_nm)**2) + theta2_rad = math.atan2(x_nm + dx_nm, y_nm + dy_nm) + + line, = self.ax.plot([theta_rad, theta2_rad], [r_nm, r2_nm], color='red', linewidth=1.2) + self.target_artists.append(line) + + self.canvas.draw_idle() + + def draw_trajectory_preview(self, waypoints: List[Waypoint], use_spline: bool): + """ + Simulates and draws a trajectory preview by leveraging the static path generator. + """ + self.clear_previews() + + if not waypoints or waypoints[0].maneuver_type != ManeuverType.FLY_TO_POINT: + return + + # Use the static method to get the path without creating a Target instance + path, _ = Target.generate_path_from_waypoints(waypoints, use_spline) + + if not path: + return + + # --- Draw the main path (splined or polygonal) --- + path_thetas = [] + path_rs = [] + for point in path: + _time, x_ft, y_ft, _z_ft = point + r_ft = math.sqrt(x_ft**2 + y_ft**2) + theta_rad = math.atan2(x_ft, y_ft) + path_rs.append(r_ft / NM_TO_FT) + path_thetas.append(theta_rad) + + self._path_plot.set_data(path_thetas, path_rs) + + # --- Draw waypoint markers (only for Fly to Point) --- + 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 + theta_rad = math.radians(wp.target_azimuth_deg or 0.0) + wp_rs.append(r_nm) + wp_thetas.append(theta_rad) + + self._waypoints_plot.set_data(wp_thetas, wp_rs) + + # --- Draw the start point --- + start_wp = waypoints[0] + start_r = start_wp.target_range_nm or 0.0 + start_theta = math.radians(start_wp.target_azimuth_deg or 0.0) + self._start_plot.set_data([start_theta], [start_r]) + self.canvas.draw_idle() def reconfigure_radar(self, max_range_nm: int, scan_limit_deg: int): """ @@ -388,20 +206,20 @@ class PPIDisplay(ttk.Frame): self.scan_limit_deg = scan_limit_deg # Update the range combobox values - steps = [10, 20, 40, 80, 100, 160] + steps = [10, 20, 40, 80, 100, 160, 240, 320] valid_steps = sorted([s for s in steps if s <= self.max_range]) - if not valid_steps: valid_steps = [self.max_range] - if self.max_range not in valid_steps: + if not valid_steps or self.max_range not in valid_steps: valid_steps.append(self.max_range) valid_steps.sort() + current_range = self.range_var.get() self.range_selector['values'] = valid_steps - self.range_var.set(self.max_range) # Set to the new max range - - # Update the scan limit lines - limit_rad = np.deg2rad(self.scan_limit_deg) - self._scan_line_1.set_xdata([limit_rad, limit_rad]) - self._scan_line_2.set_xdata([-limit_rad, -limit_rad]) + + # If the current range is still valid, keep it. Otherwise, reset to max. + if current_range in valid_steps: + self.range_var.set(current_range) + else: + self.range_var.set(self.max_range) # Apply the new range and redraw everything self._on_range_selected() \ No newline at end of file diff --git a/target_simulator/gui/trajectory_editor_window.py b/target_simulator/gui/trajectory_editor_window.py index 97d95f8..f7c845c 100644 --- a/target_simulator/gui/trajectory_editor_window.py +++ b/target_simulator/gui/trajectory_editor_window.py @@ -1,15 +1,17 @@ +# target_simulator/gui/trajectory_editor_window.py + import tkinter as tk from tkinter import ttk, messagebox import math -from typing import List, Optional +from typing import List, Optional, cast import copy -from queue import Queue, Empty +from queue import Queue # Use absolute imports from target_simulator.core.models import Target, Waypoint, ManeuverType, MIN_TARGET_ID, MAX_TARGET_ID, FPS_TO_KNOTS, Scenario from target_simulator.gui.ppi_display import PPIDisplay from target_simulator.gui.waypoint_editor_window import WaypointEditorWindow -from target_simulator.core.simulation_engine import SimulationEngine +from target_simulator.core.simulation_engine import SimulationEngine, TICK_INTERVAL_S GUI_QUEUE_POLL_INTERVAL_MS = 50 @@ -29,21 +31,21 @@ class TrajectoryEditorWindow(tk.Toplevel): self.waypoints: List[Waypoint] = [] is_editing = target_to_edit is not None if is_editing: - self.target_id = target_to_edit.target_id - self.waypoints = copy.deepcopy(target_to_edit.trajectory) - self.use_spline_var = tk.BooleanVar(value=bool(getattr(target_to_edit, 'use_spline', False))) + # Ensure we are working with a valid Target object + target = cast(Target, target_to_edit) + self.target_id = target.target_id + self.waypoints = copy.deepcopy(target.trajectory) + self.use_spline_var = tk.BooleanVar(value=target.use_spline) else: self.target_id = next((i for i in range(MIN_TARGET_ID, MAX_TARGET_ID + 1) if i not in existing_ids), -1) self.use_spline_var = tk.BooleanVar(value=False) - # Accept scenario_name as argument, fallback to default if not provided - if scenario_name is None: - scenario_name = f"Trajectory_{self.target_id}" - self.title(f"Trajectory Editor - {scenario_name} - Target {self.target_id}") + scenario_name_str = scenario_name or f"Target_{self.target_id}" + self.title(f"Trajectory Editor - {scenario_name_str}") self.geometry("1100x700") - self.gui_update_queue = Queue() + self.gui_update_queue: Queue = Queue() self.preview_engine: Optional[SimulationEngine] = None self.is_preview_running = tk.BooleanVar(value=False) @@ -53,33 +55,10 @@ class TrajectoryEditorWindow(tk.Toplevel): self._create_initial_waypoint() self._populate_waypoint_list() - # Aggiorna la preview in base allo stato della checkbox spline self.after(10, self._update_static_preview) - # Se c'è almeno un waypoint, mostra subito la preview del punto iniziale - if self.waypoints: - self.ppi_preview.clear_previews() - if self.use_spline_var.get(): - temp_target = Target( - target_id=self.target_id, - trajectory=copy.deepcopy(self.waypoints), - use_spline=True - ) - temp_target.reset_simulation() - self.ppi_preview.draw_trajectory_preview(temp_target) - else: - self.ppi_preview.draw_trajectory_preview([self.waypoints[0]]) - self.ppi_preview.canvas.draw() - #self._center_window() self.protocol("WM_DELETE_WINDOW", self._on_cancel) self.wait_window(self) - - def _center_window(self): - self.update_idletasks() - parent = self.master.winfo_toplevel() - x = parent.winfo_x() + (parent.winfo_width() // 2) - (self.winfo_width() // 2) - y = parent.winfo_y() + (parent.winfo_height() // 2) - (self.winfo_height() // 2) - self.geometry(f"+{x}+{y}") def _create_widgets(self): main_pane = ttk.PanedWindow(self, orient=tk.HORIZONTAL) @@ -89,101 +68,76 @@ class TrajectoryEditorWindow(tk.Toplevel): main_pane.add(left_frame, weight=1) list_frame = ttk.LabelFrame(left_frame, text="Trajectory Waypoints") - list_frame.pack(fill=tk.BOTH, expand=True) + list_frame.pack(fill=tk.BOTH, expand=True, pady=(0, 5)) self._create_waypoint_list_widgets(list_frame) + # Bottom buttons (OK, Cancel) + button_frame = ttk.Frame(left_frame) + button_frame.pack(fill=tk.X, side=tk.BOTTOM, pady=(5, 0)) + ttk.Button(button_frame, text="OK", command=self._on_ok).pack(side=tk.RIGHT, padx=5) + ttk.Button(button_frame, text="Cancel", command=self._on_cancel).pack(side=tk.RIGHT) + + # Preview Pane preview_frame = ttk.LabelFrame(main_pane, text="Trajectory Preview") main_pane.add(preview_frame, weight=2) - - self.ppi_preview = PPIDisplay(preview_frame, max_range_nm=self.initial_max_range) - self.ppi_preview.pack(fill=tk.BOTH, expand=True) - - preview_controls = ttk.Frame(preview_frame) - preview_controls.pack(fill=tk.X, pady=5) - self.play_button = ttk.Button(preview_controls, text="▶ Play", command=self._on_preview_play) - self.play_button.pack(side=tk.LEFT, padx=5) - self.pause_button = ttk.Button(preview_controls, text="⏸ Pause", command=self._on_preview_pause, state=tk.DISABLED) - self.pause_button.pack(side=tk.LEFT, padx=5) - self.stop_button = ttk.Button(preview_controls, text="■ Stop", command=self._on_preview_stop, state=tk.DISABLED) - self.stop_button.pack(side=tk.LEFT, padx=5) - self.reset_button = ttk.Button(preview_controls, text="⟲ Reset", command=self._on_preview_reset) - self.reset_button.pack(side=tk.LEFT, padx=5) - # ...existing code... - ttk.Label(preview_controls, text="Speed:").pack(side=tk.LEFT, padx=(10, 2), pady=5) - self.time_multiplier_var = tk.StringVar(value="1x") - self.multiplier_combo = ttk.Combobox(preview_controls, textvariable=self.time_multiplier_var, values=["1x", "2x", "4x", "10x", "20x"], state="readonly", width=4) - self.multiplier_combo.pack(side=tk.LEFT, padx=(0, 5), pady=5) - self.multiplier_combo.bind("<>", self._on_time_multiplier_changed) - - # --- Progress Bar --- - progress_frame = ttk.Frame(preview_frame) - progress_frame.pack(fill=tk.X, pady=(5, 0)) - self.sim_progress_var = tk.DoubleVar(value=0.0) - self.sim_progress = ttk.Scale(progress_frame, variable=self.sim_progress_var, from_=0.0, to=1.0, orient=tk.HORIZONTAL, command=self._on_progress_seek) - self.sim_progress.pack(side=tk.LEFT, fill=tk.X, expand=True, padx=5) - self.sim_time_label = ttk.Label(progress_frame, text="0.0s / 0.0s") - self.sim_time_label.pack(side=tk.RIGHT, padx=5) - - button_frame = ttk.Frame(self) - button_frame.pack(fill=tk.X, padx=10, pady=(0, 10), side=tk.BOTTOM) - ttk.Button(button_frame, text="Cancel", command=self._on_cancel).pack(side=tk.RIGHT) - ttk.Button(button_frame, text="OK", command=self._on_ok).pack(side=tk.RIGHT, padx=5) - def _on_spline_toggle(self): - # Conta i punti effettivi (waypoint + calcolati) - waypoints = self.waypoints - points = [] - curr_r = None - curr_theta = None - from target_simulator.core.models import ManeuverType, NM_TO_FT - for i, wp in enumerate(waypoints): - if getattr(wp, 'maneuver_type', None) == ManeuverType.FLY_TO_POINT: - curr_r = getattr(wp, 'target_range_nm', 0) - curr_theta = math.radians(getattr(wp, 'target_azimuth_deg', 0)) - points.append((curr_r, curr_theta)) - elif getattr(wp, 'maneuver_type', None) == ManeuverType.FLY_FOR_DURATION: - if curr_r is None or curr_theta is None: - continue - vel_fps = getattr(wp, 'target_velocity_fps', 0) - vel_nmps = vel_fps / NM_TO_FT if vel_fps else 0 - duration = getattr(wp, 'duration_s', 0) - heading_deg = getattr(wp, 'target_heading_deg', 0) - heading_rad = math.radians(heading_deg) - dr = vel_nmps * duration - theta1 = curr_theta + heading_rad - r1 = curr_r + dr - points.append((r1, theta1)) - curr_r = r1 - curr_theta = theta1 - if len(points) < 4: - self.use_spline_var.set(False) - messagebox.showinfo("Spline not available", "Spline mode requires at least 4 points (explicit or calculated waypoints).") - return - self._update_static_preview() - # Aggiorna la label tempo simulazione all'apertura - total_time = self._get_total_simulation_time() - self.sim_time_label.config(text=f"0.0s / {total_time:.1f}s") + self._create_preview_widgets(preview_frame) def _create_waypoint_list_widgets(self, parent): - # Checkbox per spline sotto i tasti della tabella - self.spline_checkbox = ttk.Checkbutton(parent, text="Use Splined Trajectory", variable=self.use_spline_var, command=self._on_spline_toggle) - self.spline_checkbox.pack(fill=tk.X, padx=5, pady=(5, 0)) - self.wp_tree = ttk.Treeview(parent, columns=("num", "type", "details"), show="headings", height=10) self.wp_tree.heading("num", text="#") self.wp_tree.heading("type", text="Maneuver") self.wp_tree.heading("details", text="Parameters") - self.wp_tree.column("num", width=30, anchor=tk.CENTER) - self.wp_tree.column("type", width=120) + self.wp_tree.column("num", width=30, anchor=tk.CENTER, stretch=False) + self.wp_tree.column("type", width=120, stretch=False) self.wp_tree.column("details", width=250) - self.wp_tree.pack(side=tk.TOP, fill=tk.BOTH, expand=True) + + scrollbar = ttk.Scrollbar(parent, orient=tk.VERTICAL, command=self.wp_tree.yview) + self.wp_tree.configure(yscrollcommand=scrollbar.set) + + self.wp_tree.grid(row=0, column=0, sticky="nsew") + scrollbar.grid(row=0, column=1, sticky="ns") + parent.grid_rowconfigure(0, weight=1) + parent.grid_columnconfigure(0, weight=1) + self.wp_tree.bind('', self._on_edit_waypoint) btn_frame = ttk.Frame(parent) - btn_frame.pack(fill=tk.X, pady=5) - ttk.Button(btn_frame, text="Add Waypoint", command=self._on_add_waypoint).pack(side=tk.LEFT, padx=2) - ttk.Button(btn_frame, text="Edit Waypoint", command=self._on_edit_waypoint).pack(side=tk.LEFT, padx=2) - ttk.Button(btn_frame, text="Remove Waypoint", command=self._on_remove_waypoint).pack(side=tk.LEFT, padx=2) - + btn_frame.grid(row=1, column=0, columnspan=2, sticky="ew", pady=(5,0)) + + ttk.Button(btn_frame, text="Add", command=self._on_add_waypoint).pack(side=tk.LEFT, padx=(0,2)) + ttk.Button(btn_frame, text="Edit", command=self._on_edit_waypoint).pack(side=tk.LEFT, padx=2) + ttk.Button(btn_frame, text="Remove", command=self._on_remove_waypoint).pack(side=tk.LEFT, padx=2) + + self.spline_checkbox = ttk.Checkbutton(btn_frame, text="Use Spline", variable=self.use_spline_var, command=self._on_spline_toggle) + self.spline_checkbox.pack(side=tk.RIGHT, padx=5) + + def _create_preview_widgets(self, parent): + self.ppi_preview = PPIDisplay(parent, max_range_nm=self.initial_max_range) + self.ppi_preview.pack(fill=tk.BOTH, expand=True) + + preview_controls = ttk.Frame(parent) + preview_controls.pack(fill=tk.X, pady=5) + + self.play_button = ttk.Button(preview_controls, text="▶ Play", command=self._on_preview_play) + self.play_button.pack(side=tk.LEFT, padx=5) + + self.stop_button = ttk.Button(preview_controls, text="■ Stop", command=self._on_preview_stop, state=tk.DISABLED) + self.stop_button.pack(side=tk.LEFT, padx=5) + + ttk.Label(preview_controls, text="Speed:").pack(side=tk.LEFT, padx=(10, 2)) + self.time_multiplier_var = tk.StringVar(value="1x") + self.multiplier_combo = ttk.Combobox(preview_controls, textvariable=self.time_multiplier_var, values=["1x", "2x", "4x", "10x", "20x"], state="readonly", width=4) + self.multiplier_combo.pack(side=tk.LEFT) + self.multiplier_combo.bind("<>", self._on_time_multiplier_changed) + + def _on_spline_toggle(self): + # A simple spline requires at least 4 waypoints to be meaningful. + if self.use_spline_var.get() and len(self.waypoints) < 4: + self.use_spline_var.set(False) + messagebox.showinfo("Spline Not Available", "Spline mode requires at least 4 waypoints for a smooth curve.", parent=self) + return + self._update_static_preview() + def _create_initial_waypoint(self): """Forces the user to create the first waypoint (initial position).""" editor = WaypointEditorWindow(self, is_first_waypoint=True) @@ -194,16 +148,17 @@ class TrajectoryEditorWindow(tk.Toplevel): self.after(10, self._on_cancel) def _populate_waypoint_list(self): + """Refreshes the treeview with the current list of waypoints.""" self.wp_tree.delete(*self.wp_tree.get_children()) for i, wp in enumerate(self.waypoints): details = "" if wp.maneuver_type == ManeuverType.FLY_TO_POINT: - details = f"R:{wp.target_range_nm:.1f}, Az:{wp.target_azimuth_deg:.1f}, Alt:{wp.target_altitude_ft:.0f}, T:{wp.duration_s:.1f}s" + details = f"R:{wp.target_range_nm:.1f}, Az:{wp.target_azimuth_deg:.1f}, Alt:{wp.target_altitude_ft:.0f}" elif wp.maneuver_type == ManeuverType.FLY_FOR_DURATION: vel_kn = (wp.target_velocity_fps or 0.0) * FPS_TO_KNOTS details = f"Vel:{vel_kn:.1f}kn, Hdg:{wp.target_heading_deg:.1f}°, T:{wp.duration_s:.1f}s" - self.wp_tree.insert("", tk.END, iid=str(i), values=(i, wp.maneuver_type.value, details)) + self.wp_tree.insert("", tk.END, iid=str(i), values=(i + 1, wp.maneuver_type.value, details)) def _on_add_waypoint(self): editor = WaypointEditorWindow(self, is_first_waypoint=False) @@ -211,28 +166,30 @@ class TrajectoryEditorWindow(tk.Toplevel): self.waypoints.append(editor.result_waypoint) self._populate_waypoint_list() self._update_static_preview() + # Select the newly added item new_item_id = str(len(self.waypoints) - 1) - self.wp_tree.focus(new_item_id); self.wp_tree.selection_set(new_item_id) + self.wp_tree.see(new_item_id) + self.wp_tree.focus(new_item_id) + self.wp_tree.selection_set(new_item_id) def _on_edit_waypoint(self, event=None): selected_item = self.wp_tree.focus() if not selected_item: - if not event: + if not event: # Only show warning on button click messagebox.showwarning("No Selection", "Please select a waypoint to edit.", parent=self) return wp_index = int(selected_item) - waypoint_to_edit = self.waypoints[wp_index] - is_first = (wp_index == 0) - - # Passa l'indice come parametro - editor = WaypointEditorWindow(self, is_first_waypoint=is_first, waypoint_to_edit=waypoint_to_edit, waypoint_index=wp_index) + editor = WaypointEditorWindow(self, + is_first_waypoint=(wp_index == 0), + waypoint_to_edit=self.waypoints[wp_index]) if editor.result_waypoint: self.waypoints[wp_index] = editor.result_waypoint self._populate_waypoint_list() self._update_static_preview() - self.wp_tree.focus(selected_item); self.wp_tree.selection_set(selected_item) + self.wp_tree.focus(selected_item) + self.wp_tree.selection_set(selected_item) def _on_remove_waypoint(self): selected_item = self.wp_tree.focus() @@ -240,12 +197,11 @@ class TrajectoryEditorWindow(tk.Toplevel): messagebox.showwarning("No Selection", "Please select a waypoint to remove.", parent=self) return - wp_index = int(selected_item) - # Prevent deleting the last waypoint, which defines the start position if len(self.waypoints) == 1: - messagebox.showerror("Action Denied", "Cannot remove the initial position waypoint. Edit it instead.", parent=self) + messagebox.showerror("Action Denied", "Cannot remove the initial position waypoint.", parent=self) return + wp_index = int(selected_item) del self.waypoints[wp_index] self._populate_waypoint_list() self._update_static_preview() @@ -254,22 +210,11 @@ class TrajectoryEditorWindow(tk.Toplevel): """Draws the static trajectory path on the PPI.""" if self.is_preview_running.get(): self._on_preview_stop() - self.ppi_preview.clear_previews() - self.ppi_preview.update_targets([]) - - # Passa direttamente la lista o il Target in base alla flag spline - if self.use_spline_var.get(): - temp_target = Target( - target_id=self.target_id, - trajectory=copy.deepcopy(self.waypoints), - use_spline=True - ) - temp_target.reset_simulation() - self.ppi_preview.draw_trajectory_preview(temp_target) - else: - self.ppi_preview.draw_trajectory_preview(copy.deepcopy(self.waypoints)) - # Forza l'aggiornamento della canvas - self.ppi_preview.canvas.draw() + + self.ppi_preview.draw_trajectory_preview( + waypoints=copy.deepcopy(self.waypoints), + use_spline=self.use_spline_var.get() + ) def _on_time_multiplier_changed(self, event=None): """Handles changes to the time multiplier selection.""" @@ -284,17 +229,16 @@ class TrajectoryEditorWindow(tk.Toplevel): def _on_preview_play(self): if self.is_preview_running.get(): return if not self.waypoints or self.waypoints[0].maneuver_type != ManeuverType.FLY_TO_POINT: - messagebox.showinfo("Incomplete Trajectory", "Add a 'Fly to Point' waypoint as the starting position.", parent=self) + messagebox.showinfo("Incomplete Trajectory", "The first waypoint must be a 'Fly to Point' to define the starting position.", parent=self) return self.is_preview_running.set(True) self._update_preview_button_states() - if self.use_spline_var.get(): - preview_target = Target(target_id=self.target_id, trajectory=copy.deepcopy(self.waypoints), use_spline=True) - else: - preview_target = Target(target_id=self.target_id, trajectory=copy.deepcopy(self.waypoints), use_spline=False) - preview_scenario = Scenario(name=f"Trajectory_{self.target_id}") + preview_target = Target(target_id=self.target_id, + trajectory=copy.deepcopy(self.waypoints), + use_spline=self.use_spline_var.get()) + preview_scenario = Scenario(name=f"Preview_{self.target_id}") preview_scenario.add_target(preview_target) self.preview_engine = SimulationEngine(communicator=None, update_queue=self.gui_update_queue) @@ -306,53 +250,45 @@ class TrajectoryEditorWindow(tk.Toplevel): def _on_preview_stop(self): if not self.is_preview_running.get() or not self.preview_engine: return + self.preview_engine.stop() self.preview_engine = None self.is_preview_running.set(False) self._update_preview_button_states() - self.ppi_preview.update_targets([]) - - def _on_preview_reset(self): - if self.is_preview_running.get(): self._on_preview_stop() + + # After stopping, reset the preview to the static path self._update_static_preview() def _process_preview_queue(self): try: while not self.gui_update_queue.empty(): - updated_targets: List[Target] = self.gui_update_queue.get_nowait() - self.ppi_preview.update_targets(updated_targets) - # Aggiorna la progress bar e la label tempo - if self.preview_engine and self.preview_engine.scenario: - # Prendi il target simulato - targets = self.preview_engine.scenario.get_all_targets() - if targets: - t = targets[0] - # Usa l'attributo _sim_time_s del target - sim_time = getattr(t, '_sim_time_s', 0.0) - total_time = self._get_total_simulation_time() - progress = min(sim_time / total_time, 1.0) if total_time > 0 else 0.0 - self.sim_progress_var.set(progress) - self.sim_time_label.config(text=f"{sim_time:.1f}s / {total_time:.1f}s") - # Se la simulazione è finita (tutti i waypoint raggiunti), ferma la preview e aggiorna i tasti - if self.preview_engine and hasattr(self.preview_engine, 'scenario') and self.preview_engine.scenario: - if hasattr(self.preview_engine.scenario, 'is_finished') and self.preview_engine.scenario.is_finished: - self._on_preview_stop() + update = self.gui_update_queue.get_nowait() + if update == 'SIMULATION_FINISHED': + self._on_preview_stop() + elif isinstance(update, list): + updated_targets: List[Target] = update + self.ppi_preview.update_targets(updated_targets) finally: if self.is_preview_running.get(): self.after(GUI_QUEUE_POLL_INTERVAL_MS, self._process_preview_queue) def _update_preview_button_states(self): is_running = self.is_preview_running.get() - state = tk.DISABLED if is_running else tk.NORMAL self.play_button.config(state=tk.DISABLED if is_running else tk.NORMAL) self.stop_button.config(state=tk.NORMAL if is_running else tk.DISABLED) self.multiplier_combo.config(state="readonly" if not is_running else tk.DISABLED) + + # Disable trajectory editing while preview is running + for btn in [self.wp_tree.master.children[f] for f in self.wp_tree.master.children if isinstance(self.wp_tree.master.children[f], ttk.Button)]: + btn.config(state = tk.DISABLED if is_running else tk.NORMAL) + self.spline_checkbox.config(state=tk.DISABLED if is_running else tk.NORMAL) self.wp_tree.config(selectmode="browse" if not is_running else "none") + def _on_ok(self): if not self.waypoints or self.waypoints[0].maneuver_type != ManeuverType.FLY_TO_POINT: - messagebox.showerror("Invalid Trajectory", "The first waypoint must be of type 'Fly to Point' to define a starting position.", parent=self) + messagebox.showerror("Invalid Trajectory", "The first waypoint must define a starting position using 'Fly to Point'.", parent=self) return try: @@ -361,37 +297,12 @@ class TrajectoryEditorWindow(tk.Toplevel): trajectory=self.waypoints, use_spline=self.use_spline_var.get() ) - self.destroy() # Chiamiamo destroy() invece di _on_cancel() per evitare di impostare result_target a None + self.destroy() except (ValueError, tk.TclError) as e: messagebox.showerror("Validation Error", str(e), parent=self) def _on_cancel(self): - if self.is_preview_running.get(): self._on_preview_stop() + if self.is_preview_running.get(): + self._on_preview_stop() self.result_target = None - self.destroy() - - def _on_preview_pause(self): - # Pausa la simulazione preview - if self.preview_engine and self.is_preview_running.get(): - self.preview_engine.pause() - self.is_preview_running.set(False) - self._update_preview_button_states() - - - def _on_progress_seek(self, value): - # Sposta la simulazione al tempo selezionato - total_time = self._get_total_simulation_time() - seek_time = float(value) * total_time - if self.preview_engine: - self.preview_engine.set_simulation_time(seek_time) - # Aggiorna la preview grafica - updated_targets = self.preview_engine.scenario.get_all_targets() if self.preview_engine.scenario else [] - self.ppi_preview.update_targets(updated_targets) - self.sim_time_label.config(text=f"{seek_time:.1f}s / {total_time:.1f}s") - - def _get_total_simulation_time(self): - # Calcola la durata totale della simulazione sommando le durate dei waypoint - total = 0.0 - for wp in self.waypoints: - total += wp.duration_s or 0.0 - return total \ No newline at end of file + self.destroy() \ No newline at end of file