From d152c8a559310d6b06b6e78c543d3cd9e46d127b Mon Sep 17 00:00:00 2001 From: Daniel Lovell Date: Sat, 14 Sep 2024 15:00:35 -0700 Subject: [PATCH] Added telemetry monitoring and sample visualization Python script --- .gitignore | 2 + Assets/Scenes/MainScene.unity | 45 ++++++++++++++++ Assets/Scripts/Agent.cs | 11 +++- Assets/Scripts/Interceptors/Hydra70.cs | 4 -- Assets/Scripts/Monitor.cs | 69 ++++++++++++++++++++++++ Assets/Scripts/Monitor.cs.meta | 11 ++++ Assets/Scripts/SimManager.cs | 60 +++++++++++++-------- Telemetry/visualize_telemetry.py | 74 ++++++++++++++++++++++++++ 8 files changed, 250 insertions(+), 26 deletions(-) create mode 100644 Assets/Scripts/Monitor.cs create mode 100644 Assets/Scripts/Monitor.cs.meta create mode 100644 Telemetry/visualize_telemetry.py diff --git a/.gitignore b/.gitignore index ea8c50b..9dfb970 100644 --- a/.gitignore +++ b/.gitignore @@ -75,3 +75,5 @@ crashlytics-build.properties .vscode/ .vsconfig +# Telemetry Logs +Telemetry/Logs/ \ No newline at end of file diff --git a/Assets/Scenes/MainScene.unity b/Assets/Scenes/MainScene.unity index b9790b2..dca0110 100644 --- a/Assets/Scenes/MainScene.unity +++ b/Assets/Scenes/MainScene.unity @@ -122,6 +122,50 @@ NavMeshSettings: debug: m_Flags: 0 m_NavMeshData: {fileID: 0} +--- !u!1 &17322847 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 17322849} + - component: {fileID: 17322848} + m_Layer: 0 + m_Name: SimMonitor + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!114 &17322848 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 17322847} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: d822a6be5cb96f54ca63228eeb5ebf23, type: 3} + m_Name: + m_EditorClassIdentifier: +--- !u!4 &17322849 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 17322847} + serializedVersion: 2 + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 1658.5714, y: 1799.5472, z: 3209.2483} + m_LocalScale: {x: 1, y: 1, z: 1} + m_ConstrainProportionsScale: 0 + m_Children: [] + m_Father: {fileID: 0} + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} --- !u!1 &61846315 GameObject: m_ObjectHideFlags: 0 @@ -2463,3 +2507,4 @@ SceneRoots: - {fileID: 253946927} - {fileID: 694951366} - {fileID: 2052906806} + - {fileID: 17322849} diff --git a/Assets/Scripts/Agent.cs b/Assets/Scripts/Agent.cs index 4f0b7e0..80bf831 100644 --- a/Assets/Scripts/Agent.cs +++ b/Assets/Scripts/Agent.cs @@ -24,6 +24,14 @@ public abstract class Agent : MonoBehaviour { [SerializeField] public StaticConfig StaticConfig; + // Define delegates + public delegate void AgentHitEventHandler(Agent agent); + public delegate void AgentMissEventHandler(Agent agent); + + // Define events + public event AgentHitEventHandler OnAgentHit; + public event AgentMissEventHandler OnAgentMiss; + public void SetFlightPhase(FlightPhase flightPhase) { Debug.Log( $"Setting flight phase to {flightPhase} at time {SimManager.Instance.GetElapsedSimulationTime()}"); @@ -91,13 +99,14 @@ public abstract class Agent : MonoBehaviour { // Mark the agent as having hit the target or been hit. public void MarkAsHit() { _isHit = true; + OnAgentHit?.Invoke(this); TerminateAgent(); } public void MarkAsMiss() { _isMiss = true; if (_target != null) { - SimManager.Instance.RegisterTargetMiss(_target as Target); + OnAgentMiss?.Invoke(this); _target = null; } TerminateAgent(); diff --git a/Assets/Scripts/Interceptors/Hydra70.cs b/Assets/Scripts/Interceptors/Hydra70.cs index dfd0b9d..7847423 100644 --- a/Assets/Scripts/Interceptors/Hydra70.cs +++ b/Assets/Scripts/Interceptors/Hydra70.cs @@ -36,9 +36,6 @@ public class Hydra70 : Missile { } public void SpawnSubmunitions() { - Debug.Log("Spawning submunitions"); - // print the callstack - Debug.Log(new System.Diagnostics.StackTrace().ToString()); List submunitions = new List(); switch (_agentConfig.submunitions_config.agent_config.missile_type) { case MissileType.MICROMISSILE: @@ -50,7 +47,6 @@ public class Hydra70 : Missile { convertedConfig.initial_state.velocity = GetComponent().velocity; Missile submunition = SimManager.Instance.CreateMissile(convertedConfig); submunitions.Add(submunition); - Debug.Log("Created submunition"); } break; } diff --git a/Assets/Scripts/Monitor.cs b/Assets/Scripts/Monitor.cs new file mode 100644 index 0000000..f6ce5d3 --- /dev/null +++ b/Assets/Scripts/Monitor.cs @@ -0,0 +1,69 @@ +using UnityEngine; +using System.Collections; +using System.IO; +using System; +using System.Linq; + +public class SimMonitor : MonoBehaviour +{ + private const float UpdateRate = 0.01f; // 100 Hz + private StreamWriter writer; + private Coroutine monitorRoutine; + + private void Start() + { + SimManager.Instance.OnSimulationEnded += RegisterSimulationEnded; + InitializeFile(); + monitorRoutine = StartCoroutine(MonitorRoutine()); + } + + private void InitializeFile() + { + string timestamp = DateTime.Now.ToString("yyyyMMdd_HHmmss"); + string fileName = $"sim_telemetry_{timestamp}.csv"; + string directory = "Telemetry/Logs/"; + Directory.CreateDirectory(directory); + string path = Path.Combine(directory, fileName); + writer = new StreamWriter(path, false); + writer.WriteLine("Time,AgentID,AgentX,AgentY,AgentZ,AgentVX,AgentVY,AgentVZ,AgentState,AgentType"); + Debug.Log($"Monitoring simulation data to {path}"); + } + + private IEnumerator MonitorRoutine() + { + while (true) + { + ExportTelemetry(); + yield return new WaitForSeconds(UpdateRate); + } + } + + private void ExportTelemetry() + { + float time = (float)SimManager.Instance.GetElapsedSimulationTime(); + foreach (var agent in SimManager.Instance.GetActiveTargets().Cast().Concat(SimManager.Instance.GetActiveMissiles().Cast())) + { + Vector3 pos = agent.transform.position; + Vector3 vel = agent.GetComponent().velocity; + string type = agent is Target ? "T" : "M"; + writer.WriteLine($"{time:F2},{agent.name},{pos.x:F2},{pos.y:F2},{pos.z:F2},{vel.x:F2},{vel.y:F2},{vel.z:F2},{(int)agent.GetFlightPhase()},{type}"); + } + + writer.Flush(); + } + + private void RegisterSimulationEnded() + { + writer.Close(); + StopCoroutine(monitorRoutine); + } + + + private void OnDestroy() + { + if (writer != null) + { + writer.Close(); + } + } +} \ No newline at end of file diff --git a/Assets/Scripts/Monitor.cs.meta b/Assets/Scripts/Monitor.cs.meta new file mode 100644 index 0000000..ec180b2 --- /dev/null +++ b/Assets/Scripts/Monitor.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: d822a6be5cb96f54ca63228eeb5ebf23 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/Scripts/SimManager.cs b/Assets/Scripts/SimManager.cs index 7aea577..6a9f501 100644 --- a/Assets/Scripts/SimManager.cs +++ b/Assets/Scripts/SimManager.cs @@ -20,14 +20,19 @@ public class SimManager : MonoBehaviour { public SimulationConfig simulationConfig; private List _missiles = new List(); + private List _activeMissiles = new List(); private List _unassignedTargets = new List(); private List _targets = new List(); + private List _activeTargets = new List(); private float _elapsedSimulationTime = 0f; private float endTime = 100f; // Set an appropriate end time private bool simulationRunning = false; private IAssignment _assignmentScheme; + public delegate void SimulationEndedHandler(); + public event SimulationEndedHandler OnSimulationEnded; + /// /// Gets the elapsed simulation time. /// @@ -36,6 +41,14 @@ public class SimManager : MonoBehaviour { return _elapsedSimulationTime; } + public List GetActiveMissiles() { + return _activeMissiles; + } + + public List GetActiveTargets() { + return _activeTargets; + } + void Awake() { // Ensure only one instance of SimManager exists if (Instance == null) { @@ -63,6 +76,7 @@ public class SimManager : MonoBehaviour { foreach (var swarmConfig in simulationConfig.missile_swarm_configs) { for (int i = 0; i < swarmConfig.num_agents; i++) { var missile = CreateMissile(swarmConfig.agent_config); + missile.OnAgentHit += RegisterMissileHit; } } @@ -71,6 +85,8 @@ public class SimManager : MonoBehaviour { foreach (var swarmConfig in simulationConfig.target_swarm_configs) { for (int i = 0; i < swarmConfig.num_agents; i++) { var target = CreateTarget(swarmConfig.agent_config); + target.OnAgentHit += RegisterTargetHit; + target.OnAgentMiss += RegisterTargetMiss; } } @@ -81,8 +97,22 @@ public class SimManager : MonoBehaviour { AssignMissilesToTargets(_missiles); } - public void RegisterTargetMiss(Target target) { - _unassignedTargets.Add(target); + public void RegisterMissileHit(Agent missile) { + if (missile is Missile missileComponent) { + _activeMissiles.Remove(missileComponent); + } + } + + public void RegisterTargetHit(Agent target) { + if (target is Target targetComponent) { + _activeTargets.Remove(targetComponent); + } + } + + public void RegisterTargetMiss(Agent target) { + if (target is Target targetComponent) { + _unassignedTargets.Add(targetComponent); + } } /// @@ -137,16 +167,9 @@ public class SimManager : MonoBehaviour { break; } - // Missile missile = missileObject.GetComponent(); - // if (missile == null) - // { - // Debug.LogError($"Missile component not found on prefab '{prefabName}'."); - // Destroy(missileObject); - // return null; - // } - - // missile.SetAgentConfig(config); _missiles.Add(missileObject.GetComponent()); + _activeMissiles.Add(missileObject.GetComponent()); + // Assign a unique and simple target ID int missileId = _missiles.Count; missileObject.name = $"{config.missile_type}_Missile_{missileId}"; @@ -167,17 +190,10 @@ public class SimManager : MonoBehaviour { if (targetObject == null) return null; - // Target target = targetObject.GetComponent(); - // if (target == null) - // { - // Debug.LogError($"Target component not found on prefab '{config.prefabName}'."); - // Destroy(targetObject); - // return null; - // } - - // target.SetAgentConfig(config); _targets.Add(targetObject.GetComponent()); + _activeTargets.Add(targetObject.GetComponent()); _unassignedTargets.Add(targetObject.GetComponent()); + // Assign a unique and simple target ID int targetId = _targets.Count; targetObject.name = $"{config.target_type}_Target_{targetId}"; @@ -214,7 +230,9 @@ public class SimManager : MonoBehaviour { } - private void RestartSimulation() { + public void RestartSimulation() { + OnSimulationEnded?.Invoke(); + Debug.Log("Simulation ended"); // Reset simulation time _elapsedSimulationTime = 0f; simulationRunning = true; diff --git a/Telemetry/visualize_telemetry.py b/Telemetry/visualize_telemetry.py new file mode 100644 index 0000000..ed97768 --- /dev/null +++ b/Telemetry/visualize_telemetry.py @@ -0,0 +1,74 @@ +import os +import glob +import argparse +import pandas as pd +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import Axes3D +import numpy as np + +def find_latest_telemetry_file(directory='./Logs/'): + list_of_files = glob.glob(os.path.join(directory, 'sim_telemetry_*.csv')) + if not list_of_files: + print(f"No telemetry files found in {directory}") + return None + latest_file = max(list_of_files, key=os.path.getctime) + print(f"Using latest telemetry file: {latest_file}") + return latest_file + +def plot_telemetry(file_path): + # Read the telemetry CSV file + df = pd.read_csv(file_path) + + # Create a 3D plot + fig = plt.figure(figsize=(12, 8)) + ax = fig.add_subplot(111, projection='3d') + + # Define colors for different agent types + colors = {'T': 'red', 'M': 'blue'} + + # Group data by AgentID + for agent_type, type_data in df.groupby('AgentType'): + color = colors.get(agent_type, 'black') + downsampled = type_data.groupby('AgentID').apply(lambda x: x.iloc[::10]) + + ax.plot( + downsampled['AgentX'], + downsampled['AgentZ'], + downsampled['AgentY'], + color=color, + alpha=0.5, + linewidth=0.5, + label=f"{agent_type}" + ) + + ax.set_xlabel('X (Right)') + ax.set_ylabel('Z (Forward)') + ax.set_zlabel('Y (Up)') + + ax.view_init(elev=20, azim=45) + + # Add a ground plane + x_min, x_max = ax.get_xlim() + z_min, z_max = ax.get_ylim() + xx, zz = np.meshgrid(np.linspace(x_min, x_max, 2), np.linspace(z_min, z_max, 2)) + yy = np.zeros_like(xx) + ax.plot_surface(xx, zz, yy, alpha=0.2, color='green') + + plt.title('Agents Trajectories (X: Right, Z: Forward, Y: Up)') + plt.legend() + plt.tight_layout() + plt.show() + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description='Visualize telemetry data.') + parser.add_argument('file', nargs='?', default=None, help='Path to telemetry CSV file.') + args = parser.parse_args() + + if args.file: + file_path = args.file + else: + file_path = find_latest_telemetry_file() + if file_path is None: + exit(1) + + plot_telemetry(file_path) \ No newline at end of file