From be167722e749cc3ea37ac51ec2f4b311477069c9 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Tue, 22 Apr 2025 17:47:22 +0200 Subject: [PATCH] Improved network sync --- Unity/Thing.cs | 23 +++++++----- Unity/TouchSensor.cs | 29 +++------------ src/Messages/BinaryMsg.cs | 2 +- src/Messages/DestroyMsg.cs | 5 ++- src/Messages/PoseMsg.cs | 2 -- src/Participant.cs | 2 +- src/ParticipantUDP.cs | 72 ++++++++++++++++++++++++++------------ src/SiteServer.cs | 53 ++++++++++++++++++---------- src/Thing.cs | 2 ++ src/Things/TouchSensor.cs | 16 +++++---- 10 files changed, 121 insertions(+), 85 deletions(-) diff --git a/Unity/Thing.cs b/Unity/Thing.cs index 9263c75..7804c3b 100644 --- a/Unity/Thing.cs +++ b/Unity/Thing.cs @@ -17,6 +17,8 @@ namespace RoboidControl.Unity { [field: SerializeField] public RoboidControl.Thing core { get; set; } + public SiteServer participant; + private string modelUrl = null; /// @@ -41,18 +43,22 @@ namespace RoboidControl.Unity { new("Thing") : new(core.name); Thing component = gameObj.AddComponent(); + component.Init(core); + return component; + } - component.core = core; + protected void Init(RoboidControl.Thing core) { + this.core = core; + this.participant = FindAnyObjectByType(); if (core.parent != null && core.parent.component != null) - gameObj.transform.SetParent(core.parent.component.transform, false); + this.transform.SetParent(core.parent.component.transform, false); if (core.position != null) - gameObj.transform.localPosition = core.position.ToVector3(); + this.transform.localPosition = core.position.ToVector3(); if (core.orientation != null) - gameObj.transform.localRotation = core.orientation.ToQuaternion(); + this.transform.localRotation = core.orientation.ToQuaternion(); - core.OnPoseChanged += component.PoseChanged; - return component; + core.OnPoseChanged += this.PoseChanged; } /// @@ -70,7 +76,8 @@ namespace RoboidControl.Unity { // this.transform.localPosition = core.position.ToVector3(); if (core.angularVelocity != null && core.angularVelocity.distance != 0) { - Vector3 angularVelocity = core.angularVelocity.ToVector3(); + // 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); @@ -95,7 +102,7 @@ namespace RoboidControl.Unity { } private void PoseChanged() { - // Debug.Log($"{this} pose changed"); + //Debug.Log($"{this} pose changed"); if (core.positionUpdated) this.transform.localPosition = core.position.ToVector3(); if (core.orientationUpdated) diff --git a/Unity/TouchSensor.cs b/Unity/TouchSensor.cs index bcce86e..9814bee 100644 --- a/Unity/TouchSensor.cs +++ b/Unity/TouchSensor.cs @@ -8,7 +8,7 @@ namespace RoboidControl.Unity { /// public class TouchSensor : Thing { - public SiteServer participant; + // public SiteServer participant; /// /// The core touch sensor /// @@ -24,18 +24,6 @@ namespace RoboidControl.Unity { participant = FindAnyObjectByType(); SetCoreThing(new RoboidControl.TouchSensor(participant.site)); } - // Somehow this does not work. - - // Rigidbody rb = GetComponentInParent(); - // if (rb == null) { - // RoboidControl.Thing thing = core; - // while (thing.parent != null) - // thing = thing.parent; - - // Thing unityThing = thing.component; - // rb = unityThing.gameObject.AddComponent(); - // rb.isKinematic = true; - // } } /// @@ -48,6 +36,8 @@ namespace RoboidControl.Unity { new(core.name) : new("Touch Sensor"); TouchSensor component = gameObj.AddComponent(); + component.Init(core); + Rigidbody rb = gameObj.AddComponent(); rb.isKinematic = true; @@ -55,17 +45,6 @@ namespace RoboidControl.Unity { collider.radius = 0.01f; collider.isTrigger = true; - component.core = core; - component.participant = FindAnyObjectByType(); - core.thisParticipant = component.participant.site; - - if (core.parent != null && core.parent.component != null) { - gameObj.transform.SetParent(core.parent.component.transform, false); - } - - if (core.position != null) - gameObj.transform.localPosition = core.position.ToVector3(); - if (gameObj.transform.parent != null && gameObj.transform.localPosition.magnitude > 0) { collider.radius = Vector3.Distance(gameObj.transform.position, gameObj.transform.parent.position) / 2; gameObj.transform.position = (gameObj.transform.position + gameObj.transform.parent.position) / 2; @@ -80,12 +59,14 @@ namespace RoboidControl.Unity { if (this.transform.root == other.transform.root) return; + Debug.Log($"*** {this} Touch"); this.coreSensor.touchedSomething = true; } private void OnTriggerExit(Collider other) { if (other.isTrigger) return; + Debug.Log($"*** {this} Touch end"); this.coreSensor.touchedSomething = false; } } diff --git a/src/Messages/BinaryMsg.cs b/src/Messages/BinaryMsg.cs index 676ed41..f91f496 100644 --- a/src/Messages/BinaryMsg.cs +++ b/src/Messages/BinaryMsg.cs @@ -61,7 +61,7 @@ namespace RoboidControl { return 0; #if DEBUG - // System.Console.WriteLine($"Send BinaryMsg [{this.networkId}/{this.thingId}] {this.dataLength}"); + System.Console.WriteLine($"Send BinaryMsg [{this.networkId}/{this.thingId}] {this.dataLength}"); #endif byte ix = 0; buffer[ix++] = BinaryMsg.Id; diff --git a/src/Messages/DestroyMsg.cs b/src/Messages/DestroyMsg.cs index 1963bc3..0fad15e 100644 --- a/src/Messages/DestroyMsg.cs +++ b/src/Messages/DestroyMsg.cs @@ -31,7 +31,10 @@ namespace RoboidControl { this.thingId = thingId; } /// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer) - public DestroyMsg(byte[] buffer) : base(buffer) { } + public DestroyMsg(byte[] buffer) : base(buffer) { + this.networkId = buffer[1]; + this.thingId = buffer[2]; + } /// @copydoc Passer::RoboidControl::IMessage::Serialize public override byte Serialize(ref byte[] buffer) { diff --git a/src/Messages/PoseMsg.cs b/src/Messages/PoseMsg.cs index f335cef..123a2e5 100644 --- a/src/Messages/PoseMsg.cs +++ b/src/Messages/PoseMsg.cs @@ -96,12 +96,10 @@ namespace RoboidControl { if (thing.positionUpdated || force) { this.position = thing.position; this.poseType |= Pose_Position; - //thing.positionUpdated = false; // this is also reset in Thing.update, leave it out here? } if (thing.orientationUpdated || force) { this.orientation = thing.orientation; this.poseType |= Pose_Orientation; - //thing.orientationUpdated = false; // this is also reset in Thing.update, leave it out here? } if (thing.linearVelocityUpdated) { this.linearVelocity = thing.linearVelocity; diff --git a/src/Participant.cs b/src/Participant.cs index 8a40c34..460925d 100644 --- a/src/Participant.cs +++ b/src/Participant.cs @@ -42,7 +42,7 @@ namespace RoboidControl { /// /// The things managed by this participant /// - protected readonly List things = new List(); + public readonly List things = new List(); public virtual void Update(ulong currentTimeMS = 0) { int n = this.things.Count; diff --git a/src/ParticipantUDP.cs b/src/ParticipantUDP.cs index d48ee6f..2f261b9 100644 --- a/src/ParticipantUDP.cs +++ b/src/ParticipantUDP.cs @@ -47,7 +47,7 @@ namespace RoboidControl { this.remoteSite = new Participant(ipAddress, port); this.endPoint = new IPEndPoint(IPAddress.Any, localPort); - this.udpClient = new UdpClient(localPort); + this.udpClient = new UdpClient(localPort); this.udpClient.BeginReceive(new AsyncCallback(result => ReceiveUDP(result)), null); } @@ -95,7 +95,7 @@ namespace RoboidControl { #region Update protected void ReceiveUDP(IAsyncResult result) { - // UnityEngine.Debug.Log("received"); + // UnityEngine.Debug.Log("received"); if (this.udpClient == null) // || this.endPoint == null) return; @@ -129,24 +129,47 @@ namespace RoboidControl { this.nextPublishMe = currentTimeMS + this.publishInterval; } + UpdateMyThings(currentTimeMS); + UpdateOtherThings(currentTimeMS); + } + + 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) { - thing.Update(currentTimeMS, true); - // if (this.isIsolated == false) { - if (thing.owner != this) { // should not happen.... - PoseMsg poseMsg = new(thing.owner.networkId, thing); - this.Send(thing.owner, poseMsg); - BinaryMsg binaryMsg = new(thing.owner.networkId, thing); - this.Send(thing.owner, binaryMsg); - } + if (thing == null || thing.parent != null) + continue; + + thing.Update(currentTimeMS, true); + if (this.isIsolated || this.networkId == 0) + continue; + + // 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); + } + } + + protected virtual void UpdateOtherThings(ulong currentTimeMS) { + for (int ownerIx = 0; ownerIx < Participant.participants.Count; ownerIx++) { + Participant participant = Participant.participants[ownerIx]; + if (participant == null || participant == this) + continue; + + participant.Update(currentTimeMS); + if (this.isIsolated) + continue; + + foreach (Thing thing in participant.things) { + PoseMsg poseMsg = new(thing.owner.networkId, thing); + this.Send(participant, poseMsg); + BinaryMsg binaryMsg = new(thing.owner.networkId, thing); + this.Send(participant, binaryMsg); } } - for (int ownerIx = 0; ownerIx < Participant.participants.Count; ownerIx++) { - Participant owner = Participant.participants[ownerIx]; - owner.Update(currentTimeMS); - } + } public virtual void Publish() { @@ -253,7 +276,8 @@ namespace RoboidControl { // result = await TextMsg.Receive(dataStream, client, packetSize); break; case DestroyMsg.Id: // 0x20 / 32 - // result = await DestroyMsg.Receive(dataStream, client, packetSize); + this.Process(sender, new DestroyMsg(data)); + // result = await DestroyMsg.Receive(dataStream, client, packetSize); break; default: break; @@ -324,17 +348,21 @@ namespace RoboidControl { thing.position = msg.position; if ((msg.poseType & PoseMsg.Pose_Orientation) != 0) thing.orientation = msg.orientation; - if ((msg.poseType & PoseMsg.Pose_LinearVelocity) != 0) + if ((msg.poseType & PoseMsg.Pose_LinearVelocity) != 0) { thing.linearVelocity = msg.linearVelocity; - if ((msg.poseType & PoseMsg.Pose_AngularVelocity) != 0) + //Console.Write($"linear velocity = {thing.linearVelocity.ToVector3()}"); + } + if ((msg.poseType & PoseMsg.Pose_AngularVelocity) != 0) { thing.angularVelocity = msg.angularVelocity; + //Console.Write($"angular velocity = {thing.angularVelocity.ToVector3()}"); + } } } 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); } @@ -348,7 +376,7 @@ namespace RoboidControl { protected virtual void Process(Participant sender, DestroyMsg msg) { #if DEBUG - Console.WriteLine($"Participant: Process ThingMsg [{msg.networkId}/{msg.thingId}]"); + Console.WriteLine($"Participant: Process Destroy Msg [{msg.networkId}/{msg.thingId}]"); #endif } diff --git a/src/SiteServer.cs b/src/SiteServer.cs index e6a9141..51abbfb 100644 --- a/src/SiteServer.cs +++ b/src/SiteServer.cs @@ -30,6 +30,30 @@ namespace RoboidControl { this.udpClient?.Close(); } + protected override 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) + continue; + + thing.Update(currentTimeMS, true); + if (this.isIsolated || this.networkId == 0) + continue; + + // 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); + } + } + } + public override void Publish() { } @@ -44,30 +68,21 @@ namespace RoboidControl { protected override void Process(Participant sender, NetworkIdMsg msg) { } protected override void Process(Participant sender, ThingMsg msg) { - Console.WriteLine($"SiteServer: Process thing [{msg.networkId}/{msg.thingId}]"); + 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 newThing = null; - // if (thingMsgProcessors.TryGetValue(msg.thingType, out Func msgProcessor)) { - // //Console.WriteLine("Found thing message processor"); - // if (msgProcessor != null) - // newThing = msgProcessor(sender, msg.networkId, msg.thingId); - // } - // if (newThing == null) { - newThing = new Thing(sender, msg.networkId, msg.thingId, msg.thingType); + thing = new Thing(sender, msg.networkId, msg.thingId, msg.thingType); // Console.WriteLine("Created generic new core thing"); // } - if (msg.parentId != 0) { - Thing parentThing = Get(msg.networkId, msg.parentId); - if (parentThing == null) - Console.WriteLine($"Could not find parent [{msg.networkId}/{msg.parentId}]"); - else - newThing.parent = parentThing; - } - //Console.WriteLine("Adding to remote sender"); - //sender.Add(newThing, false, true); + } + + if (msg.parentId != 0) { + Thing parentThing = Get(msg.networkId, msg.parentId); + if (parentThing == null) + Console.WriteLine($"Could not find parent [{msg.networkId}/{msg.parentId}]"); + else + thing.parent = parentThing; } } - } } \ No newline at end of file diff --git a/src/Thing.cs b/src/Thing.cs index 5f06818..4c75727 100644 --- a/src/Thing.cs +++ b/src/Thing.cs @@ -367,6 +367,8 @@ namespace RoboidControl { OnPoseChanged?.Invoke(); this.positionUpdated = false; this.orientationUpdated = false; + this.linearVelocityUpdated = false; + this.angularVelocityUpdated = false; // should recurse over children... if (recursively) { diff --git a/src/Things/TouchSensor.cs b/src/Things/TouchSensor.cs index 915a4a3..88cb88d 100644 --- a/src/Things/TouchSensor.cs +++ b/src/Things/TouchSensor.cs @@ -19,7 +19,7 @@ namespace RoboidControl { } public TouchSensor(Participant owner, byte networkId, byte thingId) : base(owner, networkId, thingId) { - Console.Write("TouchSensor constructor"); + // Console.Write("TouchSensor constructor"); //touchedSomething = false; //thisParticipant = participant; } @@ -36,15 +36,13 @@ namespace RoboidControl { public bool touchedSomething { get { return _touchedSomething; } set { - _touchedSomething = value; - if (thisParticipant != null && this.owner != thisParticipant) { - BinaryMsg msg = new(networkId, this); - this.thisParticipant.Send(this.owner, msg); - // foreach (Participant remoteParticipant in thisParticipant.participants) - // thisParticipant.Send(remoteParticipant, msg); + if (_touchedSomething != value) { + touchUpdated = true; + _touchedSomething = value; } } } + private bool touchUpdated = false; #if UNITY_5_3_OR_NEWER /// @copydoc Passer::RoboidControl::Thing::CreateComponent @@ -55,8 +53,12 @@ namespace RoboidControl { } #endif public override byte[] GenerateBinary() { + if (!touchUpdated) + return new byte[0]; + byte[] buffer = new byte[1]; buffer[0] = (byte)(touchedSomething ? 1 : 0); + touchUpdated = false; return buffer; } }