[Communication] PR8: Modify Simulation and SimManager

Assets/Scripts/Agents/AgentBase.cs

@@ -4,6 +4,12 @@
 //
 // See the agent interface for property and method documentation.
 public class AgentBase : MonoBehaviour, IAgent {
+
+  public virtual CommsNode NodeType => CommsNode.Interceptor;
+
+  // Make sure the same agent does not subscribe to the mailbox event more than once (keeping track).
+  private bool _mailboxRegistered = false;
+
   public event AgentTerminatedEventHandler OnTerminated;
 
   private const float _epsilon = 1e-12f;
@@ -188,10 +194,13 @@ protected virtual void Awake() {
     if (EarlyFixedUpdateManager.Instance != null) {
       EarlyFixedUpdateManager.Instance.OnEarlyFixedUpdate += UpdateTransformData;
     }
+    TryRegisterMailbox();
   }
 
   // Start is called before the first frame update.
-  protected virtual void Start() {}
+  protected virtual void Start() {
+    TryRegisterMailbox();
+  }
 
   // FixedUpdate is called multiple times per frame. All physics calculations and updates occur
   // immediately after FixedUpdate, and all movement values are multiplied by Time.deltaTime.
@@ -210,6 +219,10 @@ protected virtual void LateUpdate() {}
 
   // OnDestroy is called when the object is being destroyed.
   protected virtual void OnDestroy() {
+    if (_mailboxRegistered && Mailbox.Instance != null) {
+      Mailbox.Instance.OnMessageDelivered -= HandleMailboxDelivery;
+      _mailboxRegistered = false;
+    }
     if (EarlyFixedUpdateManager.Instance != null) {
       EarlyFixedUpdateManager.Instance.OnEarlyFixedUpdate -= UpdateTransformData;
     }
@@ -330,4 +343,22 @@ private Transformation GetRelativeTransformation(in Vector3 position, in Vector3
       Acceleration = accelerationTransformation,
     };
   }
+
+  protected virtual void OnMessage(Message message) {}
+
+  // Event Subscription to Mailbox.
+  protected void TryRegisterMailbox() {
+    if (_mailboxRegistered) { return; }
+    Mailbox mailbox = Mailbox.GetOrCreateInstance();
+    if (mailbox == null) { return; }
+    mailbox.OnMessageDelivered += HandleMailboxDelivery;
+    _mailboxRegistered = true;
+  }
+
+  // Delivers a message to the right receiver. Check if self is the right receiver, if so then handle the message.
+  private void HandleMailboxDelivery(IAgent receiver, Message message) {
+    if (!ReferenceEquals(receiver, this)) { return; }
+    OnMessage(message);
+  }
+
 }

Assets/Scripts/Agents/IAgent.cs

@@ -8,6 +8,8 @@
 public delegate void AgentTerminatedEventHandler(IAgent agent);
 
 public interface IAgent {
+  CommsNode NodeType { get; }
+
   event AgentTerminatedEventHandler OnTerminated;
 
   HierarchicalAgent HierarchicalAgent { get; set; }

Assets/Scripts/Agents/Messaging/IMessagePayload.cs

@@ -0,0 +1,3 @@
+// Interface for MessagePayload
+
+public interface IMessagePayload {}

Assets/Scripts/Agents/Messaging/LatencyTable.cs

@@ -0,0 +1,36 @@
+using System;
+using UnityEngine;
+
+
+// Latency Table keeps track of the delay between every agent.
+// Table is n * n sized, n is the size of CommsNode.
+
+public enum CommsNode {
+    IADS = 0,
+    Carrier = 1,
+    Interceptor = 2,
+}
+
+// Setting everything to 0 should behave like with no latency
+public sealed class LatencyTable {
+    private readonly float[,] _latency;
+
+    // creates empty _latency array with defaultSeconds seconds each
+    public LatencyTable(float defaultSeconds = 0f) {
+        int n = Enum.GetValues(typeof(CommsNode)).Length;
+        _latency = new float[n, n];
+        for (int i = 0; i < n; i++) {
+            for (int j = 0; j < n; j++) {
+                _latency[i, j] = defaultSeconds;
+            }
+        }
+    }
+
+    public void Set(CommsNode from, CommsNode to, float seconds) {
+        _latency[(int) from, (int) to] = Mathf.Max(0f, seconds);
+    }
+
+    public float Get(CommsNode from, CommsNode to) {
+        return _latency[(int) from, (int) to];
+    }
+}

Assets/Scripts/Agents/Messaging/Mailbox.cs

@@ -0,0 +1,160 @@
+using System;
+using UnityEngine;
+
+// The centralized mailbox is what all agents use to send and receive delayed messages.
+
+public class Mailbox : MonoBehaviour {
+    public static Mailbox Instance { get; private set; }
+    public event Action<IAgent, Message> OnMessageDelivered;
+    private PriorityQueue<PendingMessage> _messageQueue;
+    private LatencyTable _latencyTable;
+
+    // 3 modes for constructing the latency table.
+    private enum LatencyMode {
+        NoLatency, // Ideal communication with zero latency.
+        UniformLatency, // Set all latency entries to one value.
+        IndividualLatency, // Configure each from->to pair independently.
+    }
+
+    private struct LatencyOverride {
+        public CommsNode From;
+        public CommsNode To;
+        public float Seconds;
+    }
+
+    // Latency jitter standard deviation in seconds.
+    [SerializeField]
+    private float _latencyJitterStdSeconds = 0f;
+
+    // Latency mode for setting latencyTable
+    [SerializeField]
+    private LatencyMode _latencyMode = LatencyMode.UniformLatency;
+
+    // Used for to set all latency to _uniformLatency value. Only works when _latencyMode = LatencyMode.UniformLatency.
+    [SerializeField]
+    private float _uniformLatency = 0.2f;
+
+    // TODO (Joseph): make this table serializable or put in protobuf.
+    // Can set individual latency based on node-to-node types
+    private static readonly LatencyOverride[] DefaultLatencyOverrides = {
+        new LatencyOverride { From = CommsNode.IADS, To = CommsNode.IADS, Seconds = 0.2f },
+        new LatencyOverride { From = CommsNode.IADS, To = CommsNode.Carrier, Seconds = 0.2f },
+        new LatencyOverride { From = CommsNode.Carrier, To = CommsNode.IADS, Seconds = 0.2f },
+        new LatencyOverride { From = CommsNode.Carrier, To = CommsNode.Carrier, Seconds = 0.2f },
+        new LatencyOverride { From = CommsNode.Carrier, To = CommsNode.Interceptor, Seconds = 0.2f },
+        new LatencyOverride { From = CommsNode.Interceptor, To = CommsNode.Carrier, Seconds = 0.2f },
+        new LatencyOverride { From = CommsNode.Interceptor, To = CommsNode.Interceptor, Seconds = 0.2f },
+    };
+
+    // instantiate Mailbox Component
+    public static Mailbox GetOrCreateInstance() {
+        if (Instance != null) {
+            return Instance;
+        }
+        Instance = UnityEngine.Object.FindFirstObjectByType<Mailbox>();
+        if (Instance != null) {
+            return Instance;
+        }
+        var mailboxObject = new GameObject(nameof(Mailbox));
+        DontDestroyOnLoad(mailboxObject);
+        Instance = mailboxObject.AddComponent<Mailbox>();
+        return Instance;
+    }
+
+    private void Awake() {
+        if (Instance != null && Instance != this) {
+            Destroy(gameObject);
+            return;
+        }
+        Instance = this;
+        ApplyLatencyOverrides();
+    }
+
+    private void Update() {
+        DeliverDueMessages();
+    }
+
+    private void OnValidate() {
+        _latencyJitterStdSeconds = Mathf.Max(0f, _latencyJitterStdSeconds);
+        _uniformLatency = Mathf.Max(0f, _uniformLatency);
+    }
+
+    public void ClearPendingMessages() {
+        _messageQueue = new PriorityQueue<PendingMessage>();
+    }
+
+    // Enqueue a message for delayed delivery. Message will be released when DeliverTime has reached.
+    public void Send(Message message) {
+        if (message == null || message.Sender == null || message.Receiver == null) { return; }
+        float baseLatency = _latencyTable.Get(message.Sender.NodeType, message.Receiver.NodeType);
+        float jitter = _latencyJitterStdSeconds > 0f ? SampleGaussian(mean: 0f, stdDev: _latencyJitterStdSeconds) : 0f;
+        float totalLatency = Mathf.Max(0f, baseLatency + jitter);
+        float deliverAt = GetCurrentTime() + totalLatency;
+        _messageQueue.Enqueue(new PendingMessage(deliverAt, message), deliverAt);
+    }
+
+    // Override latency values into LatencyTable.
+    private void ApplyLatencyOverrides() {
+        ClearPendingMessages();
+        switch (_latencyMode) {
+            case LatencyMode.NoLatency: {
+                _latencyTable = new LatencyTable();
+                break;
+            }
+            case LatencyMode.UniformLatency: {
+                _latencyTable = new LatencyTable(_uniformLatency);
+                break;
+            }
+            case LatencyMode.IndividualLatency: {
+                _latencyTable = new LatencyTable();
+                foreach (var latencyEntry in DefaultLatencyOverrides) {
+                    _latencyTable.Set(latencyEntry.From, latencyEntry.To, latencyEntry.Seconds);
+                }
+                break;
+            }
+            default: {
+                _latencyTable = new LatencyTable();
+                break;
+            }
+        }
+    }
+
+    // Repeadly pop due messages off the queue. Apply PDR so only certain amount of messages pass through.
+    private void DeliverDueMessages() {
+        if (_messageQueue == null) {
+            return;
+        }
+        float currentTime = GetCurrentTime();
+        var dueMessages = new System.Collections.Generic.List<PendingMessage>();
+        while (!_messageQueue.IsEmpty() && currentTime >= _messageQueue.Peek().DeliverAt) {
+            dueMessages.Add(_messageQueue.Dequeue());
+        }
+        foreach (PendingMessage pending in dueMessages) {
+            if (!IsReceiverValid(pending.Receiver)) { continue; }
+            OnMessageDelivered?.Invoke(pending.Receiver, pending.Message);
+        }
+    }
+
+    // Helper function for jitter calculation.
+    private static float SampleGaussian(float mean, float stdDev) {
+        float u1 = Mathf.Max(float.Epsilon, UnityEngine.Random.value);
+        float u2 = UnityEngine.Random.value;
+        float standardNormal = Mathf.Sqrt(-2f * Mathf.Log(u1)) * Mathf.Cos(2f * Mathf.PI * u2);
+        return mean + stdDev * standardNormal;
+    }
+
+    private static float GetCurrentTime() {
+        return SimManager.Instance != null ? SimManager.Instance.ElapsedTime : Time.time;
+    }
+
+    // Check if receiver still exists.
+    private static bool IsReceiverValid(IAgent receiver) {
+        if (receiver == null) {
+            return false;
+        }
+        if (receiver is UnityEngine.Object unityObject && unityObject == null) {
+            return false;
+        }
+        return !receiver.IsTerminated;
+    }
+}

Assets/Scripts/Agents/Messaging/Message.cs

@@ -0,0 +1,56 @@
+using System;
+
+// Message is a base envolope for message sending. It always carries Sender, Reciever, Type, and Payload.
+
+// Types of Message types based on inter-agent communication contents.
+public enum MessageType {
+  AssignSubInterceptorRequest,
+  AssignTarget,
+  ReassignTargetRequest,
+}
+
+/* Message is a base envolope for message sending. It always carries Sender, Reciever, Type, and Payload.
+This file wraps the payload data with transport metadata. It interntionally layers and splits the
+payload and the transportation data. Mailbox only focuses on transportation (enqueue, latency,
+and delivery timing) */
+
+public abstract class Message {
+  public IAgent Sender { get; }
+  public IAgent Receiver { get; }
+  public MessageType Type { get; }
+
+  public abstract IMessagePayload Payload { get; }
+
+  protected Message(IAgent sender, IAgent receiver, MessageType type) {
+    Sender = sender ?? throw new ArgumentNullException(nameof(sender));
+    Receiver = receiver ?? throw new ArgumentNullException(nameof(receiver));
+    Type = type;
+  }
+}
+
+public sealed class AssignSubInterceptorRequestMessage : Message {
+  public AssignSubInterceptorRequestPayload PayloadData { get; }
+  public override IMessagePayload Payload => PayloadData;
+
+  public AssignSubInterceptorRequestMessage(IAgent sender, IAgent receiver, IInterceptor subInterceptor) : base(sender, receiver, MessageType.AssignSubInterceptorRequest) {
+    PayloadData = new AssignSubInterceptorRequestPayload(subInterceptor);
+  }
+}
+
+public sealed class AssignTargetMessage : Message {
+  public AssignTargetPayload PayloadData { get; }
+  public override IMessagePayload Payload => PayloadData;
+
+  public AssignTargetMessage(IAgent sender, IAgent receiver, IHierarchical target) : base(sender, receiver, MessageType.AssignTarget) {
+    PayloadData = new AssignTargetPayload(target);
+  }
+}
+
+public sealed class ReassignTargetRequestMessage : Message {
+  public ReassignTargetRequestPayload PayloadData { get; }
+  public override IMessagePayload Payload => PayloadData;
+
+  public ReassignTargetRequestMessage(IAgent sender, IAgent receiver, IHierarchical target) : base(sender, receiver, MessageType.ReassignTargetRequest) {
+    PayloadData = new ReassignTargetRequestPayload(target);
+  }
+}

Assets/Scripts/Agents/Messaging/MessagePayload.cs

@@ -0,0 +1,29 @@
+using System;
+
+/* MessagePayload defines different types of new payload objects. Payload is carried by 
+Message envolopes. Concrete payload content lives in here and are only read explicitly 
+by receivers. */
+
+public sealed class AssignSubInterceptorRequestPayload : IMessagePayload {
+  public IInterceptor SubInterceptor { get; }
+
+  public AssignSubInterceptorRequestPayload(IInterceptor subInterceptor) {
+    SubInterceptor = subInterceptor ?? throw new ArgumentNullException(nameof(subInterceptor));
+  }
+}
+
+public sealed class AssignTargetPayload : IMessagePayload {
+  public IHierarchical Target { get; }
+
+  public AssignTargetPayload(IHierarchical target) {
+    Target = target ?? throw new ArgumentNullException(nameof(target));
+  }
+}
+
+public sealed class ReassignTargetRequestPayload : IMessagePayload {
+  public IHierarchical Target { get; }
+
+  public ReassignTargetRequestPayload(IHierarchical target) {
+    Target = target ?? throw new ArgumentNullException(nameof(target));
+  }
+}