Unity/DebugConsole.cs

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

Unity/DifferentialDrive.cs

@@ -125,6 +125,7 @@ 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,11 +48,7 @@ namespace RoboidControl.Unity {
                     if (coreThing.component == null) {
                         Thing[] things = FindObjectsByType<Thing>(FindObjectsSortMode.None);
                         // Debug.Log(things.Length);
-                        Thing thing = things.FirstOrDefault(t =>
+                        Thing thing = things.FirstOrDefault(t => t.core != null && t.core.id == coreThing.id);
-                            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,30 +1,21 @@
 #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;
+
-        private RoboidControl.Thing model;
+        //public Queue<RoboidControl.Thing> thingQueue = new();
 
         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() {
@@ -40,6 +31,8 @@ namespace RoboidControl.Unity {
                 HandleUpdateEvent(e);
 
             site.Update();
+            // while (thingQueue.TryDequeue(out RoboidControl.Thing thing))
+            //     thing.CreateComponent();
         }
 
         private void HandleUpdateEvent(RoboidControl.Participant.UpdateEvent e) {
@@ -51,10 +44,6 @@ 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 byte length = 4 + 4 + 4;
+        public const 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);
             if ((this.poseType & Pose_AngularVelocity) != 0)
                 this.angularVelocity = LowLevelMessages.ReceiveSpherical(buffer, ref ix);
-            this.length = ix;
         }
 
         /// @copydoc Passer::RoboidControl::IMessage::Serialize
@@ -133,6 +133,7 @@ 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,14 +191,6 @@ 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; // = 0.1 seconds for object updates
+        public ulong sendUpdateIntervalMS = 100; // 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,6 +232,14 @@ 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));
@@ -297,58 +305,37 @@ namespace RoboidControl {
         }
 
         public void ReceiveData(byte[] data, Participant sender) {
-            int dataIx = 0;
+            byte msgId = data[0];
-            byte msgId = data[dataIx];
             if (msgId == 0xFF) {
                 // Timeout
                 return;
             }
 
             switch (msgId) {
-                case ParticipantMsg.Id: { // 0xA0 / 160
+                case ParticipantMsg.Id: // 0xA0 / 160
-                        ParticipantMsg msg = new(data);
+                    this.Process(sender, new ParticipantMsg(data));
-                        this.Process(sender, msg);
-                        dataIx += ParticipantMsg.length;
-                    }
                     break;
-                case NetworkIdMsg.Id: {// 0xA1 / 161
+                case NetworkIdMsg.Id: // 0xA1 / 161
-                        NetworkIdMsg msg = new(data);
+                    this.Process(sender, new NetworkIdMsg(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
-                        ThingMsg msg = new(data);
+                    this.Process(sender, new ThingMsg(data));
-                        this.Process(sender, msg);
-                        dataIx += ThingMsg.length;
-                    }
                     break;
-                case NameMsg.Id: { // 0x91 / 145
+                case NameMsg.Id: // 0x91 / 145
-                        NameMsg msg = new(data);
+                    this.Process(sender, new NameMsg(data));
-                        this.Process(sender, msg);
-                        dataIx += NameMsg.length + msg.nameLength;
-                    }
                     break;
-                case ModelUrlMsg.Id: { // 0x90 / 144
+                case ModelUrlMsg.Id: // 0x90 / 144
-                        ModelUrlMsg msg = new(data);
+                    this.Process(sender, new ModelUrlMsg(data));
-                        this.Process(sender, msg);
-                        dataIx += ModelUrlMsg.length + msg.urlLength;
-                    }
                     break;
-                case PoseMsg.Id: { // 0x10 / 16
+                case PoseMsg.Id: // 0x10 / 16
-                        PoseMsg msg = new(data);
+                    this.Process(sender, new PoseMsg(data));
-                        this.Process(sender, msg);
+                    // result = await PoseMsg.Receive(dataStream, client, packetSize);
-                        dataIx += msg.length;
-                    }
                     break;
-                case BinaryMsg.Id: { // 0xB1 / 177
+                case BinaryMsg.Id: // 0xB1 / 177
-                        BinaryMsg msg = new(data);
+                    this.Process(sender, new BinaryMsg(data));
-                        this.Process(sender, msg);
-                        dataIx += BinaryMsg.length + msg.dataLength;
-                    }
                     break;
                 case TextMsg.Id: // 0xB0 / 176
                     // result = await TextMsg.Receive(dataStream, client, packetSize);
@@ -360,8 +347,6 @@ 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) {
@@ -378,7 +363,7 @@ namespace RoboidControl {
             if (this.networkId != msg.networkId) {
                 this.networkId = msg.networkId;
                 foreach (Thing thing in this.things) //Thing.GetAllThings())
-                    sender.SendThingInfo(thing);
+                    this.SendThingInfo(sender, thing);
             }
         }
 
@@ -416,7 +401,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

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