From 027c0c7b80ff523b0f7d88bd47e18f93fe068837 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Fri, 6 Jun 2025 17:20:39 +0200 Subject: [PATCH] Moved ip/port out of participant to participantUDP --- Unity/Participant.cs | 6 +- Unity/SiteServer.cs | 9 +- Unity/Thing.cs | 12 +- src/Participant.cs | 83 +++++++------ src/Participants/ParticipantUDP.cs | 185 ++++++++++++++++++++--------- src/Participants/SiteServer.cs | 7 ++ src/Thing.cs | 3 +- 7 files changed, 196 insertions(+), 109 deletions(-) diff --git a/Unity/Participant.cs b/Unity/Participant.cs index b7aec55..5c324a5 100644 --- a/Unity/Participant.cs +++ b/Unity/Participant.cs @@ -22,18 +22,19 @@ namespace RoboidControl.Unity { case ThingMsg.id: HandleThingEvent(e); break; + default: + Debug.Log($"Unhandled event: {e.messageId}"); + break; } } protected virtual void HandleThingEvent(RoboidControl.Participant.UpdateEvent e) { switch (e.thing) { case RoboidControl.TouchSensor coreTouchSensor: - Debug.Log("Handle TouchSensor"); TouchSensor touchSensor = TouchSensor.Create(coreTouchSensor); coreTouchSensor.component = touchSensor; break; case RoboidControl.DifferentialDrive coreDrive: - Debug.Log("Handle Diff.Drive"); DifferentialDrive differentialDrive = DifferentialDrive.Create(coreDrive); coreDrive.component = differentialDrive; break; @@ -44,7 +45,6 @@ namespace RoboidControl.Unity { // before we can create the wheel reliably break; case RoboidControl.Thing coreThing: - // Debug.Log("Handle Thing"); if (coreThing.component == null) { Thing[] things = FindObjectsByType(FindObjectsSortMode.None); // Debug.Log(things.Length); diff --git a/Unity/SiteServer.cs b/Unity/SiteServer.cs index 522e3af..d428b01 100644 --- a/Unity/SiteServer.cs +++ b/Unity/SiteServer.cs @@ -49,11 +49,14 @@ namespace RoboidControl.Unity { Participant participant = remoteParticipant.AddComponent(); participant.coreParticipant = e.participant; participant.coreParticipant.component = participant; - participant.ipAddress = e.participant.ipAddress; - participant.port = e.participant.port; + ParticipantUDP participantUDP = participant.coreParticipant as ParticipantUDP; + if (participantUDP != null) { + participant.ipAddress = participantUDP.ipAddress; + participant.port = participantUDP.port; + } foreach (RoboidControl.Thing thing in this.site.things) - participant.coreParticipant.SendThingInfo(thing); + participant.coreParticipant.SendThingInfo(thing); break; case ThingMsg.id: diff --git a/Unity/Thing.cs b/Unity/Thing.cs index c1597ab..2e94935 100644 --- a/Unity/Thing.cs +++ b/Unity/Thing.cs @@ -29,7 +29,6 @@ namespace RoboidControl.Unity { /// The core of the thing /// The created thing public static Thing Create(RoboidControl.Thing core) { - Debug.Log("Creating new Unity thing"); GameObject gameObj = string.IsNullOrEmpty(core.name) ? new("Thing") : new(core.name); @@ -117,18 +116,15 @@ namespace RoboidControl.Unity { private void HandleCoreEvent(RoboidControl.Thing.CoreEvent coreEvent) { switch (coreEvent.messageId) { case ThingMsg.id: - Debug.Log($"{this.core.id} Handle Thing"); if (core.parent == null) this.transform.SetParent(core.owner.component.transform, true); else if (core.parent.component != null) this.transform.SetParent(core.parent.component.transform, true); break; case NameMsg.Id: - Debug.Log($"{this.core.id} Handle Name"); this.gameObject.name = core.name; break; case ModelUrlMsg.Id: - Debug.Log($"{this.core.id} Handle Model URL"); string extension = core.modelUrl[core.modelUrl.LastIndexOf(".")..]; if (extension == ".jpg" || extension == ".png") StartCoroutine(LoadJPG()); @@ -138,11 +134,9 @@ namespace RoboidControl.Unity { #endif break; case PoseMsg.Id: - Debug.Log($"{this.core.id} Handle Pose"); this.HandlePose(); break; case BinaryMsg.Id: - Debug.Log($"{this.core.id} Handle Binary"); this.HandleBinary(); break; } @@ -185,7 +179,9 @@ namespace RoboidControl.Unity { if (!loadingModel) { loadingModel = true; +#if DEBUG Debug.Log("Loading GLTF model from :" + coreThing.modelUrl); +#endif GltfImport gltfImport = new GltfImport(); bool success = await gltfImport.Load(coreThing.modelUrl); if (success) { @@ -206,7 +202,7 @@ namespace RoboidControl.Unity { if (meshRenderers.Length > 0) { foreach (SkinnedMeshRenderer meshRenderer in meshRenderers) { if (meshRenderer.rootBone != null) { - Debug.Log("Found a skinned mesh with bones"); + // Debug.Log("Found a skinned mesh with bones"); ScanForThings(meshRenderer.rootBone); break; } @@ -232,7 +228,7 @@ namespace RoboidControl.Unity { if (foundObj != null && (thing.component != null && foundObj != thing.component.gameObject)) { Thing foundThing = foundObj.GetComponent(); if (foundThing == null) { - Debug.Log($"move thing [{thing.owner.networkId}/{thing.id}] to {foundObj.name}"); + // Debug.Log($"move thing [{thing.owner.networkId}/{thing.id}] to {foundObj.name}"); foundThing = foundObj.AddComponent(); foundThing.core = thing; foundThing.core.position = LinearAlgebra.Spherical.FromVector3(foundObj.transform.localPosition); diff --git a/src/Participant.cs b/src/Participant.cs index 079c70c..919124c 100644 --- a/src/Participant.cs +++ b/src/Participant.cs @@ -31,13 +31,13 @@ namespace RoboidControl { /// The UDP port of the participant /// This does not belong here, it should move to ParticipantUDP or something like that in the future public Participant(string ipAddress, int port, Participant localParticipant = null) { - this.ipAddress = ipAddress; - this.port = port; + // this.ipAddress = ipAddress; + // this.port = port; Thing.CreateRoot(this); - if (localParticipant != null) - this.udpClient = localParticipant.udpClient; + // if (localParticipant != null) + // this.udpClient = localParticipant.udpClient; } /// @@ -56,28 +56,32 @@ namespace RoboidControl { #region Properties + /* /// /// The Ip Address of a participant. When the participant is local, this contains 0.0.0.0 /// /// This does not belong here, it should move to ParticipantUDP or something like that in the future - public string ipAddress = "0.0.0.0"; + protected string ipAddress = "0.0.0.0"; /// /// The port number for UDP communication with the participant. This is 0 for isolated participants. /// /// This does not belong here, it should move to ParticipantUDP or something like that in the future - public int port = 0; + protected int port = 0; /// /// The udpClient for this participant /// /// This does not belong here, it should move to ParticipantUDP or something like that in the future public UdpClient udpClient = null; + */ /// /// The network Id to identify the participant /// public byte networkId = 0; + public bool isRemote = false; + /// /// The root thing for this participant /// @@ -178,17 +182,18 @@ namespace RoboidControl { public byte[] buffer = new byte[1024]; public virtual bool Send(IMessage msg) { - int bufferSize = msg.Serialize(ref this.buffer); - if (bufferSize <= 0) - return true; + return false; + // int bufferSize = msg.Serialize(ref this.buffer); + // if (bufferSize <= 0) + // return true; - IPEndPoint participantEndpoint = new IPEndPoint(IPAddress.Parse(this.ipAddress), this.port); - // Console.WriteLine($"msg to remote participant {participantEndpoint.Address.ToString()} {participantEndpoint.Port}"); - if (udpClient != null) { - //Console.WriteLine("sending..."); - this.udpClient?.Send(this.buffer, bufferSize, participantEndpoint); - } - return true; + // IPEndPoint participantEndpoint = new IPEndPoint(IPAddress.Parse(this.ipAddress), this.port); + // // Console.WriteLine($"msg to remote participant {participantEndpoint.Address.ToString()} {participantEndpoint.Port}"); + // if (udpClient != null) { + // //Console.WriteLine("sending..."); + // this.udpClient?.Send(this.buffer, bufferSize, participantEndpoint); + // } + // return true; } public void SendThingInfo(Thing thing) { @@ -214,14 +219,14 @@ namespace RoboidControl { /// The ip address of the participant /// The port number used to send messages to the participant /// The participant or null if it is not found. - public static Participant GetParticipant(string ipAddress, int port) { - //Console.WriteLine($"Get Participant {ipAddress}:{port}"); - foreach (Participant participant in Participant.participants) { - if (participant.ipAddress == ipAddress && participant.port == port) - return participant; - } - return null; - } + // public static Participant GetParticipant(string ipAddress, int port) { + // //Console.WriteLine($"Get Participant {ipAddress}:{port}"); + // foreach (Participant participant in Participant.participants) { + // if (participant.ipAddress == ipAddress && participant.port == port) + // return participant; + // } + // return null; + // } /// /// Retrieve a participant using a network ID /// @@ -236,21 +241,21 @@ namespace RoboidControl { return null; } - /// - /// Add a new participant to the collection of participants - /// - /// The IP address of the participant - /// The port used to send messages to this participant - /// The added participant - public static Participant AddParticipant(string ipAddress, int port, Participant localParticipant = null) { - Console.WriteLine($"New Participant {ipAddress}:{port}"); - // This code is only valid for site, because those can distribute networkIds..... - Participant participant = new(ipAddress, port, localParticipant) { - networkId = (byte)(Participant.participants.Count + 1) - }; - Participant.participants.Add(participant); - return participant; - } + // /// + // /// Add a new participant to the collection of participants + // /// + // /// The IP address of the participant + // /// The port used to send messages to this participant + // /// The added participant + // public static Participant AddParticipant(string ipAddress, int port, Participant localParticipant = null) { + // Console.WriteLine($"New Participant {ipAddress}:{port}"); + // // This code is only valid for site, because those can distribute networkIds..... + // Participant participant = new(ipAddress, port, localParticipant) { + // networkId = (byte)(Participant.participants.Count + 1) + // }; + // Participant.participants.Add(participant); + // return participant; + // } /// /// Add a new participant to the collection of participants /// diff --git a/src/Participants/ParticipantUDP.cs b/src/Participants/ParticipantUDP.cs index 5640756..4e085d2 100644 --- a/src/Participants/ParticipantUDP.cs +++ b/src/Participants/ParticipantUDP.cs @@ -83,6 +83,15 @@ namespace RoboidControl { Participant.ReplaceLocalParticipant(this); } + public ParticipantUDP(string ipAddress, int port, ParticipantUDP localParticipant) : base(ipAddress, port, localParticipant) { + this.ipAddress = ipAddress; + this.port = port; + + if (localParticipant != null) + this.udpClient = localParticipant.udpClient; + + } + #endregion Init #region Properties @@ -92,6 +101,23 @@ namespace RoboidControl { /// public string name = "ParticipantUDP"; + /// + /// The Ip Address of a participant. When the participant is local, this contains 0.0.0.0 + /// + /// This does not belong here, it should move to ParticipantUDP or something like that in the future + public string ipAddress = "0.0.0.0"; + /// + /// The port number for UDP communication with the participant. This is 0 for isolated participants. + /// + /// This does not belong here, it should move to ParticipantUDP or something like that in the future + public int port = 0; + + /// + /// The udpClient for this participant + /// + /// This does not belong here, it should move to ParticipantUDP or something like that in the future + public UdpClient udpClient = null; + /// /// True if the participant is running isolated. /// @@ -109,10 +135,7 @@ namespace RoboidControl { public ulong publishIntervalMS = 3000; // = 3 seconds public ulong sendUpdateIntervalMS = 100; // = 0.1 seconds for object updates - //public byte[] buffer = new byte[1024]; - public IPEndPoint endPoint = null; - //public UdpClient udpClient = null; public string broadcastIpAddress = "255.255.255.255"; public readonly ConcurrentQueue messageQueue = new ConcurrentQueue(); @@ -132,8 +155,8 @@ namespace RoboidControl { IPAddress broadcastAddress = new(broadcastBytes); this.broadcastIpAddress = broadcastAddress.ToString(); - Console.WriteLine($"Subnet mask: {subnetMask.ToString()}"); - Console.WriteLine($"Broadcast address: {this.broadcastIpAddress}"); + // Console.WriteLine($"Subnet mask: {subnetMask.ToString()}"); + // Console.WriteLine($"Broadcast address: {this.broadcastIpAddress}"); } #endregion Properties @@ -143,7 +166,7 @@ namespace RoboidControl { public override void Update() { ulong currentTimeMS = Thing.GetTimeMs(); - if (this.isIsolated == false) { + if (this.isIsolated == false && this.isRemote == false) { if (this.publishIntervalMS > 0 && currentTimeMS > this.nextPublishMe) { ParticipantMsg msg = new(this.networkId); if (this.remoteSite == null) @@ -156,7 +179,8 @@ namespace RoboidControl { } UpdateMyThings(currentTimeMS); - UpdateOtherThings(currentTimeMS); + if (this.isRemote == false) + UpdateOtherThings(currentTimeMS); } protected ulong nextPublishMe = 0; @@ -167,11 +191,11 @@ namespace RoboidControl { if (thing == null) continue; - if (thing.hierarchyChanged && !(this.isIsolated || this.networkId == 0)) { - ThingMsg thingMsg = new(this.networkId, thing); - // this.Send(this.remoteSite, thingMsg); - this.remoteSite.Send(thingMsg); - } + // if (thing.hierarchyChanged && !(this.isIsolated || this.networkId == 0)) { + // ThingMsg thingMsg = new(this.networkId, thing); + // // this.Send(this.remoteSite, thingMsg); + // this.remoteSite.Send(thingMsg); + // } // Why don't we do recursive? // Because when a thing creates a thing in the update, @@ -184,15 +208,15 @@ namespace RoboidControl { // this.Send(this.remoteSite, destroyMsg); this.remoteSite.Send(destroyMsg); } - else { - // 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); - this.remoteSite.Send(new PoseMsg(thing.owner.networkId, thing)); - this.remoteSite.Send(new BinaryMsg(thing.owner.networkId, thing)); - } + // else { + // // 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); + // this.remoteSite.Send(new PoseMsg(thing.owner.networkId, thing)); + // this.remoteSite.Send(new BinaryMsg(thing.owner.networkId, thing)); + // } } if (thing.terminate) this.Remove(thing); @@ -209,29 +233,42 @@ namespace RoboidControl { 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 < participant.things.Count; thingIx++) { + // Thing thing = participant.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; - } + // // 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; + // } } - } #endregion Update #region Send + public override bool Send(IMessage msg) { + int bufferSize = msg.Serialize(ref this.buffer); + if (bufferSize <= 0) + return true; + + IPEndPoint participantEndpoint = new IPEndPoint(IPAddress.Parse(this.ipAddress), this.port); + // Console.WriteLine($"msg to remote participant {participantEndpoint.Address.ToString()} {participantEndpoint.Port}"); + if (udpClient != null) { + //Console.WriteLine("sending..."); + this.udpClient?.Send(this.buffer, bufferSize, participantEndpoint); + } + return true; + } + public void PublishThingInfo(Thing thing) { // Console.WriteLine("Publish thing info"); this.Publish(new ThingMsg(this.networkId, thing)); @@ -281,10 +318,19 @@ namespace RoboidControl { // It is hard to determine our source port string ipAddress = endPoint.Address.ToString(); if (ipAddress != this.ipAddress) { - Participant remoteParticipant = GetParticipant(ipAddress, endPoint.Port); - remoteParticipant ??= AddParticipant(ipAddress, endPoint.Port, this); + Participant sender = GetParticipant(ipAddress, endPoint.Port); + if (sender == null) { + sender = AddParticipant(ipAddress, endPoint.Port, this); + sender.isRemote = true; + } - ReceiveData(data, remoteParticipant); + UpdateEvent e = new() { + messageId = NetworkIdMsg.Id, + participant = sender + }; + this.updateQueue.Enqueue(e); + + ReceiveData(data, sender); } } @@ -383,12 +429,6 @@ namespace RoboidControl { // HACK to get the networkId for sites corrected to 0. // This is needed because AddParticipant assigns a networkId for non-sites sender.networkId = 0; - UpdateEvent e = new() { - messageId = NetworkIdMsg.Id, - participant = sender - }; - this.updateQueue.Enqueue(e); - } } @@ -414,6 +454,7 @@ namespace RoboidControl { // Console.Write($"Dropped {thing.id}"); thing.parent = null; } + ForwardMessage(sender, msg); } protected virtual Thing ProcessNewThing(Participant sender, ThingMsg msg) { @@ -448,7 +489,7 @@ namespace RoboidControl { } protected virtual void Process(Participant sender, PoseMsg msg) { -#if DEBUG +#if DEBUG2 Console.WriteLine($"{this.name}: Process PoseMsg [{msg.networkId}/{msg.thingId}] {msg.poseType}"); #endif Participant owner = Participant.GetParticipant(msg.networkId); @@ -496,17 +537,51 @@ namespace RoboidControl { #endif } - private void ForwardMessage(IMessage msg) { - // foreach (Participant client in senders) { - // if (client == this) - // continue; - // //UnityEngine.Debug.Log($"---> {client.ipAddress}"); - // //IMessage.SendMsg(client, msg); - // msg.Serialize(ref client.buffer); - // client.SendBuffer(client.buffer.Length); - // } + private void ForwardMessage(Participant sender, IMessage msg) { + foreach (Participant participant in Participant.participants) { + if (participant == this || participant == sender) + continue; + + Console.WriteLine($"---> {participant.networkId}"); + participant.Send(msg); + } } #endregion + + #region Participant Registry + + /// + /// Retrieve a participant using ip address and port number + /// + /// The ip address of the participant + /// The port number used to send messages to the participant + /// The participant or null if it is not found. + public static ParticipantUDP GetParticipant(string ipAddress, int port) { + //Console.WriteLine($"Get Participant {ipAddress}:{port}"); + foreach (Participant participant in Participant.participants) { + ParticipantUDP participantUDP = participant as ParticipantUDP; + if (participantUDP.ipAddress == ipAddress && participantUDP.port == port) + return participantUDP; + } + return null; + } + + /// + /// Add a new participant to the collection of participants + /// + /// The IP address of the participant + /// The port used to send messages to this participant + /// The added participant + public static ParticipantUDP AddParticipant(string ipAddress, int port, ParticipantUDP localParticipant = null) { + Console.WriteLine($"New Participant {ipAddress}:{port}"); + // This code is only valid for site, because those can distribute networkIds..... + ParticipantUDP participant = new(ipAddress, port, localParticipant) { + networkId = (byte)(Participant.participants.Count + 1) + }; + Participant.participants.Add(participant); + return participant; + } + #endregion } } \ No newline at end of file diff --git a/src/Participants/SiteServer.cs b/src/Participants/SiteServer.cs index 71922cf..fb23c1e 100644 --- a/src/Participants/SiteServer.cs +++ b/src/Participants/SiteServer.cs @@ -63,6 +63,13 @@ namespace RoboidControl { #region Update + public override void Update() { + ulong currentTimeMS = Thing.GetTimeMs(); + + UpdateMyThings(currentTimeMS); + UpdateOtherThings(currentTimeMS); + } + protected override void UpdateMyThings(ulong currentTimeMS) { // We don't use foreach to prevent the 'Collection was modified' error int n = this.things.Count; diff --git a/src/Thing.cs b/src/Thing.cs index a9a3ef0..36720db 100644 --- a/src/Thing.cs +++ b/src/Thing.cs @@ -84,6 +84,7 @@ namespace RoboidControl { this.owner = owner; this.parent = null; this.owner.Add(this); + this.id = 0; // Root always has id 0 } /// @@ -166,7 +167,7 @@ namespace RoboidControl { public string modelUrl { get => _modelUrl; set { - if (_modelUrl != value) { + if (value != _modelUrl) { _modelUrl = value; this.updateQueue.Enqueue(new CoreEvent(ModelUrlMsg.Id)); }