From 6d58b741e1db94d1e29f566871e283d6b6aee308 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 9 Apr 2025 11:07:43 +0200 Subject: [PATCH] ESP32 tracker sync. --- src/Messages/PoseMsg.cs | 28 ++++++++++++++++++++++++ src/Participant.cs | 10 +++++++++ src/ParticipantUDP.cs | 48 ++++++++++++++++++++++++----------------- src/SiteServer.cs | 18 ++++++++-------- src/Thing.cs | 14 +++++------- 5 files changed, 80 insertions(+), 38 deletions(-) diff --git a/src/Messages/PoseMsg.cs b/src/Messages/PoseMsg.cs index f29588f..1a1e5f2 100644 --- a/src/Messages/PoseMsg.cs +++ b/src/Messages/PoseMsg.cs @@ -88,6 +88,34 @@ namespace RoboidControl { this.linearVelocity = linearVelocity; this.angularVelocity = angularVelocity; } + public PoseMsg(byte networkId, Thing thing, bool force = false) { + this.networkId = networkId; + this.thingId = thing.id; + + this.poseType = 0; + 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; + this.poseType |= Pose_LinearVelocity; + thing.linearVelocityUpdated = false; + } + if (thing.angularVelocityUpdated) { + this.angularVelocity = thing.angularVelocity; + this.poseType |= Pose_AngularVelocity; + thing.angularVelocityUpdated = false; + } + + } + /// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer) public PoseMsg(byte[] buffer) : base(buffer) { byte ix = 1; diff --git a/src/Participant.cs b/src/Participant.cs index 172aa35..c85d437 100644 --- a/src/Participant.cs +++ b/src/Participant.cs @@ -44,6 +44,16 @@ namespace RoboidControl { /// protected readonly List things = new List(); + public virtual void Update(ulong currentTimeMS = 0) { + int n = this.things.Count; + for (int ix = 0; ix < n; ix++) { + Thing thing = this.things[ix]; + if (thing != null) + thing.Update(currentTimeMS, true); + } + + } + /// /// Get a thing with the given ids /// diff --git a/src/ParticipantUDP.cs b/src/ParticipantUDP.cs index bf1db17..3065e62 100644 --- a/src/ParticipantUDP.cs +++ b/src/ParticipantUDP.cs @@ -94,20 +94,20 @@ namespace RoboidControl { protected readonly Dictionary> thingMsgProcessors = new(); - public delegate Thing ThingConstructor(Participant sender, byte networkId, byte thingId); - public void Register(byte thingType, ThingConstructor constr) { - thingMsgProcessors[thingType] = new Func(constr); - } + // public delegate Thing ThingConstructor(Participant sender, byte networkId, byte thingId); + // public void Register(byte thingType, ThingConstructor constr) { + // thingMsgProcessors[thingType] = new Func(constr); + // } - public void Register(Thing.Type thingType) where ThingClass : Thing { - Register((byte)thingType); - } + // public void Register(Thing.Type thingType) where ThingClass : Thing { + // Register((byte)thingType); + // } - public void Register(byte thingType) where ThingClass : Thing { - thingMsgProcessors[thingType] = (participant, networkId, thingId) => - Activator.CreateInstance(typeof(ThingClass), participant, networkId, thingId) as ThingClass; - Console.WriteLine($"Registering {typeof(ThingClass)} for thing type {thingType}"); - } + // public void Register(byte thingType) where ThingClass : Thing { + // thingMsgProcessors[thingType] = (participant, networkId, thingId) => + // Activator.CreateInstance(typeof(ThingClass), participant, networkId, thingId) as ThingClass; + // Console.WriteLine($"Registering {typeof(ThingClass)} for thing type {thingType}"); + // } #endregion Init @@ -134,7 +134,7 @@ namespace RoboidControl { } protected ulong nextPublishMe = 0; - public virtual void Update(ulong currentTimeMS = 0) { + public override void Update(ulong currentTimeMS = 0) { if (currentTimeMS == 0) { #if UNITY_5_3_OR_NEWER currentTimeMS = (ulong)(UnityEngine.Time.time * 1000); @@ -152,12 +152,19 @@ namespace RoboidControl { Thing thing = this.things[ix]; if (thing != null && thing.parent == null) { thing.Update(currentTimeMS, true); - // if (thing.owner != this) { - // BinaryMsg binaryMsg = new(thing.owner.networkId, thing); - // this.Send(thing.owner, binaryMsg); - // } + // 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); + } } } + for (int ownerIx = 0; ownerIx < this.owners.Count; ownerIx++) { + Participant owner = this.owners[ownerIx]; + owner.Update(currentTimeMS); + } } public virtual void Publish() { @@ -309,11 +316,13 @@ namespace RoboidControl { } protected virtual void Process(Participant sender, PoseMsg msg) { - //Console.WriteLine($"Participant: Process pose [{msg.networkId}/{msg.thingId}] {msg.poseType}"); + Console.WriteLine($"Participant: Process pose [{msg.networkId}/{msg.thingId}] {msg.poseType}"); Thing thing = sender.Get(msg.networkId, msg.thingId); if (thing != null) { - if ((msg.poseType & PoseMsg.Pose_Position) != 0) + if ((msg.poseType & PoseMsg.Pose_Position) != 0) { thing.position = msg.position; + Console.Write($"{thing.id} position changed"); + } if ((msg.poseType & PoseMsg.Pose_Orientation) != 0) { thing.orientation = msg.orientation; Console.Write($"{thing.id} orientation changed"); @@ -322,7 +331,6 @@ namespace RoboidControl { thing.linearVelocity = msg.linearVelocity; if ((msg.poseType & PoseMsg.Pose_AngularVelocity) != 0) thing.angularVelocity = msg.angularVelocity; - } } diff --git a/src/SiteServer.cs b/src/SiteServer.cs index 211e118..1032c7f 100644 --- a/src/SiteServer.cs +++ b/src/SiteServer.cs @@ -29,7 +29,7 @@ namespace RoboidControl { new AsyncCallback(result => ReceiveUDP(result)), new Tuple(this.udpClient, new(IPAddress.Any, port))); - Register(Thing.Type.TouchSensor); + // Register(Thing.Type.TouchSensor); } /// @@ -45,7 +45,7 @@ namespace RoboidControl { protected override void Process(Participant sender, ParticipantMsg msg) { base.Process(sender, msg); //if (msg.networkId == 0) { - Console.WriteLine($"{this.name} received New Participant -> {sender.networkId}"); + //Console.WriteLine($"{this.name} received New Participant -> {sender.networkId}"); this.Send(sender, new NetworkIdMsg(sender.networkId)); //} } @@ -57,15 +57,15 @@ namespace RoboidControl { 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) { + // 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); // Console.WriteLine("Created generic new core thing"); - } + // } if (msg.parentId != 0) { Thing parentThing = Get(msg.networkId, msg.parentId); if (parentThing == null) diff --git a/src/Thing.cs b/src/Thing.cs index a39c359..c1e79ec 100644 --- a/src/Thing.cs +++ b/src/Thing.cs @@ -53,7 +53,7 @@ namespace RoboidControl { if (invokeEvent) InvokeNewThing(this); } - public Thing(Participant owner) : this(owner, Type.Undetermined) {} + public Thing(Participant owner) : this(owner, Type.Undetermined) { } /// /// Create a new thing for a participant /// @@ -286,10 +286,6 @@ namespace RoboidControl { } } /// - /// Event triggered when the orientation has changed - /// - //public event ChangeHandler OnOrientationChanged = delegate { }; - /// /// Boolean indicating the thing has an updated orientation /// public bool orientationUpdated = false; @@ -303,7 +299,7 @@ namespace RoboidControl { set { if (_linearVelocity != value) { _linearVelocity = value; - hasLinearVelocity = _linearVelocity.distance != 0; + linearVelocityUpdated = true; OnLinearVelocityChanged?.Invoke(_linearVelocity); } } @@ -315,7 +311,7 @@ namespace RoboidControl { /// /// Boolean indicating the thing has an updated linear velocity /// - public bool hasLinearVelocity = false; + public bool linearVelocityUpdated = false; private Spherical _angularVelocity = Spherical.zero; /// @@ -326,7 +322,7 @@ namespace RoboidControl { set { if (_angularVelocity != value) { _angularVelocity = value; - hasAngularVelocity = _angularVelocity.distance != 0; + angularVelocityUpdated = true; OnAngularVelocityChanged?.Invoke(_angularVelocity); } } @@ -338,7 +334,7 @@ namespace RoboidControl { /// /// Boolean indicating the thing has an updated angular velocity /// - public bool hasAngularVelocity = false; + public bool angularVelocityUpdated = false; #if UNITY_5_3_OR_NEWER ///