Fix issue with things from different participants

Unity/DebugConsole.cs

@@ -1,4 +1,4 @@
-#if UNITY_5_3_OR_NEWER
+#if UNITY_5_3_OR_NEWER && RC_DEBUG
 using System.IO;
 using System.Text;
 using UnityEngine;

Unity/DifferentialDrive.cs

@@ -125,7 +125,6 @@ namespace RoboidControl.Unity {
 
                     // Use smoothing to emulate motor inertia
                     rb.velocity = 0.9f * rb.velocity + 0.1f * forwardSpeed * transform.forward;
-                    Debug.Log(rb.velocity);
                     rb.angularVelocity = 0.9f * rb.angularVelocity + 0.1f * turningSpeed * Vector3.up;
                 }
             }

Unity/Participant.cs

@@ -48,7 +48,11 @@ namespace RoboidControl.Unity {
                     if (coreThing.component == null) {
                         Thing[] things = FindObjectsByType<Thing>(FindObjectsSortMode.None);
                         // Debug.Log(things.Length);
-                        Thing thing = things.FirstOrDefault(t => t.core != null && t.core.id == coreThing.id);
+                        Thing thing = things.FirstOrDefault(t =>
+                            t.core != null &&
+                            t.core.owner.networkId == coreThing.owner.networkId &&
+                            t.core.id == coreThing.id
+                        );
                         if (thing == null)
                             thing = Thing.Create(coreThing);
                         coreThing.component = thing;

Unity/SiteServer.cs

@@ -1,21 +1,30 @@
 #if UNITY_5_3_OR_NEWER
 using System;
-using System.Collections.Generic;
 using UnityEngine;
 
 namespace RoboidControl.Unity {
 
     public class SiteServer : Participant {
 
-        //public RoboidControl.SiteServer site;
         public RoboidControl.SiteServer site => this.coreParticipant as RoboidControl.SiteServer;
-
+        public string modelURL;
-        //public Queue<RoboidControl.Thing> thingQueue = new();
+        private RoboidControl.Thing model;
 
         protected virtual void Awake() {
+#if RC_DEBUG
             Console.SetOut(new UnityLogWriter());
+#endif            
 
             this.coreParticipant = new RoboidControl.SiteServer(port);
+
+#if GLTF
+            if (!string.IsNullOrEmpty(modelURL)) {
+                model = new() {
+                    name = "Model",
+                    modelUrl = this.modelURL
+                };
+            }
+#endif            
         }
 
         void OnApplicationQuit() {
@@ -31,8 +40,6 @@ namespace RoboidControl.Unity {
                 HandleUpdateEvent(e);
 
             site.Update();
-            // while (thingQueue.TryDequeue(out RoboidControl.Thing thing))
-            //     thing.CreateComponent();
         }
 
         private void HandleUpdateEvent(RoboidControl.Participant.UpdateEvent e) {
@@ -44,6 +51,10 @@ namespace RoboidControl.Unity {
                     participant.coreParticipant.component = participant;
                     participant.ipAddress = e.participant.ipAddress;
                     participant.port = e.participant.port;
+
+                    foreach (RoboidControl.Thing thing in this.site.things)
+                        participant.coreParticipant.SendThingInfo(thing);
+
                     break;
                 case ThingMsg.id:
                     HandleThingEvent(e);

src/Messages/PoseMsg.cs

@@ -14,7 +14,7 @@ namespace RoboidControl {
         /// <summary>
         /// The length of the message
         /// </summary>
-        public const byte length = 4 + 4 + 4;
+        public byte length = 4 + 4 + 4;
         /// <summary>
         /// The network ID of the thing
         /// </summary>
@@ -108,12 +108,12 @@ namespace RoboidControl {
             if ((this.poseType & Pose_Position) != 0)
                 this.position = LowLevelMessages.ReceiveSpherical(buffer, ref ix);
             if ((this.poseType & Pose_Orientation) != 0)
-                //this.orientation = SwingTwist.FromQuat32(LowLevelMessages.ReceiveQuat32(buffer, ref ix));
                 this.orientation = LowLevelMessages.ReceiveSwingTwist(buffer, ref ix);
             if ((this.poseType & Pose_LinearVelocity) != 0)
-                    this.linearVelocity = LowLevelMessages.ReceiveSpherical(buffer, ref ix);
+                this.linearVelocity = LowLevelMessages.ReceiveSpherical(buffer, ref ix);
             if ((this.poseType & Pose_AngularVelocity) != 0)
                 this.angularVelocity = LowLevelMessages.ReceiveSpherical(buffer, ref ix);
+            this.length = ix;
         }
 
         /// @copydoc Passer::RoboidControl::IMessage::Serialize
@@ -133,7 +133,6 @@ namespace RoboidControl {
             if ((poseType & Pose_Position) != 0)
                 LowLevelMessages.SendSpherical(buffer, ref ix, this.position);
             if ((poseType & Pose_Orientation) != 0)
-                //LowLevelMessages.SendQuat32(buffer, ref ix, this.orientation);
                 LowLevelMessages.SendSwingTwist(buffer, ref ix, this.orientation);
             if ((poseType & Pose_LinearVelocity) != 0)
                     LowLevelMessages.SendSpherical(buffer, ref ix, this.linearVelocity);

src/Participant.cs

@@ -191,6 +191,14 @@ namespace RoboidControl {
             return true;
         }
 
+        public void SendThingInfo(Thing thing) {
+            this.Send(new ThingMsg(thing.owner.networkId, thing));
+            this.Send(new NameMsg(thing.owner.networkId, thing));
+            this.Send(new ModelUrlMsg(thing.owner.networkId, thing));
+            this.Send(new PoseMsg(thing.owner.networkId, thing));
+            this.Send(new BinaryMsg(thing.owner.networkId, thing));
+        }
+
         #endregion Send
 
         #region Participant Registry

src/Participants/ParticipantUDP.cs

@@ -107,7 +107,7 @@ namespace RoboidControl {
         /// The interval in milliseconds for publishing (broadcasting) data on the local network
         /// </summary>
         public ulong publishIntervalMS = 3000; // = 3 seconds
-        public ulong sendUpdateIntervalMS = 100; // for object updates
+        public ulong sendUpdateIntervalMS = 100; // = 0.1 seconds for object updates
 
         //public byte[] buffer = new byte[1024];
 
@@ -219,8 +219,8 @@ namespace RoboidControl {
                         // this.Send(participant, poseMsg);
                         // BinaryMsg binaryMsg = new(thing.owner.networkId, thing);
                         // this.Send(participant, binaryMsg);
-                        participant.Send(new PoseMsg(thing.owner.networkId, thing));
+                        // participant.Send(new PoseMsg(thing.owner.networkId, thing));
-                        participant.Send(new BinaryMsg(thing.owner.networkId, thing));
+                        // participant.Send(new BinaryMsg(thing.owner.networkId, thing));
                     }
                     this.nextSendUpdate = currentTimeMS + this.sendUpdateIntervalMS;
                 }
@@ -232,14 +232,6 @@ namespace RoboidControl {
 
         #region Send
 
-        public void SendThingInfo(Participant owner, Thing thing) {
-            owner.Send(new ThingMsg(this.networkId, thing));
-            owner.Send(new NameMsg(this.networkId, thing));
-            owner.Send(new ModelUrlMsg(this.networkId, thing));
-            owner.Send(new PoseMsg(this.networkId, thing));
-            owner.Send(new BinaryMsg(this.networkId, thing));
-        }
-
         public void PublishThingInfo(Thing thing) {
             // Console.WriteLine("Publish thing info");
             this.Publish(new ThingMsg(this.networkId, thing));
@@ -305,37 +297,58 @@ namespace RoboidControl {
         }
 
         public void ReceiveData(byte[] data, Participant sender) {
-            byte msgId = data[0];
+            int dataIx = 0;
+            byte msgId = data[dataIx];
             if (msgId == 0xFF) {
                 // Timeout
                 return;
             }
 
             switch (msgId) {
-                case ParticipantMsg.Id: // 0xA0 / 160
+                case ParticipantMsg.Id: { // 0xA0 / 160
-                    this.Process(sender, new ParticipantMsg(data));
+                        ParticipantMsg msg = new(data);
+                        this.Process(sender, msg);
+                        dataIx += ParticipantMsg.length;
+                    }
                     break;
-                case NetworkIdMsg.Id: // 0xA1 / 161
+                case NetworkIdMsg.Id: {// 0xA1 / 161
-                    this.Process(sender, new NetworkIdMsg(data));
+                        NetworkIdMsg msg = new(data);
+                        this.Process(sender, msg);
+                        dataIx += NetworkIdMsg.length;
+                    }
                     break;
                 case InvestigateMsg.Id: // 0x81
                     // result = await InvestigateMsg.Receive(dataStream, client, packetSize);
                     break;
-                case ThingMsg.id: // 0x80 / 128
+                case ThingMsg.id: { // 0x80 / 128
-                    this.Process(sender, new ThingMsg(data));
+                        ThingMsg msg = new(data);
+                        this.Process(sender, msg);
+                        dataIx += ThingMsg.length;
+                    }
                     break;
-                case NameMsg.Id: // 0x91 / 145
+                case NameMsg.Id: { // 0x91 / 145
-                    this.Process(sender, new NameMsg(data));
+                        NameMsg msg = new(data);
+                        this.Process(sender, msg);
+                        dataIx += NameMsg.length + msg.nameLength;
+                    }
                     break;
-                case ModelUrlMsg.Id: // 0x90 / 144
+                case ModelUrlMsg.Id: { // 0x90 / 144
-                    this.Process(sender, new ModelUrlMsg(data));
+                        ModelUrlMsg msg = new(data);
+                        this.Process(sender, msg);
+                        dataIx += ModelUrlMsg.length + msg.urlLength;
+                    }
                     break;
-                case PoseMsg.Id: // 0x10 / 16
+                case PoseMsg.Id: { // 0x10 / 16
-                    this.Process(sender, new PoseMsg(data));
+                        PoseMsg msg = new(data);
-                    // result = await PoseMsg.Receive(dataStream, client, packetSize);
+                        this.Process(sender, msg);
+                        dataIx += msg.length;
+                    }
                     break;
-                case BinaryMsg.Id: // 0xB1 / 177
+                case BinaryMsg.Id: { // 0xB1 / 177
-                    this.Process(sender, new BinaryMsg(data));
+                        BinaryMsg msg = new(data);
+                        this.Process(sender, msg);
+                        dataIx += BinaryMsg.length + msg.dataLength;
+                    }
                     break;
                 case TextMsg.Id: // 0xB0 / 176
                     // result = await TextMsg.Receive(dataStream, client, packetSize);
@@ -347,6 +360,8 @@ namespace RoboidControl {
                 default:
                     break;
             }
+            if (dataIx < data.Length)
+                Console.WriteLine($"####### Buffer not fully read, remaining {data.Length - dataIx}");
         }
 
         protected virtual void Process(Participant sender, ParticipantMsg msg) {
@@ -363,7 +378,7 @@ namespace RoboidControl {
             if (this.networkId != msg.networkId) {
                 this.networkId = msg.networkId;
                 foreach (Thing thing in this.things) //Thing.GetAllThings())
-                    this.SendThingInfo(sender, thing);
+                    sender.SendThingInfo(thing);
             }
         }
 
@@ -401,7 +416,7 @@ namespace RoboidControl {
 
         protected virtual void Process(Participant sender, PoseMsg msg) {
 #if DEBUG            
-//            Console.WriteLine($"{this.name}: Process PoseMsg [{msg.networkId}/{msg.thingId}] {msg.poseType}");
+            Console.WriteLine($"{this.name}: Process PoseMsg [{msg.networkId}/{msg.thingId}] {msg.poseType}");
 #endif            
             Participant owner = Participant.GetParticipant(msg.networkId);
             if (owner == null)

src/Things/DifferentialDrive.cs

@@ -14,7 +14,7 @@ namespace RoboidControl {
         /// <param name="parent">The parent thing</param>
         public DifferentialDrive(Thing parent = default) : base(parent) {
             this.type = Type.DifferentialDrive;
-         }
+        }
 
         /// @brief Configures the dimensions of the drive
         /// @param wheelDiameter The diameter of the wheels in meters
@@ -49,7 +49,7 @@ namespace RoboidControl {
             //     this.leftWheel.position = new Spherical(distance, Direction.left);
 
             this.rightWheel = rightWheel;
-            this.rightWheel.parent= this;
+            this.rightWheel.parent = this;
             // if (this.rightWheel != null)
             //     this.rightWheel.position = new Spherical(distance, Direction.right);
 
@@ -139,8 +139,12 @@ namespace RoboidControl {
             byte ix = 0;
             byte leftWheelId = data[ix++];
             this.leftWheel = this.owner.Get(leftWheelId) as Motor;
+            this.leftWheel ??= new Motor(this) { id = leftWheelId };
+
             byte rightWheelId = data[ix++];
             this.rightWheel = this.owner.Get(rightWheelId) as Motor;
+            this.rightWheel ??= new Motor(this) { id = rightWheelId };
+
             this._wheelRadius = LowLevelMessages.ReceiveFloat16(data, ref ix);
             //this._wheelSeparation = LowLevelMessages.ReceiveFloat16(data, ref ix);
             this.updateQueue.Enqueue(new CoreEvent(BinaryMsg.Id));

src/Things/Motor.cs

@@ -40,7 +40,8 @@ namespace RoboidControl {
             return data;
         }
         public override void ProcessBinary(byte[] data) {
-            this.targetSpeed = (float)data[0] / 127;
+            this._targetSpeed = (float)(sbyte)data[0] / 127;
+            updateQueue.Enqueue(new CoreEvent(BinaryMsg.Id));
         }
     }