From e92edbc381af08fc4730ec5f47923d2f903a1453 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Thu, 1 May 2025 11:00:08 +0200 Subject: [PATCH] Aligned the Messages --- src/Messages/BinaryMsg.cs | 2 +- src/Messages/DestroyMsg.cs | 9 ++++++--- src/Messages/{Messages.cs => IMessage.cs} | 1 - src/Messages/InvestigateMsg.cs | 10 +++++----- src/Messages/LowLevelMessages.cs | 13 +++++------- src/Messages/ModelUrlMsg.cs | 12 ------------ src/Messages/PoseMsg.cs | 24 ++--------------------- src/Messages/ThingMsg.cs | 19 +++--------------- 8 files changed, 22 insertions(+), 68 deletions(-) rename src/Messages/{Messages.cs => IMessage.cs} (95%) diff --git a/src/Messages/BinaryMsg.cs b/src/Messages/BinaryMsg.cs index f91f496..d2c0570 100644 --- a/src/Messages/BinaryMsg.cs +++ b/src/Messages/BinaryMsg.cs @@ -9,7 +9,7 @@ namespace RoboidControl { /// public const byte Id = 0xB1; /// - /// The length of the message, excluding the binary data + /// The length of the message in bytes, excluding the binary data /// /// For the total size of the message this.bytes.Length should be added to this value. public const byte length = 4; diff --git a/src/Messages/DestroyMsg.cs b/src/Messages/DestroyMsg.cs index 87eaf5b..f7d3425 100644 --- a/src/Messages/DestroyMsg.cs +++ b/src/Messages/DestroyMsg.cs @@ -9,7 +9,7 @@ namespace RoboidControl { /// public const byte Id = 0x20; /// - /// The length of the message + /// The length of the message in bytres /// public const byte length = 3; /// @@ -34,13 +34,16 @@ namespace RoboidControl { public DestroyMsg(byte[] buffer) : base(buffer) { this.networkId = buffer[1]; this.thingId = buffer[2]; - } + } /// @copydoc Passer::RoboidControl::IMessage::Serialize public override byte Serialize(ref byte[] buffer) { if (buffer.Length < DestroyMsg.length) return 0; - + +#if DEBUG + System.Console.WriteLine($"Send DestroyMsg [{this.networkId}/{this.thingId}]"); +#endif byte ix = 0; buffer[ix++] = DestroyMsg.Id; buffer[ix++] = this.networkId; diff --git a/src/Messages/Messages.cs b/src/Messages/IMessage.cs similarity index 95% rename from src/Messages/Messages.cs rename to src/Messages/IMessage.cs index 2fa0045..c299c50 100644 --- a/src/Messages/Messages.cs +++ b/src/Messages/IMessage.cs @@ -11,7 +11,6 @@ namespace RoboidControl { /// /// The byte array to parse public IMessage(byte[] buffer) { - //Deserialize(buffer); } /// diff --git a/src/Messages/InvestigateMsg.cs b/src/Messages/InvestigateMsg.cs index 08d54d9..7ebcc01 100644 --- a/src/Messages/InvestigateMsg.cs +++ b/src/Messages/InvestigateMsg.cs @@ -9,7 +9,7 @@ namespace RoboidControl { /// public const byte Id = 0x81; /// - /// The length of the message + /// /// The length of the message /// public const byte length = 3; /// @@ -22,13 +22,13 @@ namespace RoboidControl { public byte thingId; /// - /// Create a new message for sending + /// Create an investigate message /// /// The network ID for the thing - /// The ID of the thing - public InvestigateMsg(byte networkId, byte thingId) { + /// The thing for which the details are requested + public InvestigateMsg(byte networkId, Thing thing) { this.networkId = networkId; - this.thingId = thingId; + this.thingId = thing.id; } /// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer) public InvestigateMsg(byte[] buffer) : base(buffer) { } diff --git a/src/Messages/LowLevelMessages.cs b/src/Messages/LowLevelMessages.cs index 8b2d240..e8c8062 100644 --- a/src/Messages/LowLevelMessages.cs +++ b/src/Messages/LowLevelMessages.cs @@ -8,7 +8,6 @@ namespace RoboidControl { SendAngle8(buffer, ref ix, v.direction.horizontal); SendAngle8(buffer, ref ix, v.direction.vertical); } - public static Spherical ReceiveSpherical(byte[] data, ref byte ix) { float distance = ReceiveFloat16(data, ref ix); float horizontal = ReceiveAngle8(data, ref ix); @@ -17,11 +16,6 @@ namespace RoboidControl { return v; } - public static void SendQuat32(byte[] buffer, ref byte ix, SwingTwist s) { - Quat32 q32 = Quat32.FromSwingTwist(s); - SendQuat32(buffer, ref ix, q32); - - } public static void SendQuat32(byte[] buffer, ref byte ix, Quat32 q) { int qx = (int)(q.x * 127 + 128); int qy = (int)(q.y * 127 + 128); @@ -48,6 +42,11 @@ namespace RoboidControl { return q; } + public static void SendQuat32(byte[] buffer, ref byte ix, SwingTwist s) { + Quat32 q32 = Quat32.FromSwingTwist(s); + SendQuat32(buffer, ref ix, q32); + + } public static SwingTwist ReceiveSwingTwist(byte[] data, ref byte ix) { Quat32 q32 = ReceiveQuat32(data, ref ix); // UnityEngine.Quaternion q = new(q32.x, q32.y, q32.z, q32.w); @@ -66,7 +65,6 @@ namespace RoboidControl { sbyte value = (sbyte)(angle / 360.0f * 256.0f); buffer[ix++] = (byte)value; } - public static float ReceiveAngle8(byte[] data, ref byte ix) { float value = (data[ix++] * 180) / 128.0F; return value; @@ -83,7 +81,6 @@ namespace RoboidControl { data[ix++] = (byte)((binary >> 8) & 0xFF); data[ix++] = (byte)(binary & 0xFF); } - public static float ReceiveFloat16(byte[] data, ref byte ix) { byte msb = data[ix++]; byte lsb = data[ix++]; diff --git a/src/Messages/ModelUrlMsg.cs b/src/Messages/ModelUrlMsg.cs index 64efac5..9932281 100644 --- a/src/Messages/ModelUrlMsg.cs +++ b/src/Messages/ModelUrlMsg.cs @@ -40,18 +40,6 @@ namespace RoboidControl { this.urlLength = (byte)thing.modelUrl.Length; this.url = thing.modelUrl; } - /// - /// Create a new message for sending - /// - /// The network ID of the thing - /// The ID of the thing - /// The URL to send - // public ModelUrlMsg(byte networkId, byte thingId, string url) { - // this.networkId = networkId; - // this.thingId = thingId; - // this.urlLength = (byte)url.Length; - // this.url = url; - // } /// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer) public ModelUrlMsg(byte[] buffer) { byte ix = 1; diff --git a/src/Messages/PoseMsg.cs b/src/Messages/PoseMsg.cs index 123a2e5..f0d24a9 100644 --- a/src/Messages/PoseMsg.cs +++ b/src/Messages/PoseMsg.cs @@ -66,28 +66,8 @@ namespace RoboidControl { /// Create a new message for sending /// /// The network ID of the thing - /// The ID of the thing - /// The position of the thing in local space in meters - /// The orientation of the thing in local space - public PoseMsg(byte networkId, byte thingId, Spherical position, SwingTwist orientation, Spherical linearVelocity = null, Spherical angularVelocity = null) { - this.networkId = networkId; - this.thingId = thingId; - - this.poseType = 0; - if (this.position != null) - this.poseType |= Pose_Position; - if (this.orientation != null) - this.poseType |= Pose_Orientation; - if (this.linearVelocity != null) - this.poseType |= Pose_LinearVelocity; - if (this.angularVelocity != null) - this.poseType |= Pose_AngularVelocity; - - this.position = position; - this.orientation = orientation; - this.linearVelocity = linearVelocity; - this.angularVelocity = angularVelocity; - } + /// The thing for which the pose should be sent + /// If true, position and orientation are always included, even when they are not updated public PoseMsg(byte networkId, Thing thing, bool force = false) { this.networkId = networkId; this.thingId = thing.id; diff --git a/src/Messages/ThingMsg.cs b/src/Messages/ThingMsg.cs index bf93a75..c0f3c8c 100644 --- a/src/Messages/ThingMsg.cs +++ b/src/Messages/ThingMsg.cs @@ -1,7 +1,7 @@ namespace RoboidControl { /// - /// Message providing generic information about a Thing + /// Message providing generic details about a Thing /// public class ThingMsg : IMessage { /// @@ -21,11 +21,11 @@ namespace RoboidControl { /// public byte thingId; /// - /// The Thing.Type of the thing + /// The type of thing /// public byte thingType; /// - /// The parent of the thing in the hierarachy. This is null for root Things + /// The ID of the parent thing in the hierarchy. This is zero for root things /// public byte parentId; @@ -44,19 +44,6 @@ namespace RoboidControl { this.parentId = 0; } /// - /// Create a message for sending - /// - /// The network ID of the thing - /// The ID of the thing - /// The type of thing - /// The parent of the thing - public ThingMsg(byte networkId, byte thingId, byte thingType, byte parentId) { - this.networkId = networkId; - this.thingId = thingId; - this.thingType = thingType; - this.parentId = parentId; - } - /// /// Create a message for receiving /// /// The byte array to parse