From dc165edf79d89322fb35d9ebef0fa50649079a7a Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 8 Jan 2025 17:11:54 +0100 Subject: [PATCH] NW PoC works --- CustomMsg.cs | 45 ++++++++++ CustomMsg.cs.meta | 2 + Messages.cs | 46 ---------- Participant.cs | 12 ++- Sensors.meta | 8 ++ Sensors/TemperatureSensor.cs | 14 +++ Sensors/TemperatureSensor.cs.meta | 2 + SiteServer.cs | 24 ++++-- Thing.cs | 138 +++++++++++++++--------------- ThingMsg.cs | 2 +- 10 files changed, 166 insertions(+), 127 deletions(-) create mode 100644 CustomMsg.cs create mode 100644 CustomMsg.cs.meta create mode 100644 Sensors.meta create mode 100644 Sensors/TemperatureSensor.cs create mode 100644 Sensors/TemperatureSensor.cs.meta diff --git a/CustomMsg.cs b/CustomMsg.cs new file mode 100644 index 0000000..5b491b1 --- /dev/null +++ b/CustomMsg.cs @@ -0,0 +1,45 @@ +namespace Passer.Control.Core { + + public class CustomMsg : IMessage { + public const byte Id = 0xB1; + public byte networkId; + public byte thingId; + public byte[] bytes; + + public CustomMsg(byte[] buffer) { + byte ix = 1; + this.networkId = buffer[ix++]; + this.thingId = buffer[ix++]; + byte length = (byte)(buffer.Length - ix); + this.bytes = new byte[length]; + for (uint bytesIx = 0; bytesIx < length; bytesIx++) + this.bytes[bytesIx] = buffer[ix++]; + } + public CustomMsg(byte networkId, byte thingId, byte[] bytes) : base() { + this.networkId = networkId; + this.thingId = thingId; + this.bytes = bytes; + } + + public override byte Serialize(ref byte[] buffer) { + byte ix = 0; + buffer[ix++] = CustomMsg.Id; + buffer[ix++] = this.networkId; + buffer[ix++] = this.thingId; + //buffer[ix++] = (byte)bytes.Length; + foreach (byte b in bytes) + buffer[ix++] = b; + + return ix; + } + + //public static async Task Receive(Stream dataStream, Participant client, byte packetSize) { + // byte[] buffer = await Receive(dataStream, packetSize); + + // CustomMsg msg = new(buffer); + // client.messageQueue.Enqueue(msg); + // return true; + //} + } + +} \ No newline at end of file diff --git a/CustomMsg.cs.meta b/CustomMsg.cs.meta new file mode 100644 index 0000000..cbddfe9 --- /dev/null +++ b/CustomMsg.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: b9dcd611539afab429456b08500ae152 \ No newline at end of file diff --git a/Messages.cs b/Messages.cs index 2d27523..83ba4e5 100644 --- a/Messages.cs +++ b/Messages.cs @@ -197,52 +197,6 @@ namespace Passer.Control.Core { #endregion Pose - #region Custom - - public class CustomMsg : IMessage { - public const byte Id = 0xB1; - public byte networkId; - public byte thingId; - public byte[] bytes; - - public CustomMsg(byte[] buffer) { - byte ix = 0; - this.networkId = buffer[ix++]; - this.thingId = buffer[ix++]; - byte length = (byte)(buffer.Length - ix); - this.bytes = new byte[length]; - for (uint bytesIx = 0; bytesIx < length; bytesIx++) - this.bytes[bytesIx] = buffer[ix++]; - } - public CustomMsg(byte networkId, byte thingId, byte[] bytes) : base() { - this.networkId = networkId; - this.thingId = thingId; - this.bytes = bytes; - } - - public override byte Serialize(ref byte[] buffer) { - byte ix = 0; - buffer[ix++] = CustomMsg.Id; - buffer[ix++] = this.networkId; - buffer[ix++] = this.thingId; - //buffer[ix++] = (byte)bytes.Length; - foreach (byte b in bytes) - buffer[ix++] = b; - - return ix; - } - - public static async Task Receive(Stream dataStream, Participant client, byte packetSize) { - byte[] buffer = await Receive(dataStream, packetSize); - - CustomMsg msg = new(buffer); - client.messageQueue.Enqueue(msg); - return true; - } - } - - #endregion Custom - #region Text public class TextMsg : IMessage { diff --git a/Participant.cs b/Participant.cs index cb78ec6..b237fa4 100644 --- a/Participant.cs +++ b/Participant.cs @@ -189,7 +189,7 @@ namespace Passer.Control.Core { // result = await PoseMsg.Receive(dataStream, client, packetSize); break; case CustomMsg.Id: // 0xB1 / 177 - // result = await CustomMsg.Receive(dataStream, client, packetSize); + this.Process(new CustomMsg(data)); break; case TextMsg.Id: // 0xB0 / 176 // result = await TextMsg.Receive(dataStream, client, packetSize); @@ -224,7 +224,10 @@ namespace Passer.Control.Core { } protected virtual void Process(NameMsg msg) { - Console.WriteLine($"received name {msg.name}"); + //Console.WriteLine($"received name {msg.name}"); + Thing thing = Thing.Get(msg.networkId, msg.thingId); + if (thing != null) + thing.name = msg.name; } protected virtual void Process(ModelUrlMsg msg) { @@ -233,7 +236,10 @@ namespace Passer.Control.Core { protected virtual void ProcessPoseMsg(PoseMsg msg) { } - protected virtual void ProcessCustomMsg(CustomMsg msg) { } + protected virtual void Process(CustomMsg msg) { + Thing thing = Thing.Get(msg.networkId, msg.thingId); + thing?.ProcessBytes(msg.bytes); + } protected virtual void ProcessTextMsg(TextMsg temsgxt) { } diff --git a/Sensors.meta b/Sensors.meta new file mode 100644 index 0000000..6c54616 --- /dev/null +++ b/Sensors.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: a5a7a42365df0d0459195576ad3bb50b +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Sensors/TemperatureSensor.cs b/Sensors/TemperatureSensor.cs new file mode 100644 index 0000000..7a54051 --- /dev/null +++ b/Sensors/TemperatureSensor.cs @@ -0,0 +1,14 @@ +namespace Passer.Control.Core { + + public class TemperatureSensor : Thing { + public TemperatureSensor(byte networkId, byte thingId) : base(null, networkId, thingId, (byte)Type.TemperatureSensor) { + } + + public override void ProcessBytes(byte[] bytes) { + byte ix = 0; + float temp = LowLevelMessages.ReceiveFloat16(bytes, ref ix); + UnityEngine.Debug.Log($"temperature {this.name} = {temp} C"); + } + } + +} \ No newline at end of file diff --git a/Sensors/TemperatureSensor.cs.meta b/Sensors/TemperatureSensor.cs.meta new file mode 100644 index 0000000..69b6d6a --- /dev/null +++ b/Sensors/TemperatureSensor.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 5cd9e2534695ec844bd23b7a48ad498a \ No newline at end of file diff --git a/SiteServer.cs b/SiteServer.cs index 4dde48e..ab35567 100644 --- a/SiteServer.cs +++ b/SiteServer.cs @@ -16,14 +16,7 @@ namespace Passer.Control.Core { this.name = "Site Server"; } - public override void Update(long currentTimeMs) { - // for (int ix = 0; ix < this.others.Count; ix++) { - // Participant client = this.others[ix]; - // if (client == null) - // continue; - - // this.ProcessMessages(client); - // } + public override void Update(long currentTimeMs) { Thing.UpdateAll(currentTimeMs); } @@ -36,5 +29,20 @@ namespace Passer.Control.Core { protected override void Process(Participant sender, NetworkIdMsg msg) { } + + protected override void Process(ThingMsg msg) { + Thing thing = Thing.Get(msg.networkId, msg.thingId); + if (thing == null) { + switch ((Thing.Type) msg.thingType) { + case Thing.Type.TemperatureSensor: + new TemperatureSensor(msg.networkId, msg.thingId); + break; + default: + new Thing(this, msg.networkId, msg.thingId, msg.thingType); + break; + } + } + } + } } \ No newline at end of file diff --git a/Thing.cs b/Thing.cs index 4e70207..1b3de79 100644 --- a/Thing.cs +++ b/Thing.cs @@ -1,14 +1,36 @@ using System.Collections.Generic; using Passer.LinearAlgebra; -namespace Passer.Control.Core -{ +namespace Passer.Control.Core { /// /// A thing is the basic building block /// - public class Thing - { + public class Thing { + + #region Types + + public enum Type { + Undeterment, + // Sensor + Switch, + DistanceSensor, + DirectionalSensor, + TemperatureSensor, + // Motor + ControlledMotor, + UncontrolledMotor, + Servo, + // Other + Roboid, + Humanoid, + ExternalSensor + }; + + #endregion Types + + #region Properties + public Participant participant; public delegate void ChangeHandler(); @@ -19,36 +41,30 @@ namespace Passer.Control.Core public event ChangeHandler OnParentChanged; private Thing _parent; - public Thing parent - { + public Thing parent { get => _parent; - set - { + set { if (_parent == value) return; - if (value == null) - { + if (value == null) { _parent.RemoveChild(this); _parent = null; } - else - { + else { value.AddChild(this); OnParentChanged?.Invoke(); } } } - public void AddChild(Thing child) - { + public void AddChild(Thing child) { if (children.Find(thing => thing == child) != null) return; child._parent = this; children.Add(child); } - public void RemoveChild(Thing child) - { + public void RemoveChild(Thing child) { children.Remove(child); } @@ -58,13 +74,10 @@ namespace Passer.Control.Core public event ChangeHandler OnNameChanged; private string _name; - public string name - { + public string name { get => _name; - set - { - if (_name != value) - { + set { + if (_name != value) { _name = value; OnNameChanged?.Invoke(); } @@ -76,13 +89,10 @@ namespace Passer.Control.Core public byte poseUpdated = 0x00; public event ChangeHandler OnPositionChanged; private Spherical _position; - public Spherical position - { + public Spherical position { get { return _position; } - set - { - if (_position != value) - { + set { + if (_position != value) { _position = value; OnPositionChanged?.Invoke(); } @@ -91,13 +101,10 @@ namespace Passer.Control.Core public event ChangeHandler OnOrientationChanged; private SwingTwist _orientation; - public SwingTwist orientation - { + public SwingTwist orientation { get { return _orientation; } - set - { - if (_orientation != value) - { + set { + if (_orientation != value) { _orientation = value; OnOrientationChanged?.Invoke(); } @@ -106,13 +113,10 @@ namespace Passer.Control.Core public event SphericalHandler OnLinearVelocityChanged; private Spherical _linearVelocity; - public Spherical linearVelocity - { + public Spherical linearVelocity { get => _linearVelocity; - set - { - if (_linearVelocity != value) - { + set { + if (_linearVelocity != value) { _linearVelocity = value; OnLinearVelocityChanged?.Invoke(_linearVelocity); } @@ -120,21 +124,21 @@ namespace Passer.Control.Core } public Spherical angularVelocity; - public virtual void Init(bool invokeEvent = true) - { + #endregion Properties + + #region Init + + public virtual void Init(bool invokeEvent = true) { Thing.Add(this, invokeEvent); } - public Thing(bool initialize = true) - { - if (initialize) - { + public Thing(bool initialize = true) { + if (initialize) { //this.Init(); Thing.Add(this); } } - public Thing(Participant client, byte networkId, byte thingId, byte thingType = 0) - { + public Thing(Participant client, byte networkId, byte thingId, byte thingType = 0) { this.participant = client; this.id = thingId; this.type = thingType; @@ -143,17 +147,21 @@ namespace Passer.Control.Core Thing.Add(this); } - public virtual void Update(float currentTime) - { + #endregion Init + + #region Update + + public virtual void Update(float currentTime) { // should recurse over children... } - public virtual void ProcessBytes(byte[] bytes) - { + public virtual void ProcessBytes(byte[] bytes) { //if (sensor != null) // sensor.ProcessBytes(bytes); } + #endregion Update + // Experimental // public float stressLevel = 0; @@ -176,48 +184,40 @@ namespace Passer.Control.Core public delegate void ThingHandler(Thing t); public static event ThingHandler OnNewThing; - public static void Add(Thing thing, bool invokeEvent = true) - { + public static void Add(Thing thing, bool invokeEvent = true) { Thing foundThing = Get(thing.networkId, thing.id); - if (foundThing == null) - { + if (foundThing == null) { if (thing.id == 0) thing.id = (byte)(allThings.Count + 1); allThings.Add(thing); if (invokeEvent) OnNewThing?.Invoke(thing); - // UnityEngine.Debug.Log($"Add thing [{thing.networkId}/{thing.id}] {thing.name}"); + UnityEngine.Debug.Log($"Add thing [{thing.networkId}/{thing.id}] {thing.name}"); } } - public static Thing Get(byte networkId, byte thingId) - { + public static Thing Get(byte networkId, byte thingId) { Thing thing = allThings.Find(aThing => IsThing(aThing, networkId, thingId)); return thing; } - private static bool IsThing(Thing thing, byte networkId, byte thingId) - { + private static bool IsThing(Thing thing, byte networkId, byte thingId) { if (thing == null) return false; return (thing.networkId == networkId) && (thing.id == thingId); } - public static void Remove(byte networkId, byte thingId) - { + public static void Remove(byte networkId, byte thingId) { allThings.RemoveAll(t => t.networkId == networkId && t.id == thingId); } - public static Thing[] GetAllThings() - { + public static Thing[] GetAllThings() { return allThings.ToArray(); } - public static void UpdateAll(float currentTime) - { - foreach (Thing thing in allThings) - { + public static void UpdateAll(float currentTime) { + foreach (Thing thing in allThings) { if (thing.parent == null) // update only root things thing.Update(currentTime); } diff --git a/ThingMsg.cs b/ThingMsg.cs index aefcf1b..723c720 100644 --- a/ThingMsg.cs +++ b/ThingMsg.cs @@ -24,7 +24,7 @@ namespace Passer.Control.Core { this.parentId = parentId; } public ThingMsg(byte[] buffer) { - uint ix = 0; + uint ix = 1; this.networkId = buffer[ix++]; this.thingId = buffer[ix++]; this.thingType = buffer[ix++];