From 747ec3b40f84a66c0a36495c217c53bdf47be4e5 Mon Sep 17 00:00:00 2001 From: VALLONGOL Date: Mon, 27 Oct 2025 14:44:52 +0100 Subject: [PATCH] sistemata simulazione con server, target che segue la simulazione --- scenarios.json | 3 +- settings.json | 2 +- .../analysis/simulation_state_hub.py | 20 +++ target_simulator/core/models.py | 2 +- target_simulator/core/simulation_engine.py | 61 ++++--- target_simulator/gui/main_view.py | 59 +++--- target_simulator/gui/payload_router.py | 6 +- target_simulator/gui/ppi_display.py | 168 ++++++++++-------- .../gui/trajectory_editor_window.py | 2 +- tests/gui/test_trajectory_editor_window.py | 2 +- 10 files changed, 184 insertions(+), 141 deletions(-) diff --git a/scenarios.json b/scenarios.json index eb2ce9a..0f04c34 100644 --- a/scenarios.json +++ b/scenarios.json @@ -163,4 +163,5 @@ } ] } -} \ No newline at end of file +} + diff --git a/settings.json b/settings.json index 0769468..759cbdb 100644 --- a/settings.json +++ b/settings.json @@ -3,7 +3,7 @@ "scan_limit": 60, "max_range": 100, "geometry": "1599x1024+501+84", - "last_selected_scenario": "scenario2", + "last_selected_scenario": "scenario1", "connection": { "target": { "type": "sfp", diff --git a/target_simulator/analysis/simulation_state_hub.py b/target_simulator/analysis/simulation_state_hub.py index 53c930f..7be38e6 100644 --- a/target_simulator/analysis/simulation_state_hub.py +++ b/target_simulator/analysis/simulation_state_hub.py @@ -193,4 +193,24 @@ class SimulationStateHub: if tid in self._latest_raw_heading: del self._latest_raw_heading[tid] except Exception: + pass + + def clear_real_target_data(self, target_id: int): + """ + Clears only the real data history and heading caches for a specific target, + preserving the simulated data history for analysis. + """ + with self._lock: + try: + tid = int(target_id) + if tid in self._target_data: + self._target_data[tid]["real"].clear() + + # Also clear heading caches associated with this real target + if tid in self._latest_real_heading: + del self._latest_real_heading[tid] + if tid in self._latest_raw_heading: + del self._latest_raw_heading[tid] + except Exception: + # Silently ignore errors (e.g., invalid target_id type) pass \ No newline at end of file diff --git a/target_simulator/core/models.py b/target_simulator/core/models.py index 5a3981d..b59e43e 100644 --- a/target_simulator/core/models.py +++ b/target_simulator/core/models.py @@ -349,7 +349,7 @@ class Target: # Positive azimuth increases counter-clockwise (to the left), # so compute atan2(x, y) and negate the result to match that # convention (i.e. east becomes -90°, west becomes +90°). - azimuth_deg = -math.degrees(math.atan2(self._pos_x_ft, self._pos_y_ft)) + azimuth_deg = math.degrees(math.atan2(self._pos_x_ft, self._pos_y_ft)) # Normalize angle to [-180, 180] while azimuth_deg > 180: diff --git a/target_simulator/core/simulation_engine.py b/target_simulator/core/simulation_engine.py index 5f62f82..de82b55 100644 --- a/target_simulator/core/simulation_engine.py +++ b/target_simulator/core/simulation_engine.py @@ -111,17 +111,21 @@ class SimulationEngine(threading.Thread): self.update_queue.put_nowait("SIMULATION_FINISHED") break - # --- Communication and Data Hub Logging Step --- - 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 - + # --- Communication, Data Hub, and GUI Update Step --- + if (current_time - self._last_update_time) >= self.update_interval_s: + self._last_update_time = current_time + + # Only proceed if the communicator is valid and open + if self.communicator and self.communicator.is_open: commands_to_send = [] timestamp_for_batch = time.monotonic() - active_targets = [t for t in updated_targets if t.active] for target in active_targets: + # Build the command string ONCE and reuse it + cmd = command_builder.build_tgtset_from_target_state(target) + commands_to_send.append(cmd) + # 1. Log the simulated state to the hub for analysis if self.simulation_hub: state_tuple = ( @@ -132,36 +136,31 @@ class SimulationEngine(threading.Thread): self.simulation_hub.add_simulated_state( target.target_id, timestamp_for_batch, state_tuple ) - # 1b. Optionally save the sent positions to CSV for debugging - try: - append_sent_position( - timestamp_for_batch, - target.target_id, - state_tuple[0], - state_tuple[1], - state_tuple[2], - command_builder.build_tgtset_from_target_state(target), - ) - except Exception: - # Do not break the simulation for logging failures - pass - - # 2. Build the command to send to the radar - cmd = command_builder.build_tgtset_from_target_state(target) - commands_to_send.append(cmd) + # 1b. Optionally save sent positions to CSV using the pre-built command + try: + append_sent_position( + timestamp_for_batch, + target.target_id, + state_tuple[0], + state_tuple[1], + state_tuple[2], + cmd, # Use the existing command string + ) + except Exception: + pass # 3. Send all commands in a single batch if commands_to_send: self.communicator.send_commands(commands_to_send) - # --- GUI Update Step --- - if self.update_queue: - try: - self.update_queue.put_nowait(updated_targets) - except Queue.Full: - self.logger.warning( - "GUI update queue is full. A frame was skipped." - ) + # 4. Update the GUI queue, now synced with the communication update + if self.update_queue: + try: + self.update_queue.put_nowait(updated_targets) + except Queue.Full: + self.logger.warning( + "GUI update queue is full. A frame was skipped." + ) time.sleep(TICK_INTERVAL_S) diff --git a/target_simulator/gui/main_view.py b/target_simulator/gui/main_view.py index be8cdc0..a36d88b 100644 --- a/target_simulator/gui/main_view.py +++ b/target_simulator/gui/main_view.py @@ -803,10 +803,10 @@ class MainView(tk.Tk): try: # We process one update at a time to keep the GUI responsive update = self.gui_update_queue.get_nowait() - try: - self.logger.debug(f"MainView: dequeued GUI update (type={type(update)}) from queue id={id(self.gui_update_queue)}") - except Exception: - pass + # try: + # self.logger.debug(f"MainView: dequeued GUI update (type={type(update)}) from queue id={id(self.gui_update_queue)}") + # except Exception: + # pass if update == "SIMULATION_FINISHED": self.logger.info("Simulation finished signal received.") @@ -826,29 +826,25 @@ class MainView(tk.Tk): # as a lightweight notification that real states were added to # the hub. Distinguish the two cases: if len(update) == 0: - # Empty-list used as a hub refresh notification. Do not - # clear the target list; just rebuild the PPI from the hub. - try: - self.logger.debug("MainView: received hub refresh notification from GUI queue.") - except Exception: - pass + # Hub refresh notification (real data arrived). + # Only update the 'real' targets on the PPI display. + # self.logger.debug("MainView: received hub refresh. Updating real targets.") display_data = self._build_display_data_from_hub() - try: - self.ppi_widget.update_targets(display_data) - except Exception: - self.logger.exception("Failed to update PPI widget from hub display data") + self.ppi_widget.update_real_targets(display_data.get("real", [])) + else: - # This update is the list of simulated targets from the engine + # This is an update with simulated targets from the engine. + # Only update the 'simulated' targets on the PPI and the target list. simulated_targets: List[Target] = update + # self.logger.debug(f"MainView: received simulation update for {len(simulated_targets)} targets.") # Update the target list view with detailed simulated data self.target_list.update_target_list(simulated_targets) - # For the PPI, build the comparative data structure from the hub - display_data = self._build_display_data_from_hub() - self.ppi_widget.update_targets(display_data) + # Update only the simulated targets on the PPI + self.ppi_widget.update_simulated_targets(simulated_targets) - # Update progress using target times from scenario + # Update simulation progress bar try: # Use the engine's scenario simulated time as elapsed if available if self.simulation_engine and self.simulation_engine.scenario: @@ -965,7 +961,9 @@ class MainView(tk.Tk): self.scenario.targets = {t.target_id: t for t in targets} # 2. Update the PPI display with the latest target list - self.ppi_widget.update_targets(targets) + # Pass an explicit dict so PPIDisplay treats this as a simulated-only + # update and does not accidentally clear real (server) targets. + self.ppi_widget.update_simulated_targets(targets) # 3. Automatically save the changes to the current scenario file if self.current_scenario_name: @@ -987,7 +985,8 @@ class MainView(tk.Tk): targets_to_display = self.scenario.get_all_targets() self.target_list.update_target_list(targets_to_display) - self.ppi_widget.update_targets(targets_to_display) + # Use an explicit dict to indicate these are simulated scenario targets. + self.ppi_widget.update_simulated_targets(targets_to_display) def _load_scenarios_into_ui(self): scenario_names = self.config_manager.get_scenario_names() @@ -1271,15 +1270,15 @@ class MainView(tk.Tk): theta0_deg = None theta1_deg = None - self.logger.debug( - "Heading pipeline: TID %s raw=%s hub=%s used=%s theta0=%.3f theta1=%.3f", - tid, - raw_h, - getattr(self.simulation_hub, 'get_real_heading')(tid) if self.simulation_hub else None, - real_target.current_heading_deg, - theta0_deg if theta0_deg is not None else float('nan'), - theta1_deg if theta1_deg is not None else float('nan'), - ) + #self.logger.debug( + # "Heading pipeline: TID %s raw=%s hub=%s used=%s theta0=%.3f theta1=%.3f", + # tid, + # raw_h, + # getattr(self.simulation_hub, 'get_real_heading')(tid) if self.simulation_hub else None, + # real_target.current_heading_deg, + # theta0_deg if theta0_deg is not None else float('nan'), + # theta1_deg if theta1_deg is not None else float('nan'), + #) except Exception: pass diff --git a/target_simulator/gui/payload_router.py b/target_simulator/gui/payload_router.py index 6af2341..62d8733 100644 --- a/target_simulator/gui/payload_router.py +++ b/target_simulator/gui/payload_router.py @@ -138,12 +138,12 @@ class DebugPayloadRouter: parsed_for_hub = SfpRisStatusPayload.from_buffer_copy(payload) ts_s = parsed_for_hub.scenario.timetag / 1000.0 - # First: remove any targets that the server marked as inactive (flags == 0) + # First: clear real data for any targets that the server marked as inactive try: for i, ris_t in enumerate(parsed_for_hub.tgt.tgt): try: - if ris_t.flags == 0 and self._hub and hasattr(self._hub, 'remove_target'): - self._hub.remove_target(i) + if ris_t.flags == 0 and self._hub and hasattr(self._hub, 'clear_real_target_data'): + self._hub.clear_real_target_data(i) except Exception: pass except Exception: diff --git a/target_simulator/gui/ppi_display.py b/target_simulator/gui/ppi_display.py index e4525a8..3529ea0 100644 --- a/target_simulator/gui/ppi_display.py +++ b/target_simulator/gui/ppi_display.py @@ -31,7 +31,9 @@ class PPIDisplay(ttk.Frame): self.scan_limit_deg = scan_limit_deg self.sim_target_artists, self.real_target_artists = [], [] self.sim_trail_artists, self.real_trail_artists = [], [] - self.target_label_artists = [] + # Keep label artists separated so we can update simulated labels + # without removing real labels when a simulated-only update happens. + self.sim_label_artists, self.real_label_artists = [], [] self.trail_length = trail_length or self.TRAIL_LENGTH self._trails = { "simulated": collections.defaultdict(lambda: collections.deque(maxlen=self.trail_length)), @@ -47,9 +49,12 @@ class PPIDisplay(ttk.Frame): self._create_plot() def _on_display_options_changed(self): + # A full redraw is needed, but we don't have the last data sets. + # The best approach is to clear everything. The next update cycle from + # the simulation engine and/or the server communicator will repopulate + # the display with the correct visibility settings. + self.clear_all_targets() if self.canvas: - # We need to redraw everything to show/hide elements - self.update_targets({}) # This is a trick to trigger a full redraw self.canvas.draw() def _create_controls(self): @@ -119,96 +124,113 @@ class PPIDisplay(ttk.Frame): self.range_selector.bind("<>", self._on_range_selected) self._update_scan_lines() - def update_targets(self, targets_data: Union[List[Target], Dict[str, List[Target]]]): - sim_data = targets_data.get("simulated", []) if isinstance(targets_data, dict) else (targets_data if isinstance(targets_data, list) else []) - real_data = targets_data.get("real", []) if isinstance(targets_data, dict) else [] - - for artists in [self.sim_target_artists, self.real_target_artists, self.sim_trail_artists, self.real_trail_artists, self.target_label_artists]: - for artist in artists: - artist.remove() - artists.clear() - - if self.show_sim_points_var.get() or self.show_sim_trail_var.get(): - for t in sim_data: - if t.active: - pos = (np.deg2rad(-t.current_azimuth_deg), t.current_range_nm) - self._trails["simulated"][t.target_id].append(pos) - - if self.show_real_points_var.get() or self.show_real_trail_var.get(): - for t in real_data: - if t.active: - pos = (np.deg2rad(-t.current_azimuth_deg), t.current_range_nm) - self._trails["real"][t.target_id].append(pos) - - if self.show_sim_points_var.get(): - self._draw_target_visuals([t for t in sim_data if t.active], 'green', self.sim_target_artists) - if self.show_real_points_var.get(): - self._draw_target_visuals([t for t in real_data if t.active], 'red', self.real_target_artists) - if self.show_sim_trail_var.get(): - self._draw_trails(self._trails["simulated"], 'limegreen', self.sim_trail_artists) - if self.show_real_trail_var.get(): - self._draw_trails(self._trails["real"], 'tomato', self.real_trail_artists) + def clear_all_targets(self): + """Clears all target artists from the display.""" + all_artists = ( + self.sim_target_artists + self.real_target_artists + + self.sim_trail_artists + self.real_trail_artists + + self.sim_label_artists + self.real_label_artists + ) + for artist in all_artists: + artist.remove() + self.sim_target_artists.clear() + self.real_target_artists.clear() + self.sim_trail_artists.clear() + self.real_trail_artists.clear() + self.sim_label_artists.clear() + self.real_label_artists.clear() + def update_simulated_targets(self, targets: List[Target]): + """Updates and redraws only the simulated targets.""" + self._update_target_category(targets, "simulated") if self.canvas: self.canvas.draw() - def _draw_target_visuals(self, targets: List[Target], color: str, artist_list: List): + def update_real_targets(self, targets: List[Target]): + """Updates and redraws only the real targets.""" + self._update_target_category(targets, "real") + if self.canvas: + self.canvas.draw() + + def _update_target_category(self, new_data: List[Target], category: str): + """ + Generic helper to update targets for a specific category ('simulated' or 'real'). + """ + if category == "simulated": + target_artists = self.sim_target_artists + trail_artists = self.sim_trail_artists + label_artists = self.sim_label_artists + trail_data = self._trails["simulated"] + show_points = self.show_sim_points_var.get() + show_trail = self.show_sim_trail_var.get() + color = 'green' + trail_color = 'limegreen' + else: # "real" + target_artists = self.real_target_artists + trail_artists = self.real_trail_artists + label_artists = self.real_label_artists + trail_data = self._trails["real"] + show_points = self.show_real_points_var.get() + show_trail = self.show_real_trail_var.get() + color = 'red' + trail_color = 'tomato' + + # 1. Clear existing artists for this category + for artist in target_artists + trail_artists + label_artists: + artist.remove() + target_artists.clear() + trail_artists.clear() + label_artists.clear() + + # 2. Update trail data + if show_points or show_trail: + for t in new_data: + if t.active: + pos = (np.deg2rad(-t.current_azimuth_deg), t.current_range_nm) + trail_data[t.target_id].append(pos) + + # 3. Draw new visuals + if show_points: + self._draw_target_visuals([t for t in new_data if t.active], color, target_artists, label_artists) + if show_trail: + self._draw_trails(trail_data, trail_color, trail_artists) + + def _draw_target_visuals(self, targets: List[Target], color: str, artist_list: List, label_artist_list: List): vector_len_nm = self.range_var.get() / 20.0 logger = logging.getLogger(__name__) + + # Determine marker size based on the target type (color) + marker_size = 6 if color == 'red' else 8 # Simulated targets (green) are smaller + for target in targets: # Plotting position (theta, r) r_nm = target.current_range_nm - theta_rad_plot = np.deg2rad(-target.current_azimuth_deg) - (dot,) = self.ax.plot(theta_rad_plot, r_nm, "o", markersize=6, color=color) + # MODIFICATION: Removed negation. The azimuth from the model is now used directly. + theta_rad_plot = np.deg2rad(target.current_azimuth_deg) + (dot,) = self.ax.plot(theta_rad_plot, r_nm, "o", markersize=marker_size, color=color) artist_list.append(dot) # --- Robust Vector Calculation --- - # 1. Convert target position to internal Cartesian (x=East, y=North) - # Use math.* for scalar computations to avoid accidental array behaviors az_rad_model = math.radians(target.current_azimuth_deg) x_start_nm = r_nm * math.sin(az_rad_model) y_start_nm = r_nm * math.cos(az_rad_model) - - # 2. Calculate vector displacement in Cartesian from heading - # Heading is defined as degrees clockwise from North (0 = North), - # so the unit vector in Cartesian (East, North) is (sin(h), cos(h)). - # Invert the sign of the heading angle for plotting so the - # drawn heading arrow follows the same angular convention used - # for positions (theta_plot = -azimuth). Using -heading here - # ensures left/right orientation matches the displayed azimuth. - hdg_rad_plot = math.radians(-target.current_heading_deg) + # MODIFICATION: Heading should also be consistent. + # A positive heading (e.g. 10 deg) means turning left (CCW), which matches + # the standard polar plot direction. + hdg_rad_plot = math.radians(target.current_heading_deg) dx_nm = vector_len_nm * math.sin(hdg_rad_plot) dy_nm = vector_len_nm * math.cos(hdg_rad_plot) - - # 3. Find end point in Cartesian x_end_nm = x_start_nm + dx_nm y_end_nm = y_start_nm + dy_nm - - # 4. Convert start and end points to plotting coordinates (theta_plot, r) r_end_nm = math.hypot(x_end_nm, y_end_nm) - theta_end_rad_plot = -math.atan2(x_end_nm, y_end_nm) + # MODIFICATION: Removed negation here as well for consistency. + theta_end_rad_plot = math.atan2(x_end_nm, y_end_nm) (line,) = self.ax.plot([theta_rad_plot, theta_end_rad_plot], [r_nm, r_end_nm], color=color, linewidth=1.2) artist_list.append(line) - # Debug log: useful to diagnose heading vs plotting coordinates - #try: - # logger.debug( - # "PPIDisplay: TID %s az=%.6f hdg=%.6f theta0_deg=%.3f theta1_deg=%.3f x_start=%.3f y_start=%.3f x_end=%.3f y_end=%.3f", - # target.target_id, - # target.current_azimuth_deg, - # target.current_heading_deg, - # math.degrees(theta_rad_plot), - # math.degrees(theta_end_rad_plot), - # x_start_nm, - # y_start_nm, - # x_end_nm, - # y_end_nm, - # ) - #except Exception: - # pass txt = self.ax.text(theta_rad_plot, r_nm + (vector_len_nm * 0.5), str(target.target_id), color="white", fontsize=8, ha="center", va="bottom") - self.target_label_artists.append(txt) + label_artist_list.append(txt) def _draw_trails(self, trail_data: Dict, color: str, artist_list: List): for trail in trail_data.values(): @@ -220,7 +242,9 @@ class PPIDisplay(ttk.Frame): def clear_trails(self): self._trails["simulated"].clear() self._trails["real"].clear() - self.update_targets({}) + self.clear_all_targets() + if self.canvas: + self.canvas.draw() def _update_scan_lines(self): max_r = self.ax.get_ylim()[1] @@ -250,9 +274,9 @@ class PPIDisplay(ttk.Frame): for point in path: x_ft, y_ft = point[1], point[2] r_ft = math.sqrt(x_ft**2 + y_ft**2) - # Use the same plotting convention used elsewhere: theta_plot = atan2(x, y) - # (update_targets computes theta via -current_azimuth_deg where - # current_azimuth_deg = -degrees(atan2(x,y)), which yields atan2(x,y)). + # Use the same plotting convention used elsewhere: theta_plot = atan2(x, y). + # This convention is established in the _draw_target_visuals helper, + # which computes theta via -current_azimuth_deg. az_rad_plot = math.atan2(x_ft, y_ft) path_rs.append(r_ft / NM_TO_FT) path_thetas.append(az_rad_plot) diff --git a/target_simulator/gui/trajectory_editor_window.py b/target_simulator/gui/trajectory_editor_window.py index a8c38fb..e408bf3 100644 --- a/target_simulator/gui/trajectory_editor_window.py +++ b/target_simulator/gui/trajectory_editor_window.py @@ -339,7 +339,7 @@ class TrajectoryEditorWindow(tk.Toplevel): self.sim_time_label.config( text=f"{sim_time:.1f}s / {self.total_sim_time:.1f}s" ) - self.ppi_preview.update_targets(update) + self.ppi_preview.update_simulated_targets(update) finally: if self.is_preview_running.get(): self.after(GUI_QUEUE_POLL_INTERVAL_MS, self._process_preview_queue) diff --git a/tests/gui/test_trajectory_editor_window.py b/tests/gui/test_trajectory_editor_window.py index 761a69a..c908cd6 100644 --- a/tests/gui/test_trajectory_editor_window.py +++ b/tests/gui/test_trajectory_editor_window.py @@ -71,7 +71,7 @@ def _make_window(): inst.is_paused = DummyVar(False) inst.wp_tree = FakeTree() inst.ppi_preview = types.SimpleNamespace( - draw_trajectory_preview=lambda **k: None, update_targets=lambda u: None + draw_trajectory_preview=lambda **k: None, update_simulated_targets=lambda u: None ) inst.sim_progress_var = DummyVar(0.0) inst.sim_time_label = DummyLabel()