Added telemetry monitoring and sample visualization Python script

.gitignore

@@ -75,3 +75,5 @@ crashlytics-build.properties
 .vscode/
 .vsconfig
 
+# Telemetry Logs
+Telemetry/Logs/

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}

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();

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<Missile> submunitions = new List<Missile>();
     switch (_agentConfig.submunitions_config.agent_config.missile_type) {
       case MissileType.MICROMISSILE:
@@ -50,7 +47,6 @@ public class Hydra70 : Missile {
           convertedConfig.initial_state.velocity = GetComponent<Rigidbody>().velocity;
           Missile submunition = SimManager.Instance.CreateMissile(convertedConfig);
           submunitions.Add(submunition);
-          Debug.Log("Created submunition");
         }
         break;
     }

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<Agent>().Concat(SimManager.Instance.GetActiveMissiles().Cast<Agent>()))
+        {
+            Vector3 pos = agent.transform.position;
+            Vector3 vel = agent.GetComponent<Rigidbody>().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();
+        }
+    }
+}

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: 

Assets/Scripts/SimManager.cs

@@ -20,14 +20,19 @@ public class SimManager : MonoBehaviour {
   public SimulationConfig simulationConfig;
 
   private List<Missile> _missiles = new List<Missile>();
+  private List<Missile> _activeMissiles = new List<Missile>();
   private List<Target> _unassignedTargets = new List<Target>();
   private List<Target> _targets = new List<Target>();
+  private List<Target> _activeTargets = new List<Target>();
   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;
+
   /// <summary>
   /// Gets the elapsed simulation time.
   /// </summary>
@@ -36,6 +41,14 @@ public class SimManager : MonoBehaviour {
     return _elapsedSimulationTime;
   }
 
+  public List<Missile> GetActiveMissiles() {
+    return _activeMissiles;
+  }
+
+  public List<Target> 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) {
+  public void RegisterMissileHit(Agent missile) {
-    _unassignedTargets.Add(target);
+    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);
+    }
   }
 
   /// <summary>
@@ -137,16 +167,9 @@ public class SimManager : MonoBehaviour {
         break;
     }
 
-    // Missile missile = missileObject.GetComponent<Missile>();
-    // if (missile == null)
-    // {
-    //     Debug.LogError($"Missile component not found on prefab '{prefabName}'.");
-    //     Destroy(missileObject);
-    //     return null;
-    // }
-
-    // missile.SetAgentConfig(config);
     _missiles.Add(missileObject.GetComponent<Missile>());
+    _activeMissiles.Add(missileObject.GetComponent<Missile>());
+
     // 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<Target>();
-    // if (target == null)
-    // {
-    //     Debug.LogError($"Target component not found on prefab '{config.prefabName}'.");
-    //     Destroy(targetObject);
-    //     return null;
-    // }
-
-    // target.SetAgentConfig(config);
     _targets.Add(targetObject.GetComponent<Target>());
+    _activeTargets.Add(targetObject.GetComponent<Target>());
     _unassignedTargets.Add(targetObject.GetComponent<Target>());
+
     // 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;

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)