From 765231588fda1c0df60bcbdce1797d72a896bce4 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 11 Jun 2025 13:52:52 +0200 Subject: [PATCH] Site forwarding other participants' things (but not working properly) --- Unity/DifferentialDrive.cs | 14 +++++--- Unity/Resources/DifferentialDrive.prefab | 6 ++-- Unity/Resources/TouchSensor.prefab | 6 ++-- Unity/SiteServer.cs | 4 +-- Unity/Thing.cs | 2 ++ src/Participants/ParticipantUDP.cs | 43 ++++++++++++------------ src/Participants/SiteServer.cs | 43 +++++++++++++++++++----- 7 files changed, 76 insertions(+), 42 deletions(-) diff --git a/Unity/DifferentialDrive.cs b/Unity/DifferentialDrive.cs index 3ea68bf..01dcbb5 100644 --- a/Unity/DifferentialDrive.cs +++ b/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) { - motor = coreMotor.component as Motor; - if (motor != null) - wheel.transform.SetParent(motor.transform); - } + else if (motor.core.id != coreMotor.id) { + motor = coreMotor.component as Motor; + if (motor != null) + wheel.transform.SetParent(motor.transform); + } } /// @@ -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); + } } } diff --git a/Unity/Resources/DifferentialDrive.prefab b/Unity/Resources/DifferentialDrive.prefab index 2426de9..f1dc99f 100644 --- a/Unity/Resources/DifferentialDrive.prefab +++ b/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 diff --git a/Unity/Resources/TouchSensor.prefab b/Unity/Resources/TouchSensor.prefab index e8c5f02..2660e1a 100644 --- a/Unity/Resources/TouchSensor.prefab +++ b/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} diff --git a/Unity/SiteServer.cs b/Unity/SiteServer.cs index 4de91f5..fb99863 100644 --- a/Unity/SiteServer.cs +++ b/Unity/SiteServer.cs @@ -54,8 +54,8 @@ namespace RoboidControl.Unity { participant.port = participantUDP.port; } - foreach (RoboidControl.Thing thing in this.site.things) - participant.coreParticipant.SendThingInfo(thing); + // foreach (RoboidControl.Thing thing in this.site.things) + // participant.coreParticipant.SendThingInfo(thing); break; case ThingMsg.id: diff --git a/Unity/Thing.cs b/Unity/Thing.cs index 2e94935..db37889 100644 --- a/Unity/Thing.cs +++ b/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); } } diff --git a/src/Participants/ParticipantUDP.cs b/src/Participants/ParticipantUDP.cs index 9b0f585..76f6119 100644 --- a/src/Participants/ParticipantUDP.cs +++ b/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]; - if (participant == null || participant == this) + Participant owner = Participant.participants[ownerIx]; + if (owner == null || owner == this) continue; - participant.Update(); - if (this.isIsolated) - continue; + owner.Update(); + // if (this.isIsolated) + // continue; - // if (currentTimeMS > this.nextSendUpdate) { - // for (int thingIx = 0; thingIx < participant.things.Count; thingIx++) { - // Thing thing = participant.things[thingIx]; - // if (thing == null) - // continue; + if (currentTimeMS > this.nextSendUpdate) { + for (int thingIx = 0; thingIx < owner.things.Count; thingIx++) { + Thing thing = owner.things[thingIx]; + if (thing == null) + continue; - // // PoseMsg poseMsg = new(thing.owner.networkId, thing); - // // 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 BinaryMsg(thing.owner.networkId, thing)); - // } - // this.nextSendUpdate = currentTimeMS + this.sendUpdateIntervalMS; - // } + foreach (Participant participant in Participant.participants) { + if (participant == this || participant == owner) + continue; + + Console.WriteLine($"[{thing.owner.networkId}/{thing.id}] Pose ---> {participant.networkId}"); + PoseMsg poseMsg = new(thing.owner.networkId, thing, true); + participant.Send(poseMsg); + } + } + } } + 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) { diff --git a/src/Participants/SiteServer.cs b/src/Participants/SiteServer.cs index fb23c1e..55df31c 100644 --- a/src/Participants/SiteServer.cs +++ b/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) { - //Console.WriteLine($"{this.name} received New Participant -> {sender.networkId}"); - sender.Send(new NetworkIdMsg(sender.networkId)); - UpdateEvent e = new() { - messageId = ParticipantMsg.Id, - participant = sender - }; - this.updateQueue.Enqueue(e); + if (msg.networkId == sender.networkId) + return; + + if (sender.component != null) { + Console.WriteLine("Already created Unity participant"); + return; + } + + 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); + } + } } }