Binary msg receiving fix

Unity/SiteServer.cs

@@ -24,7 +24,7 @@ namespace RoboidControl.Unity {
 
         public void HandleNewThing(RoboidControl.Thing thing) {
             //Debug.Log($"Handle New thing event for {thing}");
-            site.Add(thing, false);
+            //site.Add(thing, false);
             thingQueue.Enqueue(thing);
         }
 

Unity/Thing.cs

@@ -65,25 +65,21 @@ namespace RoboidControl.Unity {
         /// Update the Unity representation
         /// </summary>
         protected virtual void Update() {
-            if (core == null)
+            if (core == null) {
+                Debug.Log("Core thing is gone, self destruct in 0 seconds...");
+                Destroy(this);
                 return;
+            }
 
             if (core.linearVelocity != null && core.linearVelocity.distance != 0) {
                 Vector3 direction = Quaternion.AngleAxis(core.linearVelocity.direction.horizontal, Vector3.up) * Vector3.forward;
                 this.transform.Translate(core.linearVelocity.distance * Time.deltaTime * direction, Space.Self);
             }
-            // else if (core.positionUpdated)
-            //     this.transform.localPosition = core.position.ToVector3();
 
             if (core.angularVelocity != null && core.angularVelocity.distance != 0) {
-                // Vector3 angularVelocity = core.angularVelocity.ToVector3();
-                // Debug.Log(angularVelocity);
                 Vector3 axis = core.angularVelocity.direction.ToVector3();
                 this.transform.localRotation *= Quaternion.AngleAxis(core.angularVelocity.distance * Time.deltaTime, axis);
-                //this.transform.localRotation *= Quaternion.Euler(angularVelocity * Time.deltaTime);
             }
-            // else if (core.orientationUpdated)
-            //     this.transform.localRotation = core.orientation.ToQuaternion();
 
             if (!string.IsNullOrEmpty(core.modelUrl) && this.modelUrl == null) {
                 string extension = core.modelUrl.Substring(core.modelUrl.LastIndexOf("."));
@@ -98,7 +94,14 @@ namespace RoboidControl.Unity {
                     this.gameObject.name = core.name;
                 core.nameChanged = false;
             }
-
+            if (core.hierarchyChanged) {
+                Debug.Log("Parent changed");
+                if (core.parent == null)
+                    this.transform.SetParent(null, true);
+                else
+                    this.transform.SetParent(core.parent.component.transform, true);
+            core.hierarchyChanged = false;
+            }
         }
 
         private void PoseChanged() {

src/Messages/DestroyMsg.cs

@@ -26,9 +26,9 @@ namespace RoboidControl {
         /// </summary>
         /// <param name="networkId">The network ID of the thing</param>
         /// <param name="thingId">The ID of the thing</param>
-        public DestroyMsg(byte networkId, byte thingId) {
+        public DestroyMsg(byte networkId, Thing thing) {
             this.networkId = networkId;
-            this.thingId = thingId;
+            this.thingId = thing.id;
         }
         /// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer)
         public DestroyMsg(byte[] buffer) : base(buffer) {

src/Participant.cs

@@ -108,13 +108,13 @@ namespace RoboidControl {
         public void Add(Thing thing, bool checkId = true, bool invokeEvent = true) {
             if (checkId && thing.id == 0) {
                 thing.id = (byte)(this.things.Count + 1);
-                things.Add(thing);
+                this.things.Add(thing);
             }
             // Console.WriteLine($"added thing [{thing.networkId}/{thing.id}]"); 
             Thing foundThing = Get(thing.networkId, thing.id);
 
             if (foundThing == null) {
-                things.Add(thing);
+                this.things.Add(thing);
 
                 // if (invokeEvent)
                 //     Thing.InvokeNewThing(thing);

src/Participants/ParticipantUDP.cs

@@ -33,6 +33,7 @@ namespace RoboidControl {
         public ParticipantUDP(int port = 0) : base("127.0.0.1", port) {
             if (this.port == 0)
                 this.isIsolated = true;
+            Participant.AddParticipant(this);
         }
 
         /// <summary>
@@ -45,6 +46,7 @@ namespace RoboidControl {
                 this.isIsolated = true;
             else
                 this.remoteSite = new Participant(ipAddress, port);
+            Participant.AddParticipant(this);
 
             this.endPoint = new IPEndPoint(IPAddress.Any, localPort);
             this.udpClient = new UdpClient(localPort);
@@ -94,39 +96,22 @@ namespace RoboidControl {
 
         #region Update
 
-        protected void ReceiveUDP(IAsyncResult result) {
-            // UnityEngine.Debug.Log("received");
-            if (this.udpClient == null) // || this.endPoint == null)
-                return;
-
-            byte[] data = this.udpClient.EndReceive(result, ref endPoint);
-            // This does not yet take multi-packet messages into account!
-            if (endPoint == null)
-                return;
-
-            // We can receive our own publish (broadcast) packages. How do we recognize them????
-            // It is hard to determine our source port
-            string ipAddress = endPoint.Address.ToString();
-            Participant remoteParticipant = GetParticipant(ipAddress, endPoint.Port);
-            remoteParticipant ??= AddParticipant(ipAddress, endPoint.Port);
-
-            ReceiveData(data, remoteParticipant);
-
-            udpClient.BeginReceive(new AsyncCallback(callbackResult => ReceiveUDP(callbackResult)), null);
-        }
-
         protected ulong nextPublishMe = 0;
-        public override void Update(ulong currentTimeMS = 0) {
-            if (currentTimeMS == 0) {
-#if UNITY_5_3_OR_NEWER
-                currentTimeMS = (ulong)(UnityEngine.Time.time * 1000);
-#endif
-            }
 
-            if (this.publishInterval > 0 && currentTimeMS > this.nextPublishMe) {
-                Publish();
-                // Console.WriteLine($"{this.name} Publish ClientMsg {this.networkId}");
-                this.nextPublishMe = currentTimeMS + this.publishInterval;
+        public override void Update(ulong currentTimeMS = 0) {
+            if (currentTimeMS == 0)
+                currentTimeMS = Thing.GetTimeMs();
+
+            if (this.isIsolated == false) {
+                if (this.publishInterval > 0 && currentTimeMS > this.nextPublishMe) {
+                    ParticipantMsg msg = new ParticipantMsg(this.networkId);
+                    if (this.remoteSite == null)
+                        this.Publish(msg);
+                    else
+                        this.Send(this.remoteSite, msg);
+
+                    this.nextPublishMe = currentTimeMS + this.publishInterval;
+                }
             }
 
             UpdateMyThings(currentTimeMS);
@@ -134,21 +119,35 @@ namespace RoboidControl {
         }
 
         protected virtual void UpdateMyThings(ulong currentTimeMS) {
-            int n = this.things.Count;
-            for (int ix = 0; ix < n; ix++) {
-                Thing thing = this.things[ix];
-                if (thing == null || thing.parent != null)
+            foreach (Thing thing in this.things) {
+                if (thing == null)
                     continue;
 
-                thing.Update(currentTimeMS, true);
-                if (this.isIsolated || this.networkId == 0)
-                    continue;
+                if (thing.hierarchyChanged && !(this.isIsolated || this.networkId == 0)) {
+                    ThingMsg thingMsg = new(this.networkId, thing);
+                    this.Send(this.remoteSite, thingMsg);
+                }
 
-                // Send to remote site
-                PoseMsg poseMsg = new(thing.owner.networkId, thing);
-                this.Send(this.remoteSite, poseMsg);
-                BinaryMsg binaryMsg = new(thing.owner.networkId, thing);
-                this.Send(this.remoteSite, binaryMsg);
+                // Why don't we do recursive?
+                // Because when a thing creates a thing in the update,
+                // that new thing is not sent out (because of hierarchyChanged)
+                // before it is updated itself: it is immediatedly updated!
+                thing.Update(currentTimeMS, false);
+                if (!(this.isIsolated || this.networkId == 0)) {
+                    if (thing.terminate) {
+                        DestroyMsg destroyMsg = new(this.networkId, thing);
+                        this.Send(this.remoteSite, destroyMsg);
+                    }
+                    else {
+                        // Send to remote site
+                        PoseMsg poseMsg = new(thing.owner.networkId, thing);
+                        this.Send(this.remoteSite, poseMsg);
+                        BinaryMsg binaryMsg = new(thing.owner.networkId, thing);
+                        this.Send(this.remoteSite, binaryMsg);
+                    }
+                }
+                if (thing.terminate)
+                    this.Remove(thing);
             }
         }
 
@@ -162,7 +161,12 @@ namespace RoboidControl {
                 if (this.isIsolated)
                     continue;
 
-                foreach (Thing thing in participant.things) {
+                //foreach (Thing thing in participant.things) {
+                for (int thingIx = 0; thingIx < participant.things.Count; thingIx++) {
+                    Thing thing = participant.things[thingIx];
+                    if (thing == null)
+                        continue;
+
                     PoseMsg poseMsg = new(thing.owner.networkId, thing);
                     this.Send(participant, poseMsg);
                     BinaryMsg binaryMsg = new(thing.owner.networkId, thing);
@@ -172,10 +176,6 @@ namespace RoboidControl {
 
         }
 
-        public virtual void Publish() {
-            this.Publish(new ParticipantMsg(this.networkId));
-        }
-
         #endregion Update
 
         #region Send
@@ -239,6 +239,27 @@ namespace RoboidControl {
 
         #region Receive
 
+        protected void ReceiveUDP(IAsyncResult result) {
+            // UnityEngine.Debug.Log("received");
+            if (this.udpClient == null) // || this.endPoint == null)
+                return;
+
+            byte[] data = this.udpClient.EndReceive(result, ref endPoint);
+            // This does not yet take multi-packet messages into account!
+            if (endPoint == null)
+                return;
+
+            // We can receive our own publish (broadcast) packages. How do we recognize them????
+            // It is hard to determine our source port
+            string ipAddress = endPoint.Address.ToString();
+            Participant remoteParticipant = GetParticipant(ipAddress, endPoint.Port);
+            remoteParticipant ??= AddParticipant(ipAddress, endPoint.Port);
+
+            ReceiveData(data, remoteParticipant);
+
+            udpClient.BeginReceive(new AsyncCallback(callbackResult => ReceiveUDP(callbackResult)), null);
+        }
+
         public void ReceiveData(byte[] data, Participant sender) {
             byte msgId = data[0];
             if (msgId == 0xFF) {
@@ -277,17 +298,13 @@ namespace RoboidControl {
                     break;
                 case DestroyMsg.Id: // 0x20 / 32
                     this.Process(sender, new DestroyMsg(data));
-                 // result = await DestroyMsg.Receive(dataStream, client, packetSize);
+                    // result = await DestroyMsg.Receive(dataStream, client, packetSize);
                     break;
                 default:
                     break;
             }
         }
 
-        #endregion
-
-        #region Process
-
         protected virtual void Process(Participant sender, ParticipantMsg msg) {
 #if DEBUG
             Console.WriteLine($"{this.name} Process participantMsg {msg.networkId}");
@@ -360,9 +377,9 @@ namespace RoboidControl {
         }
 
         protected virtual void Process(Participant sender, BinaryMsg msg) {
-            // #if DEBUG            
-            //             Console.WriteLine($"Participant: Process BinaryMsg [{msg.networkId}/{msg.thingId}] {msg.dataLength}");
-            // #endif            
+#if DEBUG
+            Console.WriteLine($"Participant: Process BinaryMsg [{msg.networkId}/{msg.thingId}] {msg.dataLength}");
+#endif
             Thing thing = sender.Get(msg.networkId, msg.thingId);
             thing?.ProcessBinary(msg.data);
         }
@@ -378,7 +395,10 @@ namespace RoboidControl {
 #if DEBUG
             Console.WriteLine($"Participant: Process Destroy Msg [{msg.networkId}/{msg.thingId}]");
 #endif            
-
+            Thing thing = sender.Get(msg.networkId, msg.thingId);
+            if (thing != null)
+                this.Remove(thing);
+            thing.component.core = null;
         }
 
         private void ForwardMessage(IMessage msg) {

src/Participants/SiteServer.cs

@@ -15,6 +15,7 @@ namespace RoboidControl {
         /// <param name="port"></param>
         public SiteServer(int port = 7681) : base(port) {
             this.name = "Site Server";
+            Participant.AddParticipant(this);
 
             Console.Write($"Prepare receive on port {port}");
             this.endPoint = new IPEndPoint(IPAddress.Any, port);
@@ -30,39 +31,44 @@ namespace RoboidControl {
             this.udpClient?.Close();
         }
 
+        #region Update
+
         protected override void UpdateMyThings(ulong currentTimeMS) {
+            // We don't use foreach to prevent the 'Collection was modified' error
             int n = this.things.Count;
             for (int ix = 0; ix < n; ix++) {
                 Thing thing = this.things[ix];
-                if (thing == null || thing.parent != null)
+
+                if (thing == null)
                     continue;
 
-                thing.Update(currentTimeMS, true);
-                if (this.isIsolated || this.networkId == 0)
-                    continue;
+                thing.Update(currentTimeMS, false);
 
-                // Send to all other participants
-                foreach (Participant participant in Participant.participants) {
-                    if (participant == null || participant == this)
-                        continue;
+                if (this.isIsolated == false) {
+                    // Send to all other participants
+                    foreach (Participant participant in Participant.participants) {
+                        if (participant == null || participant == this)
+                            continue;
 
-                    PoseMsg poseMsg = new(thing.owner.networkId, thing);
-                    this.Send(participant, poseMsg);
-                    BinaryMsg binaryMsg = new(thing.owner.networkId, thing);
-                    this.Send(participant, binaryMsg);
+                        PoseMsg poseMsg = new(thing.owner.networkId, thing);
+                        this.Send(participant, poseMsg);
+                        BinaryMsg binaryMsg = new(thing.owner.networkId, thing);
+                        this.Send(participant, binaryMsg);
+                    }
                 }
             }
         }
 
-        public override void Publish() {
-        }
+        #endregion Update
+
+        #region Receive
 
         protected override void Process(Participant sender, ParticipantMsg msg) {
             base.Process(sender, msg);
-            //if (msg.networkId == 0) {
-            //Console.WriteLine($"{this.name} received New Participant -> {sender.networkId}");
-            this.Send(sender, new NetworkIdMsg(sender.networkId));
-            //}
+            if (msg.networkId != sender.networkId) {
+                //Console.WriteLine($"{this.name} received New Participant -> {sender.networkId}");
+                this.Send(sender, new NetworkIdMsg(sender.networkId));
+            }
         }
 
         protected override void Process(Participant sender, NetworkIdMsg msg) { }
@@ -70,19 +76,22 @@ namespace RoboidControl {
         protected override void Process(Participant sender, ThingMsg msg) {
             Console.WriteLine($"SiteServer: Process thing [{msg.networkId}/{msg.thingId}] {msg.thingType} {msg.parentId} ");
             Thing thing = sender.Get(msg.networkId, msg.thingId);
-            if (thing == null) {
-                thing = new Thing(sender, msg.networkId, msg.thingId, msg.thingType);
-                // Console.WriteLine("Created generic new core thing");
-                // }
-            }
+            if (thing == null)
+                thing = new Thing(sender, msg.networkId, msg.thingId, msg.thingType);            
 
             if (msg.parentId != 0) {
-                Thing parentThing = Get(msg.networkId, msg.parentId);
-                if (parentThing == null)
+                thing.parent = sender.Get(msg.networkId, msg.parentId);
+                if (thing.parent == null)
                     Console.WriteLine($"Could not find parent [{msg.networkId}/{msg.parentId}]");
-                else
-                    thing.parent = parentThing;
+            }
+            else {
+                Console.Write($"Dropped {thing.id}");
+                thing.parent = null;
             }
         }
+
+        #endregion Receive
+
     }
+
 }

src/Thing.cs

@@ -90,6 +90,7 @@ namespace RoboidControl {
             this.id = thingId;
             this.type = thingType;
             this.networkId = networkId;
+            Console.Write($"New thing added to {owner}");
             this.owner.Add(this);
             InvokeNewThing(this);
         }
@@ -145,14 +146,10 @@ namespace RoboidControl {
                 }
                 else {
                     value.AddChild(this);
-                    OnParentChanged?.Invoke();
                 }
+                this.hierarchyChanged = true;
             }
         }
-        /// <summary>
-        /// Event which is triggered when the parent changes
-        /// </summary>
-        public event ChangeHandler OnParentChanged = delegate { };
 
         /// <summary>
         /// Add a child Thing to this Thing
@@ -219,6 +216,11 @@ namespace RoboidControl {
             return null;
         }
 
+        /// <summary>
+        /// Indicator that the hierarchy of the thing has changed
+        /// </summary>
+        public bool hierarchyChanged = true;
+
         /// <summary>
         /// The children of this thing
         /// </summary>
@@ -346,10 +348,18 @@ namespace RoboidControl {
         public Unity.Thing component = null;
 #endif
 
+        public bool terminate = false;
+
         #endregion Properties
 
         #region Methods
 
+        public static ulong GetTimeMs() {
+#if UNITY_5_3_OR_NEWER
+            return (ulong)(UnityEngine.Time.time * 1000);
+#endif
+        }
+
         /// <summary>
         /// Update de state of the thing
         /// </summary>
@@ -368,7 +378,8 @@ namespace RoboidControl {
             this.positionUpdated = false;
             this.orientationUpdated = false;
             this.linearVelocityUpdated = false;
-            this.angularVelocityUpdated = false;            
+            this.angularVelocityUpdated = false;
+            //this.hierarchyChanged = false;
 
             // should recurse over children...
             if (recursively) {