refactoring di preview e simulazione usando traiettorie calcolate
This commit is contained in:
VALLONGOL
parent
288c4f73be
commit
1575c1b38a
@ -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
|
||||
|
||||
|
@ -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
|
||||
}
|
||||
]
|
||||
}
|
||||
|
||||
@ -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"]
|
||||
|
||||
@ -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
|
||||
target_velocity_fps: Optional[float] = None
|
||||
target_heading_deg: Optional[float] = None
|
||||
|
||||
# Parameters used by both
|
||||
# Parameters used by both maneuver types
|
||||
duration_s: Optional[float] = 10.0
|
||||
target_altitude_ft: Optional[float] = 10000.0
|
||||
|
||||
# Parameters used by FLY_TO_POINT
|
||||
# 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
|
||||
|
||||
# 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:
|
||||
from target_simulator.utils.spline import catmull_rom_spline
|
||||
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]
|
||||
|
||||
# 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])
|
||||
# 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]
|
||||
|
||||
if not pts:
|
||||
self._pos_x_ft = self._pos_y_ft = self._pos_z_ft = 0.0
|
||||
self.current_velocity_fps = 0.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._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]
|
||||
|
||||
# Calculate total duration from waypoints
|
||||
total_duration = sum(wp.duration_s or 0.0 for wp in self.trajectory if wp.duration_s)
|
||||
|
||||
# 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))]
|
||||
self.current_pitch_deg = 0.0
|
||||
self.current_velocity_fps = 0.0
|
||||
else:
|
||||
self._spline_point_times = [0.0]
|
||||
|
||||
self._update_current_polar_coords()
|
||||
|
||||
else:
|
||||
# Waypoint-based simulation reset
|
||||
if not self.trajectory or self.trajectory[0].maneuver_type != ManeuverType.FLY_TO_POINT:
|
||||
# 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
|
||||
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()
|
||||
self.active = bool(self.trajectory) # A target is active if it has a trajectory
|
||||
|
||||
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
|
||||
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)
|
||||
|
||||
if self._current_waypoint_index >= len(self.trajectory):
|
||||
self._current_waypoint_index = -1; return
|
||||
@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
|
||||
|
||||
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
|
||||
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)
|
||||
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
|
||||
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
|
||||
|
||||
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
|
||||
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
|
||||
|
||||
points_to_spline = [kf[1:] for kf in keyframes]
|
||||
num_spline_points = int(total_duration_s * 10)
|
||||
|
||||
splined_points = catmull_rom_spline(points_to_spline, num_points=max(num_spline_points, 100))
|
||||
|
||||
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:
|
||||
self.current_pitch_deg = 0
|
||||
path = keyframes
|
||||
|
||||
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
|
||||
current_sim_time = min(self._sim_time_s, self._total_duration_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.active = False
|
||||
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
|
||||
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))
|
||||
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
|
||||
|
||||
pt = self._spline_path[self._spline_index]
|
||||
segment_duration = p2[0] - p1[0]
|
||||
progress = (current_sim_time - p1[0]) / segment_duration if segment_duration > 0 else 1.0
|
||||
|
||||
# Determine previous point for calculating heading/pitch
|
||||
prev_pt = self._spline_path[self._spline_index - 1] if self._spline_index > 0 else pt
|
||||
prev_x, prev_y, prev_z = self._pos_x_ft, self._pos_y_ft, self._pos_z_ft
|
||||
|
||||
# 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]
|
||||
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
|
||||
|
||||
# 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]
|
||||
dx = self._pos_x_ft - prev_x
|
||||
dy = self._pos_y_ft - prev_y
|
||||
dz = self._pos_z_ft - prev_z
|
||||
|
||||
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))
|
||||
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_since_last_tick / delta_time_s
|
||||
self.current_velocity_fps = dist_moved_3d / delta_time_s
|
||||
|
||||
self._update_current_polar_coords()
|
||||
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))
|
||||
|
||||
# --- 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()
|
||||
|
||||
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,
|
||||
|
||||
@ -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,35 +79,32 @@ 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:
|
||||
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) ---
|
||||
# Send commands individually for others (Serial)
|
||||
else:
|
||||
for target in updated_targets:
|
||||
if not target.active:
|
||||
continue
|
||||
command = command_builder.build_tgtset_from_target_state(target)
|
||||
for command in commands_to_send:
|
||||
if hasattr(self.communicator, 'send_command'):
|
||||
self.communicator.send_command(command)
|
||||
elif hasattr(self.communicator, '_send_single_command'):
|
||||
@ -124,6 +120,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."""
|
||||
self.logger.info("Simulation paused.")
|
||||
@ -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.")
|
||||
|
||||
@ -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()
|
||||
@ -49,20 +41,13 @@ class PPIDisplay(ttk.Frame):
|
||||
|
||||
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]
|
||||
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:
|
||||
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:
|
||||
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_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.canvas.draw_idle()
|
||||
|
||||
self.update_targets(self.active_targets)
|
||||
self.canvas.draw()
|
||||
|
||||
# ... il resto dei metodi rimane invariato ...
|
||||
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()
|
||||
|
||||
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()
|
||||
|
||||
# Clear previous target artists
|
||||
for artist in self.target_artists:
|
||||
artist.remove()
|
||||
self.target_artists.clear()
|
||||
self._target_dots.clear()
|
||||
vector_len = self.range_var.get() / 25
|
||||
|
||||
vector_len_nm = self.range_var.get() / 20.0 # Length of heading vector
|
||||
|
||||
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')
|
||||
# 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)
|
||||
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)
|
||||
|
||||
# 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()
|
||||
|
||||
def draw_trajectory_preview(self, trajectory):
|
||||
self.canvas.draw_idle()
|
||||
|
||||
def draw_trajectory_preview(self, waypoints: List[Waypoint], use_spline: bool):
|
||||
"""
|
||||
Simulates and draws a trajectory preview.
|
||||
Accepts either a list of Waypoint or a Target object.
|
||||
Simulates and draws a trajectory preview by leveraging the static path generator.
|
||||
"""
|
||||
# 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()
|
||||
|
||||
# Determine input type
|
||||
if isinstance(trajectory, list):
|
||||
self._spline_preview_active = False
|
||||
waypoints = trajectory
|
||||
if not waypoints:
|
||||
self.canvas.draw()
|
||||
if not waypoints or waypoints[0].maneuver_type != ManeuverType.FLY_TO_POINT:
|
||||
return
|
||||
if all(getattr(wp, 'maneuver_type', None) != ManeuverType.FLY_TO_POINT for wp in waypoints):
|
||||
self.canvas.draw()
|
||||
|
||||
# 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
|
||||
# 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)
|
||||
|
||||
# --- 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 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)
|
||||
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)
|
||||
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()
|
||||
|
||||
# --- 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()
|
||||
@ -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,34 +55,11 @@ 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)
|
||||
main_pane.pack(fill=tk.BOTH, expand=True, padx=10, pady=10)
|
||||
@ -89,100 +68,75 @@ 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("<<ComboboxSelected>>", 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('<Double-1>', 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("<<ComboboxSelected>>", 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)."""
|
||||
@ -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
|
||||
self.ppi_preview.draw_trajectory_preview(
|
||||
waypoints=copy.deepcopy(self.waypoints),
|
||||
use_spline=self.use_spline_var.get()
|
||||
)
|
||||
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()
|
||||
|
||||
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:
|
||||
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
|
||||
Loading…
Reference in New Issue
Block a user