Site forwarding other participants' things (but not working properly)

Unity/DifferentialDrive.cs

@@ -101,11 +101,11 @@ namespace RoboidControl.Unity {
                     motor = Motor.Create(coreMotor);
                 wheel.transform.SetParent(motor.transform);
             }
-                else if (motor.core.id != coreMotor.id) {
+            else if (motor.core.id != coreMotor.id) {
-                    motor = coreMotor.component as Motor;
+                motor = coreMotor.component as Motor;
-                    if (motor != null)
+                if (motor != null)
-                        wheel.transform.SetParent(motor.transform);
+                    wheel.transform.SetParent(motor.transform);
-                }
+            }
         }
 
         /// <summary>
@@ -126,6 +126,10 @@ namespace RoboidControl.Unity {
                     // Use smoothing to emulate motor inertia
                     rb.velocity = 0.9f * rb.velocity + 0.1f * forwardSpeed * transform.forward;
                     rb.angularVelocity = 0.9f * rb.angularVelocity + 0.1f * turningSpeed * Vector3.up;
+
+                    core.position = LinearAlgebra.Spherical.FromVector3(this.transform.localPosition);
+                    core.orientation = LinearAlgebra.SwingTwist.FromQuaternion(this.transform.localRotation);
+
                 }
             }
         }

Unity/Resources/DifferentialDrive.prefab

@@ -205,7 +205,7 @@ MonoBehaviour:
   m_Name: 
   m_EditorClassIdentifier: 
   owner: {fileID: 0}
-  wheelRadius: 0
+  wheelRadius: 0.025
 --- !u!135 &3720747953092717687
 SphereCollider:
   m_ObjectHideFlags: 0
@@ -281,7 +281,7 @@ MonoBehaviour:
   rightMotor: {fileID: 0}
   leftWheel: {fileID: 6773972788910618332}
   rightWheel: {fileID: 7425139233737877139}
-  casterWheel: {fileID: 0}
+  casterWheel: {fileID: 894333789048616995}
 --- !u!54 &4118868690347991998
 Rigidbody:
   m_ObjectHideFlags: 0
@@ -439,7 +439,7 @@ MonoBehaviour:
   m_Name: 
   m_EditorClassIdentifier: 
   owner: {fileID: 0}
-  wheelRadius: 0
+  wheelRadius: 0.025
 --- !u!135 &8150853676422519226
 SphereCollider:
   m_ObjectHideFlags: 0

Unity/Resources/TouchSensor.prefab

@@ -92,7 +92,7 @@ GameObject:
   serializedVersion: 6
   m_Component:
   - component: {fileID: 839661735326876684}
-  - component: {fileID: 1922171662874522792}
+  - component: {fileID: 4006928432304472165}
   - component: {fileID: 5295705618145761524}
   - component: {fileID: 5884202444353424411}
   m_Layer: 0
@@ -118,7 +118,7 @@ Transform:
   - {fileID: 1122722937167266542}
   m_Father: {fileID: 0}
   m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
---- !u!114 &1922171662874522792
+--- !u!114 &4006928432304472165
 MonoBehaviour:
   m_ObjectHideFlags: 0
   m_CorrespondingSourceObject: {fileID: 0}
@@ -127,7 +127,7 @@ MonoBehaviour:
   m_GameObject: {fileID: 6521541507066528382}
   m_Enabled: 1
   m_EditorHideFlags: 0
-  m_Script: {fileID: 11500000, guid: 74fd22179b1a3ac4186b7dc083d24e06, type: 3}
+  m_Script: {fileID: 11500000, guid: ab969221c3007d441b18dd7387cf22d4, type: 3}
   m_Name: 
   m_EditorClassIdentifier: 
   owner: {fileID: 0}

Unity/SiteServer.cs

@@ -54,8 +54,8 @@ namespace RoboidControl.Unity {
                         participant.port = participantUDP.port;
                     }
 
-                    foreach (RoboidControl.Thing thing in this.site.things)
+                    // foreach (RoboidControl.Thing thing in this.site.things)
-                        participant.coreParticipant.SendThingInfo(thing);
+                    //     participant.coreParticipant.SendThingInfo(thing);
 
                     break;
                 case ThingMsg.id:

Unity/Thing.cs

@@ -74,11 +74,13 @@ namespace RoboidControl.Unity {
             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);
+                // core.position = LinearAlgebra.Spherical.FromVector3(this.transform.localPosition);                
             }
 
             if (core.angularVelocity != null && core.angularVelocity.distance != 0) {
                 Vector3 axis = core.angularVelocity.direction.ToVector3();
                 this.transform.localRotation *= Quaternion.AngleAxis(core.angularVelocity.distance * Time.deltaTime, axis);
+                // core.orientation = LinearAlgebra.SwingTwist.FromQuaternion(this.transform.localRotation);
             }
         }
 

src/Participants/ParticipantUDP.cs

@@ -228,30 +228,33 @@ namespace RoboidControl {
 
         protected virtual void UpdateOtherThings(ulong currentTimeMS) {
             for (int ownerIx = 0; ownerIx < Participant.participants.Count; ownerIx++) {
-                Participant participant = Participant.participants[ownerIx];
+                Participant owner = Participant.participants[ownerIx];
-                if (participant == null || participant == this)
+                if (owner == null || owner == this)
                     continue;
 
-                participant.Update();
+                owner.Update();
-                if (this.isIsolated)
+                // if (this.isIsolated)
-                    continue;
+                //     continue;
 
-                // if (currentTimeMS > this.nextSendUpdate) {
+                if (currentTimeMS > this.nextSendUpdate) {
-                //     for (int thingIx = 0; thingIx < participant.things.Count; thingIx++) {
+                    for (int thingIx = 0; thingIx < owner.things.Count; thingIx++) {
-                //         Thing thing = participant.things[thingIx];
+                        Thing thing = owner.things[thingIx];
-                //         if (thing == null)
+                        if (thing == null)
-                //             continue;
+                            continue;
 
-                //         // PoseMsg poseMsg = new(thing.owner.networkId, thing);
+                        foreach (Participant participant in Participant.participants) {
-                //         // this.Send(participant, poseMsg);
+                            if (participant == this || participant == owner)
-                //         // BinaryMsg binaryMsg = new(thing.owner.networkId, thing);
+                                continue;
-                //         // this.Send(participant, binaryMsg);
+
-                //         // participant.Send(new PoseMsg(thing.owner.networkId, thing));
+                            Console.WriteLine($"[{thing.owner.networkId}/{thing.id}] Pose ---> {participant.networkId}");
-                //         // participant.Send(new BinaryMsg(thing.owner.networkId, thing));
+                            PoseMsg poseMsg = new(thing.owner.networkId, thing, true);
-                //     }
+                            participant.Send(poseMsg);
-                //     this.nextSendUpdate = currentTimeMS + this.sendUpdateIntervalMS;
+                        }
-                // }
+                    }
+                }
             }
+            if (currentTimeMS > this.nextSendUpdate)
+                this.nextSendUpdate = currentTimeMS + this.sendUpdateIntervalMS;
         }
 
         #endregion Update
@@ -533,8 +536,6 @@ namespace RoboidControl {
                 thing.linearVelocity = msg.linearVelocity;
             if ((msg.poseType & PoseMsg.Pose_AngularVelocity) != 0)
                 thing.angularVelocity = msg.angularVelocity;
-
-            ForwardMessage(sender, msg);
         }
 
         protected virtual void Process(Participant sender, BinaryMsg msg) {

src/Participants/SiteServer.cs

@@ -101,14 +101,41 @@ namespace RoboidControl {
 
         protected override void Process(Participant sender, ParticipantMsg msg) {
             base.Process(sender, msg);
-            if (msg.networkId != sender.networkId) {
+            if (msg.networkId == sender.networkId)
-                //Console.WriteLine($"{this.name} received New Participant -> {sender.networkId}");
+                return;
-                sender.Send(new NetworkIdMsg(sender.networkId));
+
-                UpdateEvent e = new() {
+            if (sender.component != null) {
-                    messageId = ParticipantMsg.Id,
+                Console.WriteLine("Already created Unity participant");
-                    participant = sender
+                return;
-                };
+            }
-                this.updateQueue.Enqueue(e);
+
+            sender.Send(new NetworkIdMsg(sender.networkId));
+            UpdateEvent e = new() {
+                messageId = ParticipantMsg.Id,
+                participant = sender
+            };
+            this.updateQueue.Enqueue(e);
+
+            foreach (Thing thing in this.things)
+                sender.SendThingInfo(thing);
+
+            foreach (Participant owner in Participant.participants) {
+                if (owner == null || owner == this)
+                    continue;
+
+                Console.WriteLine($"send thinginfo for {owner.things.Count} things");
+                foreach (Thing thing in owner.things) {
+                    if (thing == null)
+                        continue;
+
+                    foreach (Participant participant in Participant.participants) {
+                        if (participant == this || participant == owner)
+                            continue;
+
+                        Console.WriteLine($"[{thing.owner.networkId}/{thing.id}] Thing Info ---> {participant.networkId}");
+                        participant.SendThingInfo(thing);
+                    }
+                }
             }
         }