diff --git a/Unity/Participant.cs b/Unity/Participant.cs index 5c324a5..c3e9fda 100644 --- a/Unity/Participant.cs +++ b/Unity/Participant.cs @@ -19,6 +19,17 @@ namespace RoboidControl.Unity { private void HandleUpdateEvent(RoboidControl.Participant.UpdateEvent e) { switch (e.messageId) { + case ParticipantMsg.Id: + GameObject remoteParticipant = new GameObject("RemoteParticipant"); + Participant participant = remoteParticipant.AddComponent(); + participant.coreParticipant = e.participant; + participant.coreParticipant.component = participant; + if (participant.coreParticipant is ParticipantUDP participantUDP) { + participant.ipAddress = participantUDP.ipAddress; + participant.port = participantUDP.port; + } + + break; case ThingMsg.id: HandleThingEvent(e); break; @@ -38,7 +49,7 @@ namespace RoboidControl.Unity { DifferentialDrive differentialDrive = DifferentialDrive.Create(coreDrive); coreDrive.component = differentialDrive; break; - case RoboidControl.Motor coreMotor: + case RoboidControl.Motor coreMotor: Motor wheel = Motor.Create(coreMotor); coreMotor.component = wheel; // We need to know the details (though a binary msg) @@ -49,6 +60,7 @@ namespace RoboidControl.Unity { Thing[] things = FindObjectsByType(FindObjectsSortMode.None); // Debug.Log(things.Length); Thing thing = things.FirstOrDefault(t => + t != null && t.core != null && t.core.owner.networkId == coreThing.owner.networkId && t.core.id == coreThing.id diff --git a/Unity/Wheel.cs b/Unity/Wheel.cs index 978a2ff..0cbe3d1 100644 --- a/Unity/Wheel.cs +++ b/Unity/Wheel.cs @@ -51,7 +51,8 @@ namespace RoboidControl.Unity { gameObj.transform.parent = parent; Wheel component = gameObj.AddComponent(); SiteServer participant = FindAnyObjectByType(); - RoboidControl.Thing core = participant.coreParticipant.Get(thingId); + RoboidControl.Participant owner = participant.coreParticipant; + RoboidControl.Thing core = owner.Get(owner.networkId, thingId); if (core == null) { core = new(participant.coreParticipant.root) { id = thingId, diff --git a/src/Participant.cs b/src/Participant.cs index 919124c..6944992 100644 --- a/src/Participant.cs +++ b/src/Participant.cs @@ -20,7 +20,7 @@ namespace RoboidControl { /// /// Create a generic participant /// - private Participant() { + public Participant() { Thing.CreateRoot(this); } @@ -80,8 +80,8 @@ namespace RoboidControl { /// public byte networkId = 0; - public bool isRemote = false; - + public bool isRemote = false; + /// /// The root thing for this participant /// @@ -97,6 +97,7 @@ namespace RoboidControl { /// /// The ID of the thing /// The thing when it is found, null in other cases. + /* public Thing Get(byte thingId) { Thing thing = things.Find(aThing => aThing.id == thingId); //Thing thing = things.Find(aThing => Thing.IsThing(aThing, networkId, thingId)); @@ -104,6 +105,17 @@ namespace RoboidControl { // Console.WriteLine($"Could not find thing {ipAddress}:{port}[{networkId}/{thingId}]"); return thing; } + */ + public Thing Get(byte networkId, byte thingId) { + Thing thing = things.Find(aThing => + aThing.owner.networkId == networkId && + aThing.id == thingId + ); + //Thing thing = things.Find(aThing => Thing.IsThing(aThing, networkId, thingId)); + if (thing == null) + Console.WriteLine($"Could not find thing {this.networkId} [{networkId}/{thingId}]"); + return thing; + } /// /// Add a new thing for this participant @@ -111,14 +123,17 @@ namespace RoboidControl { /// The thing to add /// If true, the thing.id is regenerated if it is zero public void Add(Thing thing, bool checkId = true) { + Console.WriteLine("Add Thing to participant"); if (checkId && thing.id == 0) { thing.id = (byte)(this.things.Count + 1); this.things.Add(thing); } else { - Thing foundThing = Get(thing.id); - if (foundThing == null) + Thing foundThing = Get(thing.owner.networkId, thing.id); + if (foundThing == null) { + Console.WriteLine($"Added Thing to participant {this.networkId} [{thing.owner.networkId}/{thing.id}]"); this.things.Add(thing); + } } } diff --git a/src/Participants/ParticipantUDP.cs b/src/Participants/ParticipantUDP.cs index 4079361..688f8e7 100644 --- a/src/Participants/ParticipantUDP.cs +++ b/src/Participants/ParticipantUDP.cs @@ -326,7 +326,6 @@ namespace RoboidControl { sender = AddParticipant(ipAddress, endPoint.Port, this); sender.isRemote = true; - UpdateEvent e = new() { messageId = NetworkIdMsg.Id, participant = sender @@ -447,11 +446,24 @@ namespace RoboidControl { #if DEBUG Console.WriteLine($"{this.name}: Process ThingMsg [{msg.networkId}/{msg.thingId}] {msg.thingType} {msg.parentId}"); #endif - Thing thing = sender.Get(msg.thingId); - thing ??= ProcessNewThing(sender, msg); + Participant owner = GetParticipant(msg.networkId); + if (owner == null) { + owner = new() { + networkId = msg.networkId + }; + AddParticipant(owner); + UpdateEvent e = new() { + messageId = ParticipantMsg.Id, + participant = owner + }; + this.updateQueue.Enqueue(e); + Console.WriteLine("Added Participant"); + } + Thing thing = owner.Get(msg.networkId, msg.thingId); + thing ??= ProcessNewThing(owner, msg); if (msg.parentId != 0) { - thing.parent = sender.Get(msg.parentId); + thing.parent = owner.Get(msg.networkId, msg.parentId); if (thing.parent == null) Console.WriteLine($"Could not find parent [{msg.networkId}/{msg.parentId}]"); } @@ -463,11 +475,12 @@ namespace RoboidControl { ForwardMessage(sender, msg); } - protected virtual Thing ProcessNewThing(Participant sender, ThingMsg msg) { + protected virtual Thing ProcessNewThing(Participant owner, ThingMsg msg) { + Console.WriteLine("--- New Thing"); Thing newThing = msg.thingType switch { - Thing.Type.TouchSensor => new TouchSensor(sender.root), - Thing.Type.DifferentialDrive => new DifferentialDrive(sender.root), - _ => new Thing(sender.root) + Thing.Type.TouchSensor => new TouchSensor(owner.root), + Thing.Type.DifferentialDrive => new DifferentialDrive(owner.root), + _ => new Thing(owner.root) }; newThing.id = msg.thingId; newThing.type = msg.thingType; @@ -479,9 +492,12 @@ namespace RoboidControl { Console.WriteLine($"{this.name}: Process NameMsg [{msg.networkId}/{msg.thingId}] {msg.nameLength} {msg.name}"); #endif - Thing thing = sender.Get(msg.thingId); + Participant owner = GetParticipant(msg.networkId); + Thing thing = owner.Get(msg.networkId, msg.thingId); if (thing != null) thing.name = msg.name; + else + Console.WriteLine("--- Could not find thing"); ForwardMessage(sender, msg); } @@ -491,7 +507,7 @@ namespace RoboidControl { Console.WriteLine($"{this.name}: Process ModelUrlMsg [{msg.networkId}/{msg.thingId}] {msg.urlLength} {msg.url}"); #endif - Thing thing = sender.Get(msg.thingId); + Thing thing = sender.Get(msg.networkId, msg.thingId); if (thing != null) thing.modelUrl = msg.url; @@ -506,7 +522,7 @@ namespace RoboidControl { if (owner == null) return; - Thing thing = owner.Get(msg.thingId); + Thing thing = owner.Get(msg.networkId, msg.thingId); if (thing == null) return; @@ -526,7 +542,7 @@ namespace RoboidControl { #if DEBUG // Console.WriteLine($"{this.name}: Process BinaryMsg [{msg.networkId}/{msg.thingId}] {msg.dataLength}"); #endif - Thing thing = sender.Get(msg.thingId); + Thing thing = sender.Get(msg.networkId, msg.thingId); thing?.ProcessBinary(msg.data); } @@ -541,7 +557,7 @@ namespace RoboidControl { #if DEBUG Console.WriteLine($"Participant: Process Destroy Msg [{msg.networkId}/{msg.thingId}]"); #endif - Thing thing = sender.Get(msg.thingId); + Thing thing = sender.Get(msg.networkId, msg.thingId); if (thing != null) this.Remove(thing); #if UNITY_5_3_OR_NEWER @@ -554,7 +570,7 @@ namespace RoboidControl { if (participant == this || participant == sender) continue; - Console.WriteLine($"---> {participant.networkId}"); + // Console.WriteLine($"---> {participant.networkId}"); participant.Send(msg); } } diff --git a/src/Things/DifferentialDrive.cs b/src/Things/DifferentialDrive.cs index c973145..3ccb353 100644 --- a/src/Things/DifferentialDrive.cs +++ b/src/Things/DifferentialDrive.cs @@ -138,11 +138,11 @@ namespace RoboidControl { public override void ProcessBinary(byte[] data) { byte ix = 0; byte leftWheelId = data[ix++]; - this.leftWheel = this.owner.Get(leftWheelId) as Motor; + this.leftWheel = this.owner.Get(this.owner.networkId, leftWheelId) as Motor; this.leftWheel ??= new Motor(this) { id = leftWheelId }; byte rightWheelId = data[ix++]; - this.rightWheel = this.owner.Get(rightWheelId) as Motor; + this.rightWheel = this.owner.Get(this.owner.networkId, rightWheelId) as Motor; this.rightWheel ??= new Motor(this) { id = rightWheelId }; this._wheelRadius = LowLevelMessages.ReceiveFloat16(data, ref ix);