diff --git a/.editorconfig b/.editorconfig index 4ee3fdb..1ec7f97 100644 --- a/.editorconfig +++ b/.editorconfig @@ -10,6 +10,10 @@ end_of_line = crlf charset = utf-8 trim_trailing_whitespace = false insert_final_newline = false +max_line_length = 80 [*.cs] -csharp_new_line_before_open_brace = none \ No newline at end of file +csharp_new_line_before_open_brace = none +# Suppress warnings everywhere +dotnet_diagnostic.IDE1006.severity = none +dotnet_diagnostic.IDE0130.severity = none \ No newline at end of file diff --git a/.gitignore b/.gitignore index 6188945..2602af4 100644 --- a/.gitignore +++ b/.gitignore @@ -4,4 +4,4 @@ test/bin test/obj /bin /obj -*.meta \ No newline at end of file +**.meta \ No newline at end of file diff --git a/ControlCore.csproj b/ControlCore.csproj index 9f3c622..feb0b62 100644 --- a/ControlCore.csproj +++ b/ControlCore.csproj @@ -3,17 +3,12 @@ false false - net9.0 - enable - enable + net5.0 - - - - - + + diff --git a/ControlCore.csproj.meta b/ControlCore.csproj.meta deleted file mode 100644 index 1b43085..0000000 --- a/ControlCore.csproj.meta +++ /dev/null @@ -1,7 +0,0 @@ -fileFormatVersion: 2 -guid: 7f964f406734cf74097d61697e5cafc9 -DefaultImporter: - externalObjects: {} - userData: - assetBundleName: - assetBundleVariant: diff --git a/DoxyGen/Doxyfile b/DoxyGen/Doxyfile index 071d8c0..ae7a013 100644 --- a/DoxyGen/Doxyfile +++ b/DoxyGen/Doxyfile @@ -42,7 +42,7 @@ DOXYFILE_ENCODING = UTF-8 # title of most generated pages and in a few other places. # The default value is: My Project. -PROJECT_NAME = "Control Core for C#" +PROJECT_NAME = "Roboid Control for C#" # The PROJECT_NUMBER tag can be used to enter a project or revision number. This # could be handy for archiving the generated documentation or if some version @@ -61,14 +61,14 @@ PROJECT_BRIEF = # pixels and the maximum width should not exceed 200 pixels. Doxygen will copy # the logo to the output directory. -PROJECT_LOGO = //intranet/home/Afbeeldingen/PasserVR/Logos/Logo3NameRight100.png +PROJECT_LOGO = //intranet/home/Afbeeldingen/PasserVR/Logos/PasserLife/PasserLifeLogoLeft_300.png # The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) path # into which the generated documentation will be written. If a relative path is # entered, it will be relative to the location where doxygen was started. If # left blank the current directory will be used. -OUTPUT_DIRECTORY = //intranet/web/passer_life/apis/ControlCore/Csharp/ +OUTPUT_DIRECTORY = //intranet/web/roboidcontrol_doc/Csharp/ # If the CREATE_SUBDIRS tag is set to YES then doxygen will create up to 4096 # sub-directories (in 2 levels) under the output directory of each output format @@ -777,7 +777,7 @@ MAX_INITIALIZER_LINES = 30 # list will mention the files that were used to generate the documentation. # The default value is: YES. -SHOW_USED_FILES = YES +SHOW_USED_FILES = NO # Set the SHOW_FILES tag to NO to disable the generation of the Files page. This # will remove the Files entry from the Quick Index and from the Folder Tree View diff --git a/EchoStream.cs b/EchoStream.cs deleted file mode 100644 index 02b0781..0000000 --- a/EchoStream.cs +++ /dev/null @@ -1,175 +0,0 @@ -/* -using System; -using System.IO; -using System.Threading.Tasks; -using System.Threading; -using System.Collections.Concurrent; - -public class EchoStream : Stream { - public override bool CanTimeout { get; } = true; - public override int ReadTimeout { get; set; } = Timeout.Infinite; - public override int WriteTimeout { get; set; } = Timeout.Infinite; - public override bool CanRead { get; } = true; - public override bool CanSeek { get; } = false; - public override bool CanWrite { get; } = true; - - public bool CopyBufferOnWrite { get; set; } = false; - - private readonly object _lock = new object(); - - // Default underlying mechanism for BlockingCollection is ConcurrentQueue, which is what we want - private readonly BlockingCollection _Buffers; - private int _maxQueueDepth = 10; - - private byte[] m_buffer = null; - private int m_offset = 0; - private int m_count = 0; - - private bool m_Closed = false; - private bool m_FinalZero = false; //after the stream is closed, set to true after returning a 0 for read() - public override void Close() { - m_Closed = true; - - // release any waiting writes - _Buffers.CompleteAdding(); - } - - public bool DataAvailable { - get { - return _Buffers.Count > 0; - } - } - - private long _Length = 0L; - public override long Length { - get { - return _Length; - } - } - - private long _Position = 0L; - public override long Position { - get { - return _Position; - } - set { - throw new NotImplementedException(); - } - } - - public EchoStream() : this(10) { - } - - public EchoStream(int maxQueueDepth) { - _maxQueueDepth = maxQueueDepth; - _Buffers = new BlockingCollection(_maxQueueDepth); - } - - // we override the xxxxAsync functions because the default base class shares state between ReadAsync and WriteAsync, which causes a hang if both are called at once - public new Task WriteAsync(byte[] buffer, int offset, int count) { - return Task.Run(() => Write(buffer, offset, count)); - } - - // we override the xxxxAsync functions because the default base class shares state between ReadAsync and WriteAsync, which causes a hang if both are called at once - public new Task ReadAsync(byte[] buffer, int offset, int count) { - return Task.Run(() => { - return Read(buffer, offset, count); - }); - } - - public override void Write(byte[] buffer, int offset, int count) { - if (m_Closed || buffer.Length - offset < count || count <= 0) - return; - - byte[] newBuffer; - if (!CopyBufferOnWrite && offset == 0 && count == buffer.Length) - newBuffer = buffer; - else { - newBuffer = new byte[count]; - System.Buffer.BlockCopy(buffer, offset, newBuffer, 0, count); - } - if (!_Buffers.TryAdd(newBuffer, WriteTimeout)) - throw new TimeoutException("EchoStream Write() Timeout"); - - _Length += count; - } - - public override int Read(byte[] buffer, int offset, int count) { - if (count == 0) - return 0; - lock (_lock) { - if (m_count == 0 && _Buffers.Count == 0) { - if (m_Closed) { - if (!m_FinalZero) { - m_FinalZero = true; - return 0; - } - else { - return -1; - } - } - - if (_Buffers.TryTake(out m_buffer, ReadTimeout)) { - m_offset = 0; - m_count = m_buffer.Length; - } - else { - if (m_Closed) { - if (!m_FinalZero) { - m_FinalZero = true; - return 0; - } - else { - return -1; - } - } - else { - return 0; - } - } - } - - int returnBytes = 0; - while (count > 0) { - if (m_count == 0) { - if (_Buffers.TryTake(out m_buffer, 0)) { - m_offset = 0; - m_count = m_buffer.Length; - } - else - break; - } - - var bytesToCopy = (count < m_count) ? count : m_count; - System.Buffer.BlockCopy(m_buffer, m_offset, buffer, offset, bytesToCopy); - m_offset += bytesToCopy; - m_count -= bytesToCopy; - offset += bytesToCopy; - count -= bytesToCopy; - - returnBytes += bytesToCopy; - } - - _Position += returnBytes; - - return returnBytes; - } - } - - public override int ReadByte() { - byte[] returnValue = new byte[1]; - return (Read(returnValue, 0, 1) <= 0 ? -1 : (int)returnValue[0]); - } - - public override void Flush() { - } - - public override long Seek(long offset, SeekOrigin origin) { - throw new NotImplementedException(); - } - - public override void SetLength(long value) { - throw new NotImplementedException(); - } -} -*/ \ No newline at end of file diff --git a/LinearAlgebra/Angle.cs b/LinearAlgebra/Angle.cs index 71c44a6..0c23840 100644 --- a/LinearAlgebra/Angle.cs +++ b/LinearAlgebra/Angle.cs @@ -1,7 +1,11 @@ using System; -class Angle -{ - public static float Rad2Deg = 360.0f / ((float)Math.PI * 2); - public static float Deg2Rad = ((float)Math.PI * 2) / 360.0f; +namespace Passer.LinearAlgebra { + + public class Angle { + public const float pi = 3.1415927410125732421875F; + public static float Rad2Deg = 360.0f / ((float)Math.PI * 2); + public static float Deg2Rad = ((float)Math.PI * 2) / 360.0f; + } + } \ No newline at end of file diff --git a/LinearAlgebra/Direction.cs b/LinearAlgebra/Direction.cs index 9ab56fd..bd7477f 100644 --- a/LinearAlgebra/Direction.cs +++ b/LinearAlgebra/Direction.cs @@ -14,12 +14,12 @@ namespace Passer.LinearAlgebra { //Normalize(); } - public readonly static Direction forward = new(0, 0); - public readonly static Direction backward = new(-180, 0); - public readonly static Direction up = new(0, 90); - public readonly static Direction down = new(0, -90); - public readonly static Direction left = new(-90, 0); - public readonly static Direction right = new(90, 0); + public readonly static Direction forward = new Direction(0, 0); + public readonly static Direction backward = new Direction(-180, 0); + public readonly static Direction up = new Direction(0, 90); + public readonly static Direction down = new Direction(0, -90); + public readonly static Direction left = new Direction(-90, 0); + public readonly static Direction right = new Direction(90, 0); public void Normalize() { if (this.vertical > 90 || this.vertical < -90) { diff --git a/LinearAlgebra/Matrix.cs b/LinearAlgebra/Matrix.cs index 696c547..6327145 100644 --- a/LinearAlgebra/Matrix.cs +++ b/LinearAlgebra/Matrix.cs @@ -1,7 +1,10 @@ using System; using System.Diagnostics; -using Vector3 = UnityEngine.Vector3; -using Vector2 = UnityEngine.Vector2; +using Passer.LinearAlgebra; +#if UNITY_5_3_OR_NEWER +using Vector3Float = UnityEngine.Vector3; +using Vector2Float = UnityEngine.Vector2; +#endif using Quaternion = UnityEngine.Quaternion; public readonly struct Slice { @@ -60,7 +63,7 @@ public class Matrix2 { return new Matrix2(resultData); } - public static Matrix2 SkewMatrix(Vector3 v) { + public static Matrix2 SkewMatrix(Vector3Float v) { float[,] result = new float[3, 3] { {0, -v.z, v.y}, {v.z, 0, -v.x}, @@ -69,16 +72,16 @@ public class Matrix2 { return new Matrix2(result); } - public Vector3 GetRow3(int rowIx) { + public Vector3Float GetRow3(int rowIx) { uint cols = this.nCols; - Vector3 row = new() { + Vector3Float row = new() { x = this.data[rowIx, 0], y = this.data[rowIx, 1], z = this.data[rowIx, 2] }; return row; } - public void SetRow3(int rowIx, Vector3 v) { + public void SetRow3(int rowIx, Vector3Float v) { this.data[rowIx, 0] = v.x; this.data[rowIx, 1] = v.y; this.data[rowIx, 2] = v.z; @@ -174,12 +177,12 @@ public class Matrix2 { return new Matrix1(result); } - public static Vector3 operator *(Matrix2 A, Vector3 v) { - return new Vector3() { - x = A.data[0, 0] * v.x + A.data[0, 1] * v.y + A.data[0, 2] * v.z, - y = A.data[1, 0] * v.x + A.data[1, 1] * v.y + A.data[1, 2] * v.z, - z = A.data[2, 0] * v.x + A.data[2, 1] * v.y + A.data[2, 2] * v.z - }; + public static Vector3Float operator *(Matrix2 A, Vector3Float v) { + return new Vector3Float( + A.data[0, 0] * v.x + A.data[0, 1] * v.y + A.data[0, 2] * v.z, + A.data[1, 0] * v.x + A.data[1, 1] * v.y + A.data[1, 2] * v.z, + A.data[2, 0] * v.x + A.data[2, 1] * v.y + A.data[2, 2] * v.z + ); } public static Matrix2 operator *(Matrix2 A, float s) { @@ -360,14 +363,14 @@ public class Matrix1 { return new Matrix1(magnitude); } - public static Matrix1 FromVector2(Vector2 v) { + public static Matrix1 FromVector2(Vector2Float v) { float[] result = new float[2]; result[0] = v.x; result[1] = v.y; return new Matrix1(result); } - public static Matrix1 FromVector3(Vector3 v) { + public static Matrix1 FromVector3(Vector3Float v) { float[] result = new float[3]; result[0] = v.x; result[1] = v.y; @@ -384,18 +387,18 @@ public class Matrix1 { return new Matrix1(result); } - public Vector2 vector2 { + public Vector2Float vector2 { get { if (this.magnitude != 2) throw new System.ArgumentException("Matrix1 must be of size 2"); - return new Vector2(this.data[0], this.data[1]); + return new Vector2Float(this.data[0], this.data[1]); } } - public Vector3 vector3 { + public Vector3Float vector3 { get { if (this.magnitude != 3) throw new System.ArgumentException("Matrix1 must be of size 3"); - return new Vector3(this.data[0], this.data[1], this.data[2]); + return new Vector3Float(this.data[0], this.data[1], this.data[2]); } } public Quaternion quaternion { diff --git a/LinearAlgebra/Quat32.cs b/LinearAlgebra/Quat32.cs index b839b0e..6bebd04 100644 --- a/LinearAlgebra/Quat32.cs +++ b/LinearAlgebra/Quat32.cs @@ -42,7 +42,7 @@ namespace Passer.LinearAlgebra float yawOver2 = yaw * 0.5f; float sinYawOver2 = (float)Math.Sin((float)yawOver2); float cosYawOver2 = (float)Math.Cos((float)yawOver2); - Quat32 result = new() + Quat32 result = new Quat32() { w = cosYawOver2 * cosPitchOver2 * cosRollOver2 + sinYawOver2 * sinPitchOver2 * sinRollOver2, diff --git a/LinearAlgebra/Spherical.cs b/LinearAlgebra/Spherical.cs index e9c2b5b..be0c38a 100644 --- a/LinearAlgebra/Spherical.cs +++ b/LinearAlgebra/Spherical.cs @@ -1,12 +1,15 @@ -using UnityEngine; +using System; +#if UNITY_5_3_OR_NEWER +using Vector3Float = UnityEngine.Vector3; +#endif namespace Passer.LinearAlgebra { public class Spherical { public float distance; public Direction direction; - public static Spherical zero = new(0, 0, 0); - public static Spherical forward = new(1, 0, 0); + public static Spherical zero = new Spherical(0, 0, 0); + public static Spherical forward = new Spherical(1, 0, 0); public Spherical(float distance, float horizontal, float vertical) { this.distance = distance; @@ -17,30 +20,30 @@ namespace Passer.LinearAlgebra { this.direction = direction; } - public static Spherical FromVector3(Vector3 v) { + public static Spherical FromVector3(Vector3Float v) { float distance = v.magnitude; if (distance == 0.0f) return new Spherical(distance, 0, 0); else { - float verticalAngle = (Mathf.PI / 2 - Mathf.Acos(v.y / distance)) * Mathf.Rad2Deg; - float horizontalAngle = Mathf.Atan2(v.x, v.z) * Mathf.Rad2Deg; + float verticalAngle = (float)((Angle.pi / 2 - Math.Acos(v.y / distance)) * Angle.Rad2Deg); + float horizontalAngle = (float) Math.Atan2(v.x, v.z) * Angle.Rad2Deg; return new Spherical(distance, horizontalAngle, verticalAngle); } } - public Vector3 ToVector3() { - float verticalRad = (UnityEngine.Mathf.PI / 2) - this.direction.vertical * UnityEngine.Mathf.Deg2Rad; - float horizontalRad = this.direction.horizontal * UnityEngine.Mathf.Deg2Rad; - float cosVertical = UnityEngine.Mathf.Cos(verticalRad); - float sinVertical = UnityEngine.Mathf.Sin(verticalRad); - float cosHorizontal = UnityEngine.Mathf.Cos(horizontalRad); - float sinHorizontal = UnityEngine.Mathf.Sin(horizontalRad); + public Vector3Float ToVector3() { + float verticalRad = (Angle.pi / 2) - this.direction.vertical * Angle.Deg2Rad; + float horizontalRad = this.direction.horizontal * Angle.Deg2Rad; + float cosVertical = (float)Math.Cos(verticalRad); + float sinVertical = (float)Math.Sin(verticalRad); + float cosHorizontal = (float)Math.Cos(horizontalRad); + float sinHorizontal = (float)Math.Sin(horizontalRad); float x = this.distance * sinVertical * sinHorizontal; float y = this.distance * cosVertical; float z = this.distance * sinVertical * cosHorizontal; - Vector3 v = new Vector3(x, y, z); + Vector3Float v = new Vector3Float(x, y, z); return v; } } diff --git a/LinearAlgebra/SwingTwist.cs b/LinearAlgebra/SwingTwist.cs index 61774d1..7f4bbfd 100644 --- a/LinearAlgebra/SwingTwist.cs +++ b/LinearAlgebra/SwingTwist.cs @@ -6,7 +6,7 @@ namespace Passer.LinearAlgebra public Direction swing; public float twist; - public static readonly SwingTwist zero = new(0, 0, 0); + public static readonly SwingTwist zero = new SwingTwist(0, 0, 0); public SwingTwist(Direction swing, float twist) { @@ -24,7 +24,7 @@ namespace Passer.LinearAlgebra // UnityEngine.Quaternion q = new(q32.x, q32.y, q32.z, q32.w); // SwingTwist r = new(q.eulerAngles.y, q.eulerAngles.x, q.eulerAngles.z); q32.ToAngles(out float right, out float up, out float forward); - SwingTwist r = new(up, right, forward); + SwingTwist r = new SwingTwist(up, right, forward); return r; } } diff --git a/LinearAlgebra/Vector3.cs b/LinearAlgebra/Vector3.cs index c0bc6dd..0cc4d7b 100644 --- a/LinearAlgebra/Vector3.cs +++ b/LinearAlgebra/Vector3.cs @@ -1,5 +1,7 @@ -namespace Passer.LinearAlgebra { +#if !UNITY_5_3_OR_NEWER +using System; +namespace Passer.LinearAlgebra { public class Vector3Of { public T x; public T y; @@ -10,6 +12,10 @@ namespace Passer.LinearAlgebra { this.y = y; this.z = z; } + + // public uint magnitude { + // get => (float)Math.Sqrt(this.x * this.x + this.y * this.y + this.z * this.z); + // } } public class Vector3Int : Vector3Of { @@ -17,5 +23,11 @@ namespace Passer.LinearAlgebra { } public class Vector3Float : Vector3Of { public Vector3Float(float x, float y, float z) : base(x, y, z) { } + + public float magnitude { + get => (float)Math.Sqrt(this.x * this.x + this.y * this.y + this.z * this.z); + } } -} \ No newline at end of file +} + +#endif \ No newline at end of file diff --git a/float16.cs b/LinearAlgebra/float16.cs similarity index 87% rename from float16.cs rename to LinearAlgebra/float16.cs index b934651..c1885c3 100644 --- a/float16.cs +++ b/LinearAlgebra/float16.cs @@ -1,10 +1,8 @@ using System; -namespace Passer.LinearAlgebra -{ +namespace Passer.LinearAlgebra { - public class float16 - { + public class float16 { // // FILE: float16.cpp // AUTHOR: Rob Tillaart @@ -16,13 +14,11 @@ namespace Passer.LinearAlgebra public float16() { _value = 0; } - public float16(float f) - { + public float16(float f) { _value = f32tof16(f); } - public float toFloat() - { + public float toFloat() { return f16tof32(_value); } @@ -156,8 +152,7 @@ namespace Passer.LinearAlgebra // // CORE CONVERSION // - float f16tof32(ushort _value) - { + float f16tof32(ushort _value) { //ushort sgn; ushort man; int exp; @@ -172,13 +167,11 @@ namespace Passer.LinearAlgebra //Debug.Log($"{sgn} {exp} {man}"); // ZERO - if ((_value & 0x7FFF) == 0) - { + if ((_value & 0x7FFF) == 0) { return sgn ? -0 : 0; } // NAN & INF - if (exp == 0x001F) - { + if (exp == 0x001F) { if (man == 0) return sgn ? float.NegativeInfinity : float.PositiveInfinity; //-INFINITY : INFINITY; else @@ -192,44 +185,46 @@ namespace Passer.LinearAlgebra f = 1; // PROCESS MANTISSE - for (int i = 9; i >= 0; i--) - { + for (int i = 9; i >= 0; i--) { f *= 2; if ((man & (1 << i)) != 0) f = f + 1; } //Debug.Log($"{f}"); f = f * (float)Math.Pow(2.0f, exp - 25); - if (exp == 0) - { + if (exp == 0) { f = f * (float)Math.Pow(2.0f, -13); // 5.96046447754e-8; } //Debug.Log($"{f}"); return sgn ? -f : f; } - ushort f32tof16(float f) - { + public static uint SingleToInt32Bits(float value) { + byte[] bytes = BitConverter.GetBytes(value); + if (BitConverter.IsLittleEndian) + Array.Reverse(bytes); // If the system is little-endian, reverse the byte order + return BitConverter.ToUInt32(bytes, 0); + } + + ushort f32tof16(float f) { //uint t = *(uint*)&f; - uint t = (uint)BitConverter.SingleToInt32Bits(f); + //uint t = (uint)BitConverter.SingleToInt32Bits(f); + uint t = SingleToInt32Bits(f); // man bits = 10; but we keep 11 for rounding ushort man = (ushort)((t & 0x007FFFFF) >> 12); short exp = (short)((t & 0x7F800000) >> 23); bool sgn = (t & 0x80000000) != 0; // handle 0 - if ((t & 0x7FFFFFFF) == 0) - { + if ((t & 0x7FFFFFFF) == 0) { return sgn ? (ushort)0x8000 : (ushort)0x0000; } // denormalized float32 does not fit in float16 - if (exp == 0x00) - { + if (exp == 0x00) { return sgn ? (ushort)0x8000 : (ushort)0x0000; } // handle infinity & NAN - if (exp == 0x00FF) - { + if (exp == 0x00FF) { if (man != 0) return 0xFE00; // NAN return sgn ? (ushort)0xFC00 : (ushort)0x7C00; // -INF : INF @@ -238,13 +233,11 @@ namespace Passer.LinearAlgebra // normal numbers exp = (short)(exp - 127 + 15); // overflow does not fit => INF - if (exp > 30) - { + if (exp > 30) { return sgn ? (ushort)0xFC00 : (ushort)0x7C00; // -INF : INF } // subnormal numbers - if (exp < -38) - { + if (exp < -38) { return sgn ? (ushort)0x8000 : (ushort)0x0000; // -0 or 0 ? just 0 ? } if (exp <= 0) // subnormal diff --git a/Messages/BinaryMsg.cs b/Messages/BinaryMsg.cs new file mode 100644 index 0000000..e72c6fd --- /dev/null +++ b/Messages/BinaryMsg.cs @@ -0,0 +1,77 @@ +namespace Passer.RoboidControl { + + /// + /// A message containing binary data for custom communication + /// + public class BinaryMsg : IMessage { + /// + /// The message ID + /// + public const byte Id = 0xB1; + /// + /// The length of the message, excluding the binary data + /// + /// For the total size of the message this.bytes.Length should be added to this value. + public const byte length = 2; + /// + /// The network ID identifying the thing + /// + public byte networkId; + /// + /// The ID of the thing + /// + public byte thingId; + /// + /// The binary data + /// + public byte[] bytes; + + /// + /// Create a new message for sending + /// + /// The netowork ID of the thing + /// The ID of the thing + /// The binary data for the thing + public BinaryMsg(byte networkId, byte thingId, byte[] bytes) : base() { + this.networkId = networkId; + this.thingId = thingId; + this.bytes = bytes; + } + /// + /// Create an empty message for sending + /// + /// The netowork ID of the thing + /// The ID of the thing + public BinaryMsg(byte networkId, Thing thing) : base() { + this.networkId = networkId; + this.thingId = thing.id; + this.bytes = System.Array.Empty(); + } + /// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer) + public BinaryMsg(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++]; + } + + /// @copydoc Passer::RoboidControl::IMessage::Serialize + public override byte Serialize(ref byte[] buffer) { + if (buffer.Length < BinaryMsg.length + this.bytes.Length || this.bytes.Length == 0) + return 0; + + byte ix = 0; + buffer[ix++] = BinaryMsg.Id; + buffer[ix++] = this.networkId; + buffer[ix++] = this.thingId; + foreach (byte b in bytes) + buffer[ix++] = b; + + return ix; + } + } + +} \ No newline at end of file diff --git a/Messages/ClientMsg.cs b/Messages/ClientMsg.cs deleted file mode 100644 index d924b17..0000000 --- a/Messages/ClientMsg.cs +++ /dev/null @@ -1,23 +0,0 @@ -namespace Passer.Control.Core { - - public class ClientMsg : IMessage { - public const byte Id = 0xA0; - public const byte length = 2; - public byte networkId; - - public ClientMsg(byte networkId) { - this.networkId = networkId; - } - - public ClientMsg(byte[] buffer) { - this.networkId = buffer[1]; - } - - public override byte Serialize(ref byte[] buffer) { - byte ix = 0; - buffer[ix++] = ClientMsg.Id; - buffer[ix++] = networkId; - return ClientMsg.length; - } - } -} \ No newline at end of file diff --git a/Messages/CustomMsg.cs b/Messages/CustomMsg.cs deleted file mode 100644 index 75e4311..0000000 --- a/Messages/CustomMsg.cs +++ /dev/null @@ -1,51 +0,0 @@ -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 CustomMsg(byte networkId, Thing thing) : base() { - this.networkId = networkId; - this.thingId = thing.id; - } - - public override byte Serialize(ref byte[] buffer) { - if (bytes == null) - return 0; - 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/Messages/DestroyMsg.cs b/Messages/DestroyMsg.cs new file mode 100644 index 0000000..660c142 --- /dev/null +++ b/Messages/DestroyMsg.cs @@ -0,0 +1,51 @@ +namespace Passer.RoboidControl { + + /// + /// Message notifiying that a Thing no longer exists + /// + public class DestroyMsg : IMessage { + /// + /// The message ID + /// + public const byte Id = 0x20; + /// + /// The length of the message + /// + public const byte length = 3; + /// + /// The network ID of the thing + /// + public byte networkId; + /// + /// The ID of the thing + /// + public byte thingId; + + /// + /// Create a message for sending + /// + /// The network ID of the thing + /// The ID of the thing + public DestroyMsg(byte networkId, byte thingId) { + this.networkId = networkId; + this.thingId = thingId; + } + /// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer) + public DestroyMsg(byte[] buffer) : base(buffer) { } + + /// @copydoc Passer::RoboidControl::IMessage::Serialize + public override byte Serialize(ref byte[] buffer) { + if (buffer.Length < DestroyMsg.length) + return 0; + + byte ix = 0; + buffer[ix++] = DestroyMsg.Id; + buffer[ix++] = this.networkId; + buffer[ix++] = this.thingId; + + return ix; + } + + } + +} \ No newline at end of file diff --git a/Messages/InvestigateMsg.cs b/Messages/InvestigateMsg.cs new file mode 100644 index 0000000..d822fec --- /dev/null +++ b/Messages/InvestigateMsg.cs @@ -0,0 +1,46 @@ +namespace Passer.RoboidControl { + + /// + /// Message to request details for a Thing + /// + public class InvestigateMsg : IMessage { + /// + /// The message Id + /// + public const byte Id = 0x81; + /// + /// The length of the message + /// + public const byte length = 3; + /// + /// The network ID of the thing + /// + public byte networkId; + /// + /// The ID of the thing + /// + public byte thingId; + + /// + /// Create a new message for sending + /// + /// The network ID for the thing + /// The ID of the thing + public InvestigateMsg(byte networkId, byte thingId) { + this.networkId = networkId; + this.thingId = thingId; + } + /// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer) + public InvestigateMsg(byte[] buffer) : base(buffer) { } + + /// @copydoc Passer::RoboidControl::IMessage::Serialize + public override byte Serialize(ref byte[] buffer) { + byte ix = 0; + buffer[ix++] = InvestigateMsg.Id; + buffer[ix++] = this.networkId; + buffer[ix++] = this.thingId; + return ix; + } + } + +} \ No newline at end of file diff --git a/Messages/LowLevelMessages.cs b/Messages/LowLevelMessages.cs index 055096b..2a067ea 100644 --- a/Messages/LowLevelMessages.cs +++ b/Messages/LowLevelMessages.cs @@ -1,6 +1,6 @@ using Passer.LinearAlgebra; -namespace Passer.Control.Core +namespace Passer.RoboidControl { public class LowLevelMessages { @@ -17,7 +17,7 @@ namespace Passer.Control.Core float distance = ReceiveFloat16(data, ref ix); float horizontal = ReceiveAngle8(data, ref ix); float vertical = ReceiveAngle8(data, ref ix); - Spherical v = new(distance, horizontal, vertical); + Spherical v = new Spherical(distance, horizontal, vertical); return v; } @@ -48,7 +48,7 @@ namespace Passer.Control.Core } public static Quat32 ReceiveQuat32(byte[] data, ref byte ix) { - Quat32 q = new( + Quat32 q = new Quat32( (data[ix++] - 128.0F) / 127.0F, (data[ix++] - 128.0F) / 127.0F, (data[ix++] - 128.0F) / 127.0F, @@ -84,7 +84,7 @@ namespace Passer.Control.Core public static void SendFloat16(byte[] data, ref byte ix, float f) { - float16 f16 = new(f); + float16 f16 = new float16(f); ushort binary = f16.GetBinary(); data[ix++] = (byte)(binary >> 8); data[ix++] = (byte)(binary & 255); @@ -99,7 +99,7 @@ namespace Passer.Control.Core public static float ReceiveFloat16(byte[] data, ref byte ix) { ushort value = (ushort)(data[ix++] << 8 | data[ix++]); - float16 f16 = new(); + float16 f16 = new float16(); f16.SetBinary(value); float f = f16.toFloat(); return f; diff --git a/Messages/Messages.cs b/Messages/Messages.cs index 83ba4e5..d75e62b 100644 --- a/Messages/Messages.cs +++ b/Messages/Messages.cs @@ -1,253 +1,25 @@ -using System.IO; -using System.Threading.Tasks; - -using Passer.LinearAlgebra; -namespace Passer.Control.Core { +namespace Passer.RoboidControl { + /// + /// Root structure for all communcation messages + /// public class IMessage { public IMessage() { } + + /// + /// Create a message for receiving + /// + /// The byte array to parse public IMessage(byte[] buffer) { - Deserialize(buffer); + //Deserialize(buffer); } + /// + /// Serialize the message into a byte array for sending + /// + /// The buffer to serilize into + /// The length of the message in the buffer public virtual byte Serialize(ref byte[] buffer) { return 0; } - public virtual void Deserialize(byte[] buffer) { } - - public bool SendTo(Participant client) { - Serialize(ref client.buffer); - return client.SendBuffer(client.buffer.Length); - } - - public static bool SendMsg(Participant client, IMessage msg) { - msg.Serialize(ref client.buffer); - return client.SendBuffer(client.buffer.Length); - } - - public static bool PublishMsg(Participant client, IMessage msg) { - msg.Serialize(ref client.buffer); - return client.PublishBuffer(client.buffer.Length); - } - - public static async Task Receive(Stream dataStream, byte packetSize) { - byte[] buffer = new byte[packetSize - 1]; // without msgId - int byteCount = dataStream.Read(buffer, 0, packetSize - 1); - while (byteCount < packetSize - 1) { - // not all bytes have been read, wait and try again - await Task.Delay(1); - byteCount += dataStream.Read(buffer, byteCount, packetSize - 1 - byteCount); - } - return buffer; - } } - #region Investigate - - public class InvestigateMsg : IMessage { - public const byte Id = 0x81; - public const byte length = 3; - public byte networkId; - public byte thingId; - - public InvestigateMsg(byte networkId, byte thingId) { - this.networkId = networkId; - this.thingId = thingId; - } - public InvestigateMsg(byte[] buffer) : base(buffer) { } - - public override byte Serialize(ref byte[] buffer) { - byte ix = 0; - buffer[ix++] = InvestigateMsg.Id; - buffer[ix++] = this.networkId; - buffer[ix++] = this.thingId; - return ix; - } - public override void Deserialize(byte[] buffer) { - uint ix = 0; - this.networkId = buffer[ix++]; - this.thingId = buffer[ix++]; - } - - //public static bool Send(Participant client, CoreThing thing) { - // InvestigateMsg msg = new(thing.networkId, thing.id); - // return SendMsg(client, msg); - //} - public static async Task Receive(Stream dataStream, Participant client, byte packetSize) { - if (packetSize != length) - return false; - - byte[] buffer = await Receive(dataStream, packetSize); - InvestigateMsg msg = new(buffer); - //UnityEngine.Debug.Log($"Receive investigate [{msg.networkId}/{msg.thingId}]"); - - client.messageQueue.Enqueue(msg); - return true; - - } - } - - #endregion Investigate - - #region Pose - - public class PoseMsg : IMessage { - public const byte Id = 0x10; - public const byte length = 4 + 4 + 4; - public byte networkId; - public byte thingId; - - public byte poseType; - public const byte Pose_Position = 0x01; - public const byte Pose_Orientation = 0x02; - public const byte Pose_LinearVelocity = 0x04; - public const byte Pose_AngularVelocity = 0x08; - - public Spherical position; - public SwingTwist orientation; - public Spherical linearVelocity; - public Spherical angularVelocity; - - public PoseMsg(byte networkId, byte thingId, Spherical position, SwingTwist orientation) { - this.networkId = networkId; - this.thingId = thingId; - this.position = position; - this.orientation = orientation; - this.poseType = 0; - if (this.position != null) - this.poseType |= Pose_Position; - else - this.position = new Spherical(0, 0, 0); - if (this.orientation != null) - this.poseType |= Pose_Orientation; - else - this.orientation = SwingTwist.zero; - } - public PoseMsg(byte[] buffer) : base(buffer) { } - - public override byte Serialize(ref byte[] buffer) { - byte ix = 0; - buffer[ix++] = PoseMsg.Id; - buffer[ix++] = this.networkId; - buffer[ix++] = this.thingId; - buffer[ix++] = this.poseType; - - if ((poseType & Pose_Position) != 0) - LowLevelMessages.SendSpherical(buffer, ref ix, this.position); - if ((poseType & Pose_Orientation) != 0) - LowLevelMessages.SendQuat32(buffer, ref ix, this.orientation); - if ((poseType & Pose_LinearVelocity) != 0) - LowLevelMessages.SendSpherical(buffer, ref ix, this.linearVelocity); - if ((poseType & Pose_AngularVelocity) != 0) - LowLevelMessages.SendSpherical(buffer, ref ix, this.angularVelocity); - return ix; - } - public override void Deserialize(byte[] buffer) { - byte ix = 0; - this.networkId = buffer[ix++]; - this.thingId = buffer[ix++]; - this.poseType = buffer[ix++]; - - if ((poseType & Pose_Position) != 0) - this.position = LowLevelMessages.ReceiveSpherical(buffer, ref ix); - if ((poseType & Pose_Orientation) != 0) - this.orientation = LowLevelMessages.ReceiveSwingTwist(buffer, ref ix); - if ((poseType & Pose_LinearVelocity) != 0) - this.linearVelocity = LowLevelMessages.ReceiveSpherical(buffer, ref ix); - if ((poseType & Pose_AngularVelocity) != 0) - this.angularVelocity = LowLevelMessages.ReceiveSpherical(buffer, ref ix); - } - - public static bool Send(Participant client, byte thingId, Spherical position, SwingTwist orientation) { - PoseMsg msg = new(client.networkId, thingId, position, orientation); - return SendMsg(client, msg); - } - public static async Task Receive(Stream dataStream, Participant client, byte packetSize) { - byte[] buffer = await Receive(dataStream, packetSize); - PoseMsg msg = new(buffer); - - // Do no process poses with nwid == 0 (== local) - if (msg.networkId == 0) - return true; - - client.messageQueue.Enqueue(msg); - return true; - } - - public static bool SendTo(Participant participant, Thing thing) { - if (participant == null || thing == null) - return false; - - byte ix = 0; - participant.buffer[ix++] = PoseMsg.Id; - participant.buffer[ix++] = participant.networkId; - participant.buffer[ix++] = thing.id; - participant.buffer[ix++] = thing.poseUpdated; - - if ((thing.poseUpdated & Pose_Position) != 0 && thing.position != null) - LowLevelMessages.SendSpherical(participant.buffer, ref ix, thing.position); - if ((thing.poseUpdated & Pose_Orientation) != 0 && thing.orientation != null) - LowLevelMessages.SendQuat32(participant.buffer, ref ix, thing.orientation); - if ((thing.poseUpdated & Pose_LinearVelocity) != 0 && thing.linearVelocity != null) - LowLevelMessages.SendSpherical(participant.buffer, ref ix, thing.linearVelocity); - if ((thing.poseUpdated & Pose_AngularVelocity) != 0 && thing.angularVelocity != null) - LowLevelMessages.SendSpherical(participant.buffer, ref ix, thing.angularVelocity); - - return participant.SendBuffer(ix); - } - } - - #endregion Pose - - #region Text - - public class TextMsg : IMessage { - public const byte Id = 0xB0; - public string text; - - public TextMsg(byte[] buffer) : base(buffer) { } - public override void Deserialize(byte[] buffer) { - uint ix = 0; - uint strlen = buffer[ix++]; - this.text = System.Text.Encoding.UTF8.GetString(buffer, (int)ix, (int)strlen); - } - - public static async Task Receive(Stream dataStream, Participant client, byte packetSize) { - byte[] buffer = await Receive(dataStream, packetSize); - TextMsg msg = new(buffer); - - client.messageQueue.Enqueue(msg); - return true; - } - } - - #endregion - - #region Destroy - - public class DestroyMsg : IMessage { - public const byte Id = 0x20; - public const byte length = 3; - public byte networkId; - public byte thingId; - - public DestroyMsg(byte[] buffer) : base(buffer) { } - - public override void Deserialize(byte[] buffer) { - this.networkId = buffer[0]; - this.thingId = buffer[1]; - } - - public static async Task Receive(Stream dataStream, Participant client, byte packetSize) { - if (packetSize != length) - return false; - - byte[] buffer = await Receive(dataStream, packetSize); - DestroyMsg msg = new(buffer); - - client.messageQueue.Enqueue(msg); - return true; - } - } - - - #endregion Destroy } diff --git a/ModelUrlMsg.cs b/Messages/ModelUrlMsg.cs similarity index 51% rename from ModelUrlMsg.cs rename to Messages/ModelUrlMsg.cs index a3c02ba..01b6527 100644 --- a/ModelUrlMsg.cs +++ b/Messages/ModelUrlMsg.cs @@ -1,24 +1,52 @@ -#nullable enable - -namespace Passer.Control.Core { +namespace Passer.RoboidControl { + /// + /// Message for communicating the URL for a model of the thing + /// public class ModelUrlMsg : IMessage { + /// + /// The message ID + /// public const byte Id = 0x90; // (144) Model URL + /// + /// The length of the message without th URL itself + /// public const byte length = 4; + /// + /// The network ID of the thing + /// public byte networkId; + /// + /// The ID of the thing + /// public byte thingId; - public string? url = null; + /// + /// The URL of the model + /// + public string url = null; + /// + /// Create a new message for sending + /// + /// The network ID of the thing + /// The thing for which to send the model URL public ModelUrlMsg(byte networkId, Thing thing) { this.networkId = networkId; this.thingId = thing.id; this.url = thing.modelUrl; } - public ModelUrlMsg(byte networkId, byte thingId, string url, float scale = 1) { + /// + /// 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.url = url; } + /// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer) public ModelUrlMsg(byte[] buffer) { byte ix = 1; this.networkId = buffer[ix++]; @@ -28,6 +56,7 @@ namespace Passer.Control.Core { url = System.Text.Encoding.UTF8.GetString(buffer, (int)ix, strlen); } + /// @copydoc Passer::RoboidControl::IMessage::Serialize public override byte Serialize(ref byte[] buffer) { if (this.url == null) return 0; diff --git a/Messages/NameMsg.cs b/Messages/NameMsg.cs index 3d99f18..57de3aa 100644 --- a/Messages/NameMsg.cs +++ b/Messages/NameMsg.cs @@ -1,25 +1,56 @@ -#nullable enable - -namespace Passer.Control.Core { +namespace Passer.RoboidControl { + /// + /// Message for communicating the name of a thing + /// public class NameMsg : IMessage { + /// + /// The message ID + /// public const byte Id = 0x91; // 145 + /// + /// The length of the message without the name string + /// public const byte length = 4; + /// + /// The network ID of the thing + /// public byte networkId; + /// + /// The ID of the thing + /// public byte thingId; + /// + /// The length of the name, excluding null terminator + /// public byte len; - public string? name = null; + /// + /// The name of the thing, not terminated with a null character + /// + public string name = ""; + /// + /// Create a new message for sending + /// + /// The network ID of the thing + /// The thing public NameMsg(byte networkId, Thing thing) { this.networkId = networkId; this.thingId = thing.id; this.name = thing.name; } + /// + /// Create a new message for sending + /// + /// The network ID of the thing + /// The ID of the thing + /// The name of the thing public NameMsg(byte networkId, byte thingId, string name) { this.networkId = networkId; this.thingId = thingId; this.name = name; } + /// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer) public NameMsg(byte[] buffer) { byte ix = 1; this.networkId = buffer[ix++]; @@ -28,8 +59,9 @@ namespace Passer.Control.Core { this.name = System.Text.Encoding.UTF8.GetString(buffer, (int)ix, strlen); } + /// @copydoc Passer::RoboidControl::IMessage::Serialize public override byte Serialize(ref byte[] buffer) { - if (this.name == null) + if (buffer.Length < NameMsg.length + this.name.Length || this.name.Length == 0) return 0; byte ix = 0; @@ -42,6 +74,7 @@ namespace Passer.Control.Core { buffer[ix++] = (byte)nameLength; for (int nameIx = 0; nameIx < nameLength; nameIx++) buffer[ix++] = (byte)this.name[nameIx]; + return ix; } } diff --git a/Messages/NetworkIdMsg.cs b/Messages/NetworkIdMsg.cs index 6b65913..fc095d0 100644 --- a/Messages/NetworkIdMsg.cs +++ b/Messages/NetworkIdMsg.cs @@ -1,18 +1,39 @@ -namespace Passer.Control.Core { +namespace Passer.RoboidControl { + /// + /// A message communicating the network ID for that participant + /// public class NetworkIdMsg : IMessage { + /// + /// The message ID + /// public const byte Id = 0xA1; + /// + /// The length of the message + /// public const byte length = 2; + /// + /// The network ID for the participant + /// public byte networkId; + /// + /// Create a new message for sending + /// + /// The network ID for the participant public NetworkIdMsg(byte networkId) { this.networkId = networkId; } + /// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer) public NetworkIdMsg(byte[] buffer) { this.networkId = buffer[1]; } + /// @copydoc Passer::RoboidControl::IMessage::Serialize public override byte Serialize(ref byte[] buffer) { + if (buffer.Length < NetworkIdMsg.length) + return 0; + buffer[0] = NetworkIdMsg.Id; buffer[1] = this.networkId; return NetworkIdMsg.length; diff --git a/Messages/ParticipantMsg.cs b/Messages/ParticipantMsg.cs new file mode 100644 index 0000000..6ba3228 --- /dev/null +++ b/Messages/ParticipantMsg.cs @@ -0,0 +1,53 @@ +namespace Passer.RoboidControl { + + /// + /// A participant messages notifies other participants of its presence + /// + /// When received by another participant, it can be followed by a NetworkIdMsg + /// to announce that participant to this client such that it can join privately + public class ParticipantMsg : IMessage { + /// + /// The message ID + /// + public const byte Id = 0xA0; + /// + /// The length of the message + /// + public const byte length = 2; + /// + /// The network ID known by the participant + /// + public byte networkId; + + /// + /// Create a new message for sending + /// + /// The network ID known by the participant + public ParticipantMsg(byte networkId) { + this.networkId = networkId; + } + + /// + /// Create a message for receiving + /// + /// The byte array to parse + public ParticipantMsg(byte[] buffer) { + this.networkId = buffer[1]; + } + + /// + /// Serialize the message into a byte array + /// + /// The buffer to serialize into + /// The length of the message in the buffer + public override byte Serialize(ref byte[] buffer) { + if (buffer.Length < ParticipantMsg.length) + return 0; + + byte ix = 0; + buffer[ix++] = ParticipantMsg.Id; + buffer[ix++] = networkId; + return ParticipantMsg.length; + } + } +} \ No newline at end of file diff --git a/Messages/PoseMsg.cs b/Messages/PoseMsg.cs new file mode 100644 index 0000000..4f1320d --- /dev/null +++ b/Messages/PoseMsg.cs @@ -0,0 +1,133 @@ +using Passer.LinearAlgebra; + +namespace Passer.RoboidControl { + + /// + /// Message to communicate the pose of the thing + /// + /// The pose is in local space relative to the parent. If there is not parent (the thing is a root thing), the pose will be in world space. + public class PoseMsg : IMessage { + /// + /// The message ID + /// + public const byte Id = 0x10; + /// + /// The length of the message + /// + public const byte length = 4 + 4 + 4; + /// + /// The network ID of the thing + /// + public byte networkId; + /// + /// The ID of the thing + /// + public byte thingId; + + /// + /// Bit pattern stating which pose components are available + /// + public byte poseType; + /// + /// Bit pattern for a pose with position + /// + public const byte Pose_Position = 0x01; + /// + /// Bit pattern for a pose with orientation + /// + public const byte Pose_Orientation = 0x02; + /// + /// Bit pattern for a pose with linear velocity + /// + public const byte Pose_LinearVelocity = 0x04; + /// + /// Bit pattern for a pose with angular velocity + /// + public const byte Pose_AngularVelocity = 0x08; + + /// + /// The position of the thing in local space in meters + /// + public Spherical position = Spherical.zero; + /// + /// The orientation of the thing in local space + /// + public SwingTwist orientation = SwingTwist.zero; + /// + /// The linear velocity of the thing in local space in meters per second + /// + public Spherical linearVelocity = Spherical.zero; + /// + /// The angular velocity of the thing in local space + /// + public Spherical angularVelocity = Spherical.zero; + + /// + /// 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; + } + /// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer) + public PoseMsg(byte[] buffer) : base(buffer) { + byte ix = 1; + this.networkId = buffer[ix++]; + this.thingId = buffer[ix++]; + + this.poseType = buffer[ix++]; + this.position = null; + this.orientation = null; + this.linearVelocity = null; + this.angularVelocity = null; + if ((this.poseType & Pose_Position) != 0) + this.position = LowLevelMessages.ReceiveSpherical(buffer, ref ix); + if ((this.poseType & Pose_Orientation) != 0) + this.orientation = SwingTwist.FromQuat32(LowLevelMessages.ReceiveQuat32(buffer, ref ix)); + if ((this.poseType & Pose_LinearVelocity) != 0) + this.linearVelocity = LowLevelMessages.ReceiveSpherical(buffer, ref ix); + if ((this.poseType & Pose_AngularVelocity) != 0) + this.angularVelocity = LowLevelMessages.ReceiveSpherical(buffer, ref ix); + } + + /// @copydoc Passer::RoboidControl::IMessage::Serialize + public override byte Serialize(ref byte[] buffer) { + byte ix = 0; + buffer[ix++] = PoseMsg.Id; + buffer[ix++] = this.networkId; + buffer[ix++] = this.thingId; + buffer[ix++] = this.poseType; + + if ((poseType & Pose_Position) != 0) + LowLevelMessages.SendSpherical(buffer, ref ix, this.position); + if ((poseType & Pose_Orientation) != 0) + LowLevelMessages.SendQuat32(buffer, ref ix, this.orientation); + if ((poseType & Pose_LinearVelocity) != 0) + LowLevelMessages.SendSpherical(buffer, ref ix, this.linearVelocity); + if ((poseType & Pose_AngularVelocity) != 0) + LowLevelMessages.SendSpherical(buffer, ref ix, this.angularVelocity); + return ix; + } + + } + +} \ No newline at end of file diff --git a/Messages/TextMsg.cs b/Messages/TextMsg.cs new file mode 100644 index 0000000..c2dcf16 --- /dev/null +++ b/Messages/TextMsg.cs @@ -0,0 +1,45 @@ +namespace Passer.RoboidControl { + + /// + /// Message for sending generic text + /// + public class TextMsg : IMessage { + /// + /// The message ID + /// + public const byte Id = 0xB0; + /// + /// The length of the message without the text itself + /// + public const byte length = 2; + /// + /// The text + /// + public string text = ""; + + /// + /// Create a new message for sending + /// + /// The text to send + public TextMsg(string text) { + this.text = text; + } + + /// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer) + public TextMsg(byte[] buffer) : base(buffer) { } + + /// @copydoc Passer::RoboidControl::IMessage::Serialize + public override byte Serialize(ref byte[] buffer) { + if (buffer.Length < TextMsg.length + this.text.Length || this.text.Length == 0) + return 0; + + byte ix = 0; + buffer[ix++] = TextMsg.Id; + buffer[ix++] = (byte)this.text.Length; + for (int textIx = 0; textIx < this.text.Length; textIx++) + buffer[ix++] = (byte)this.text[textIx]; + return ix; + } + } + +} \ No newline at end of file diff --git a/Messages/ThingMsg.cs b/Messages/ThingMsg.cs new file mode 100644 index 0000000..e83827d --- /dev/null +++ b/Messages/ThingMsg.cs @@ -0,0 +1,90 @@ +namespace Passer.RoboidControl { + + /// + /// Message providing generic information about a Thing + /// + public class ThingMsg : IMessage { + /// + /// The message ID + /// + public const byte id = 0x80; + /// + /// The length of the message + /// + public const byte length = 5; + /// + /// The network ID of the thing + /// + public byte networkId; + /// + /// The ID of the thing + /// + public byte thingId; + /// + /// The Thing.Type of the thing + /// + public byte thingType; + /// + /// The parent of the thing in the hierarachy. This is null for root Things + /// + public byte parentId; + + /// + /// Create a message for sending + /// + /// The network ID of the thing + /// The thing + public ThingMsg(byte networkId, Thing thing) { + this.networkId = networkId; + this.thingId = thing.id; + this.thingType = thing.type; + if (thing.parent != null) + this.parentId = thing.parent.id; + else + 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 + public ThingMsg(byte[] buffer) { + uint ix = 1; + this.networkId = buffer[ix++]; + this.thingId = buffer[ix++]; + this.thingType = buffer[ix++]; + this.parentId = buffer[ix]; + } + + /// + /// Serialize the message into a byte array + /// + /// The buffer to serialize into + /// The length of the message in the bufer. 0 when the buffer was too small + public override byte Serialize(ref byte[] buffer) { + if (buffer.Length < ThingMsg.length) + return 0; + + byte ix = 0; + buffer[ix++] = ThingMsg.id; + buffer[ix++] = this.networkId; + buffer[ix++] = this.thingId; + buffer[ix++] = this.thingType; + buffer[ix++] = this.parentId; + return ThingMsg.length; + } + } + +} \ No newline at end of file diff --git a/ModelUrlMsg.cs.meta b/ModelUrlMsg.cs.meta deleted file mode 100644 index 7f9b6b8..0000000 --- a/ModelUrlMsg.cs.meta +++ /dev/null @@ -1,2 +0,0 @@ -fileFormatVersion: 2 -guid: 711bdb0248c9f6848a6b4da15cc05db5 \ No newline at end of file diff --git a/Participant.cs b/Participant.cs index d95d39c..349f50d 100644 --- a/Participant.cs +++ b/Participant.cs @@ -4,20 +4,22 @@ using System.Collections.Concurrent; using System.Net; using System.Net.Sockets; -namespace Passer.Control.Core { +namespace Passer.RoboidControl { + /// + /// A participant is used for communcation between things + /// public class Participant : RemoteParticipant { public byte[] buffer = new byte[1024]; public ulong publishInterval = 3000; // = 3 seconds - //public byte networkId = 0; public string name = "Participant"; public IPEndPoint endPoint = null; public UdpClient udpClient = null; public string broadcastIpAddress = "255.255.255.255"; - public readonly ConcurrentQueue messageQueue = new(); + public readonly ConcurrentQueue messageQueue = new ConcurrentQueue(); #region Init @@ -32,9 +34,9 @@ namespace Passer.Control.Core { /// Create a participant with the give UDP port /// /// The port number on which to communicate - public Participant(int port) : this() { - this.port = port; - } + // public Participant(int port) : this() { + // this.port = port; + // } /// /// Create a new participant for a site at the given address and port @@ -45,10 +47,12 @@ namespace Passer.Control.Core { this.ipAddress = ipAddress; this.port = port; - this.endPoint = new IPEndPoint(IPAddress.Parse(ipAddress), port); // for sending + this.udpClient = new UdpClient(); + this.udpClient.Client.Bind(new IPEndPoint(IPAddress.Any, port)); // local port + // this.endPoint = new IPEndPoint(IPAddress.Parse(ipAddress), port); // for sending - this.udpClient = new UdpClient(port); // for receiving - this.udpClient.Client.Bind(new IPEndPoint(IPAddress.Any, port)); + // this.udpClient = new UdpClient(port); // for receiving + // this.udpClient.Client.Bind(new IPEndPoint(IPAddress.Any, port)); this.udpClient.BeginReceive(new AsyncCallback(result => ReceiveUDP(result)), null); } @@ -62,7 +66,7 @@ namespace Passer.Control.Core { this.port = port; } - public List senders = new(); + public List senders = new List(); public RemoteParticipant GetParticipant(string ipAddress, int port) { //Console.WriteLine($"Get Participant {ipAddress}:{port}"); @@ -74,18 +78,18 @@ namespace Passer.Control.Core { } public RemoteParticipant AddParticipant(string ipAddress, int port) { // Console.WriteLine($"New Participant {ipAddress}:{port}"); - RemoteParticipant participant = new(ipAddress, port) { + RemoteParticipant participant = new RemoteParticipant(ipAddress, port) { networkId = (byte)this.senders.Count }; senders.Add(participant); return participant; } - protected readonly Dictionary> thingMsgProcessors = new(); + protected readonly Dictionary> thingMsgProcessors = new Dictionary>(); - public delegate Thing ThingConstructor(byte networkId, byte thingId); + public delegate Thing ThingConstructor(RemoteParticipant sender, byte networkId, byte thingId); public void Register(byte thingType, ThingConstructor constr) { - thingMsgProcessors[thingType] = new Func(constr); + thingMsgProcessors[thingType] = new Func(constr); } public void Register(Thing.Type thingType) where ThingClass : Thing { @@ -93,8 +97,8 @@ namespace Passer.Control.Core { } public void Register(byte thingType) where ThingClass : Thing { - thingMsgProcessors[thingType] = (byte networkId, byte thingId) => - Activator.CreateInstance(typeof(ThingClass), networkId, thingId) as ThingClass; + thingMsgProcessors[thingType] = (RemoteParticipant sender, byte networkId, byte thingId) => + Activator.CreateInstance(typeof(ThingClass), sender, networkId, thingId) as ThingClass; Console.WriteLine($"Registering {typeof(ThingClass)} for thing type {thingType}"); } @@ -103,21 +107,24 @@ namespace Passer.Control.Core { #region Update protected void ReceiveUDP(IAsyncResult result) { - if (udpClient == null || this.endPoint == null) + if (this.udpClient == null || this.endPoint == null) return; - byte[] data = udpClient.EndReceive(result, ref this.endPoint); + byte[] data = this.udpClient.EndReceive(result, ref this.endPoint); // This does not yet take multi-packet messages into account! + if (this.endPoint == null) + return; // We can receive our own publish (broadcast) packages. How do we recognize them???? // It is hard to determine our source port - RemoteParticipant remoteParticipant = this.GetParticipant(endPoint.Address.ToString(), endPoint.Port); + string ipAddress = this.endPoint.Address.ToString(); + RemoteParticipant remoteParticipant = GetParticipant(ipAddress, this.endPoint.Port); if (remoteParticipant == null) - remoteParticipant = this.AddParticipant(endPoint.Address.ToString(), endPoint.Port); + remoteParticipant = AddParticipant(ipAddress, this.endPoint.Port); ReceiveData(data, remoteParticipant); - udpClient.BeginReceive(new AsyncCallback(result => ReceiveUDP(result)), null); + udpClient.BeginReceive(new AsyncCallback(callbackResult => ReceiveUDP(callbackResult)), null); } protected ulong nextPublishMe = 0; @@ -143,7 +150,7 @@ namespace Passer.Control.Core { } public virtual void Publish() { - this.Publish(new ClientMsg(this.networkId)); + this.Publish(new ParticipantMsg(this.networkId)); } #endregion Update @@ -155,7 +162,7 @@ namespace Passer.Control.Core { this.Send(remoteParticipant, new ThingMsg(this.networkId, thing)); this.Send(remoteParticipant, new NameMsg(this.networkId, thing)); this.Send(remoteParticipant, new ModelUrlMsg(this.networkId, thing)); - this.Send(remoteParticipant, new CustomMsg(this.networkId, thing)); + this.Send(remoteParticipant, new BinaryMsg(this.networkId, thing)); } public bool Send(IMessage msg) { @@ -173,7 +180,7 @@ namespace Passer.Control.Core { if (bufferSize <= 0) return true; - IPEndPoint participantEndpoint = new(IPAddress.Parse(remoteParticipant.ipAddress), remoteParticipant.port); + IPEndPoint participantEndpoint = new IPEndPoint(IPAddress.Parse(remoteParticipant.ipAddress), remoteParticipant.port); Console.WriteLine($"msg to {participantEndpoint.Address.ToString()} {participantEndpoint.Port}"); this.udpClient?.Send(this.buffer, bufferSize, participantEndpoint); return true; @@ -184,7 +191,7 @@ namespace Passer.Control.Core { this.Publish(new ThingMsg(this.networkId, thing)); this.Publish(new NameMsg(this.networkId, thing)); this.Publish(new ModelUrlMsg(this.networkId, thing)); - this.Publish(new CustomMsg(this.networkId, thing)); + this.Publish(new BinaryMsg(this.networkId, thing)); } public bool Publish(IMessage msg) { @@ -227,8 +234,8 @@ namespace Passer.Control.Core { } switch (msgId) { - case ClientMsg.Id: // 0xA0 / 160 - this.Process(remoteParticipant, new ClientMsg(data)); + case ParticipantMsg.Id: // 0xA0 / 160 + this.Process(remoteParticipant, new ParticipantMsg(data)); break; case NetworkIdMsg.Id: // 0xA1 / 161 this.Process(remoteParticipant, new NetworkIdMsg(data)); @@ -246,10 +253,11 @@ namespace Passer.Control.Core { this.Process(remoteParticipant, new ModelUrlMsg(data)); break; case PoseMsg.Id: // 0x10 / 16 + this.Process(remoteParticipant, new PoseMsg(data)); // result = await PoseMsg.Receive(dataStream, client, packetSize); break; - case CustomMsg.Id: // 0xB1 / 177 - this.Process(remoteParticipant, new CustomMsg(data)); + case BinaryMsg.Id: // 0xB1 / 177 + this.Process(remoteParticipant, new BinaryMsg(data)); break; case TextMsg.Id: // 0xB0 / 176 // result = await TextMsg.Receive(dataStream, client, packetSize); @@ -266,7 +274,7 @@ namespace Passer.Control.Core { #region Process - protected virtual void Process(RemoteParticipant sender, ClientMsg msg) { } + protected virtual void Process(RemoteParticipant sender, ParticipantMsg msg) { } protected virtual void Process(RemoteParticipant sender, NetworkIdMsg msg) { Console.WriteLine($"{this.name} receive network id {this.networkId} {msg.networkId}"); @@ -292,11 +300,33 @@ namespace Passer.Control.Core { protected virtual void Process(RemoteParticipant sender, ModelUrlMsg msg) { Console.WriteLine($"Participant: Process model [{msg.networkId}/{msg.thingId}] {msg.url}"); + Thing thing = sender.Get(msg.networkId, msg.thingId); + if (thing != null) + thing.modelUrl = msg.url; } - protected virtual void Process(PoseMsg msg) { } + protected virtual void Process(RemoteParticipant sender, PoseMsg msg) { + //Console.WriteLine($"Participant: Process pose [{msg.networkId}/{msg.thingId}] {msg.poseType}"); + Thing thing = sender.Get(msg.networkId, msg.thingId); + if (thing != null) { + thing.hasPosition = false; + if ((msg.poseType & PoseMsg.Pose_Position) != 0) { + thing.position = msg.position; + thing.hasPosition = true; + } + if ((msg.poseType & PoseMsg.Pose_Orientation) != 0) + thing.orientation = msg.orientation; + else + thing.orientation = null; + if ((msg.poseType & PoseMsg.Pose_LinearVelocity) != 0) + thing.linearVelocity = msg.linearVelocity; + if ((msg.poseType & PoseMsg.Pose_AngularVelocity) != 0) + thing.angularVelocity = msg.angularVelocity; + + } + } - protected virtual void Process(RemoteParticipant sender, CustomMsg msg) { + protected virtual void Process(RemoteParticipant sender, BinaryMsg msg) { // Console.WriteLine($"Participant: Process binary [{msg.networkId}/{msg.thingId}]"); Thing thing = sender.Get(msg.networkId, msg.thingId); thing?.ProcessBinary(msg.bytes); @@ -311,7 +341,9 @@ namespace Passer.Control.Core { if (client == this) continue; //UnityEngine.Debug.Log($"---> {client.ipAddress}"); - IMessage.SendMsg(client, msg); + //IMessage.SendMsg(client, msg); + msg.Serialize(ref client.buffer); + client.SendBuffer(client.buffer.Length); } } diff --git a/README.md b/README.md index 338a37c..a094cee 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,10 @@ -\mainpage Control Core for C# +\mainpage Roboid Control for C# -Control Core contains generic functionality for Controlling Things. \ No newline at end of file +Roboid Control support for C# applications. +Includes support for the Unity game engine. + +# Basic components + +- Passer::RoboidControl::Thing +- Passer::RoboidControl::Participant +- Passer::RoboidControl::SiteServer \ No newline at end of file diff --git a/RemoteParticipant.cs b/RemoteParticipant.cs index 67d8b13..64b9fb4 100644 --- a/RemoteParticipant.cs +++ b/RemoteParticipant.cs @@ -1,24 +1,51 @@ using System; using System.Collections.Generic; -namespace Passer.Control.Core { +namespace Passer.RoboidControl { + /// + /// A reference to a participant, possibly on a remote location + /// public class RemoteParticipant { + /// + /// The internet address of the participant + /// public string ipAddress = "0.0.0.0"; + /// + /// The UDP port on which the participant can be reached + /// public int port = 0; + /// + /// The network ID of the participant + /// public byte networkId; - public RemoteParticipant() { - - } + /// + /// Default constructor + /// + public RemoteParticipant() {} + /// + /// Create a new remote participant + /// + /// The IP address of the participant + /// The UDP port of the participant public RemoteParticipant(string ipAddress, int port) { this.ipAddress = ipAddress; this.port = port; } - protected readonly List things = new(); + /// + /// The things reported by this participant + /// + protected readonly List things = new List(); + /// + /// Get a thing with the given ids + /// + /// The network ID of the thing + /// The ID of the thing + /// The thing when it is found, null in other cases. public Thing Get(byte networkId, byte thingId) { Thing thing = things.Find(aThing => Thing.IsThing(aThing, networkId, thingId)); // if (thing == null) @@ -26,6 +53,11 @@ namespace Passer.Control.Core { return thing; } + /// + /// Add a new thing for this participant + /// + /// The thing to add + /// Invoke an notification event when the thing has been added public void Add(Thing thing, bool invokeEvent = true) { // Console.WriteLine($"added thing [{thing.networkId}/{thing.id}]"); Thing foundThing = Get(thing.networkId, thing.id); diff --git a/Sensors.meta b/Sensors.meta deleted file mode 100644 index 6c54616..0000000 --- a/Sensors.meta +++ /dev/null @@ -1,8 +0,0 @@ -fileFormatVersion: 2 -guid: a5a7a42365df0d0459195576ad3bb50b -folderAsset: yes -DefaultImporter: - externalObjects: {} - userData: - assetBundleName: - assetBundleVariant: diff --git a/Sensors/DistanceSensor.cs b/Sensors/DistanceSensor.cs index 0845d21..3967ae5 100644 --- a/Sensors/DistanceSensor.cs +++ b/Sensors/DistanceSensor.cs @@ -1,22 +1,39 @@ -using System; - -namespace Passer.Control.Core { +namespace Passer.RoboidControl { + /// + /// A sensor measuring the distance in the forward direction + /// public class DistanceSensor : Thing { + /// + /// The current measured distance + /// public float distance = 0; - public DistanceSensor() : base(true) { } - - public DistanceSensor(byte networkId, byte thingId) : base(null, networkId, thingId, (byte)Type.TemperatureSensor) { + /// + /// Constructor for a new distance sensor + /// + /// The participant for which the sensor is needed + public DistanceSensor(RemoteParticipant participant) : base(participant, true) { } + /// + /// Create a distance sensor with the given ID + /// + /// The participant for with the sensor is needed + /// The network ID of the sensor + /// The ID of the thing + public DistanceSensor(RemoteParticipant participant, byte networkId, byte thingId) : base(participant, networkId, thingId, (byte)Type.TemperatureSensor) { } #if UNITY_5_3_OR_NEWER + /// @copydoc Passer::RoboidControl::Thing::CreateComponent public override void CreateComponent() { - this.component = Unity.DistanceSensor.Create(this.parent); + this.component = Unity.DistanceSensor.Create(this); this.component.core = this; } #endif - + /// + /// Function to extract the distance received in the binary message + /// + /// The byte array public override void ProcessBinary(byte[] bytes) { byte ix = 0; this.distance = LowLevelMessages.ReceiveFloat16(bytes, ref ix); diff --git a/Sensors/TemperatureSensor.cs b/Sensors/TemperatureSensor.cs index b6e61b5..edb9750 100644 --- a/Sensors/TemperatureSensor.cs +++ b/Sensors/TemperatureSensor.cs @@ -1,17 +1,32 @@ -using System; +//using System; -namespace Passer.Control.Core { +namespace Passer.RoboidControl { + /// + /// A temperature sensor + /// public class TemperatureSensor : Thing { - public float temp = 0; - - public TemperatureSensor(byte networkId, byte thingId) : base(null, networkId, thingId, (byte)Type.TemperatureSensor) { - } + /// + /// The measured temperature + /// + public float temperature = 0; + /// + /// Create a temperature sensor with the given ID + /// + /// The participant for with the sensor is needed + /// The network ID of the sensor + /// The ID of the thing + public TemperatureSensor(Participant participant, byte networkId, byte thingId) : base(participant, networkId, thingId, (byte)Type.TemperatureSensor) {} + + /// + /// Function to extract the temperature received in the binary message + /// + /// The byte array public override void ProcessBinary(byte[] bytes) { byte ix = 0; - this.temp = LowLevelMessages.ReceiveFloat16(bytes, ref ix); - Console.WriteLine($"temperature {this.name} = {this.temp} C"); + this.temperature = LowLevelMessages.ReceiveFloat16(bytes, ref ix); + //Console.WriteLine($"temperature {this.name} = {this.temperature} C"); } } diff --git a/Sensors/TemperatureSensor.cs.meta b/Sensors/TemperatureSensor.cs.meta deleted file mode 100644 index 69b6d6a..0000000 --- a/Sensors/TemperatureSensor.cs.meta +++ /dev/null @@ -1,2 +0,0 @@ -fileFormatVersion: 2 -guid: 5cd9e2534695ec844bd23b7a48ad498a \ No newline at end of file diff --git a/Sensors/TouchSensor.cs b/Sensors/TouchSensor.cs new file mode 100644 index 0000000..f6bc873 --- /dev/null +++ b/Sensors/TouchSensor.cs @@ -0,0 +1,34 @@ +namespace Passer.RoboidControl { + + /// + /// A sensor which can detect touches + /// + public class TouchSensor : Thing { + /// + /// Value which is true when the sensor is touching something, false otherwise + /// + public bool touchedSomething = false; + + /// + /// Create a touch sensor + /// + /// The participant for with the sensor is needed + /// True when the creation should trigger an event + public TouchSensor(RemoteParticipant participant, bool invokeEvent = true) : base(participant, invokeEvent) { + touchedSomething = false; + } + + public TouchSensor(RemoteParticipant participant, byte networkId, byte thingId) : base(participant, networkId, thingId) { + touchedSomething = false; + } + +#if UNITY_5_3_OR_NEWER + /// @copydoc Passer::RoboidControl::Thing::CreateComponent + public override void CreateComponent() { + System.Console.Write("Create touch sensor component"); + this.component = Unity.TouchSensor.Create(this); + this.component.core = this; + } +#endif + } +} \ No newline at end of file diff --git a/SiteServer.cs b/SiteServer.cs index 09e3a5f..9bd1ae9 100644 --- a/SiteServer.cs +++ b/SiteServer.cs @@ -1,16 +1,24 @@ using System; -using System.Collections.Generic; using System.Net; using System.Net.Sockets; -namespace Passer.Control.Core { +namespace Passer.RoboidControl { + /// + /// A site server is a participant which provides a shared simulated environment + /// public class SiteServer : Participant { - public SiteServer(int port = 7681) { + public SiteServer(int port = 7681) : this("0.0.0.0", port) { } + + /// + /// Create a new site server + /// + /// + public SiteServer(string ipAddress = "0.0.0.0", int port = 7681) : base() { this.name = "Site Server"; - this.ipAddress = "0.0.0.0"; + this.ipAddress = ipAddress; this.port = port; this.endPoint = new IPEndPoint(IPAddress.Parse(ipAddress), port); // for sending @@ -20,16 +28,21 @@ namespace Passer.Control.Core { this.udpClient.BeginReceive( new AsyncCallback(result => ReceiveUDP(result)), new Tuple(this.udpClient, new(IPAddress.Any, port))); + + Register(Thing.Type.TouchSensor); } + /// + /// Close the site + /// public void Close() { - this.udpClient.Close(); + this.udpClient?.Close(); } - public override void Publish() { + public override void Publish() { } - protected override void Process(RemoteParticipant sender, ClientMsg msg) { + protected override void Process(RemoteParticipant sender, ParticipantMsg msg) { if (msg.networkId == 0) { Console.WriteLine($"{this.name} received New Client -> {sender.networkId}"); this.Send(sender, new NetworkIdMsg(sender.networkId)); @@ -39,14 +52,27 @@ namespace Passer.Control.Core { protected override void Process(RemoteParticipant sender, NetworkIdMsg msg) { } protected override void Process(RemoteParticipant sender, ThingMsg msg) { - //Console.WriteLine($"SiteServer: Process thing [{msg.networkId}/{msg.thingId}]"); + Console.WriteLine($"SiteServer: Process thing [{msg.networkId}/{msg.thingId}]"); Thing thing = sender.Get(msg.networkId, msg.thingId); if (thing == null) { - Thing newThing; - if (thingMsgProcessors.ContainsKey(msg.thingType)) - newThing = thingMsgProcessors[msg.thingType](msg.networkId, msg.thingId); - else + Thing newThing = null; + if (thingMsgProcessors.TryGetValue(msg.thingType, out Func value)) { + // Console.WriteLine("Found thing message processor"); + if (value != null) + newThing = value(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) + Console.WriteLine("Could not find parent"); + else + newThing.parent = parentThing; + } + sender.Add(newThing); } } diff --git a/Thing.cs b/Thing.cs index c78851a..92d929d 100644 --- a/Thing.cs +++ b/Thing.cs @@ -2,23 +2,49 @@ using System; using System.Collections.Generic; using Passer.LinearAlgebra; -namespace Passer.Control.Core { +namespace Passer.RoboidControl { /// - /// A thing is the basic building block + /// A thing is the primitive building block /// [Serializable] public class Thing { #region Types + #endregion Types + + #region Properties + + public delegate void ChangeHandler(); + public delegate void SphericalHandler(Spherical v); + public delegate void ThingHandler(Thing t); + + /// + /// The participant to which this thing belongs + /// + public RemoteParticipant participant; + + /// + /// The network ID of this thing. + /// + public byte networkId; + /// + /// The ID of this thing + /// + public byte id; + + /// + /// Predefined thing types + /// public enum Type { - Undeterment, + Undetermined, // Sensor Switch, DistanceSensor, DirectionalSensor, TemperatureSensor, + TouchSensor, // Motor ControlledMotor, UncontrolledMotor, @@ -28,21 +54,20 @@ namespace Passer.Control.Core { Humanoid, ExternalSensor }; + /// + /// The type of this thing. This can be either a Thing::Type (needs casting) + /// or a byte value for custom types. + /// + public byte type; - #endregion Types - - #region Properties - - public RemoteParticipant participant; - - public delegate void ChangeHandler(); - public delegate void SphericalHandler(Spherical v); - - public byte networkId; - public byte id; - - public event ChangeHandler OnParentChanged; + /// + /// Event which is triggered when the parent changes + /// + public event ChangeHandler OnParentChanged = delegate { }; private Thing _parent; + /// + /// The parent of this thing + /// public Thing parent { get => _parent; set { @@ -50,7 +75,7 @@ namespace Passer.Control.Core { return; if (value == null) { - _parent.RemoveChild(this); + _parent?.RemoveChild(this); _parent = null; } else { @@ -60,22 +85,38 @@ namespace Passer.Control.Core { } } + /// + /// Attach a thing as a child of this thing + /// + /// The thing to attach as a child public void AddChild(Thing child) { if (children.Find(thing => thing == child) != null) return; child._parent = this; children.Add(child); } + /// + /// Remove the given thing as a child of this thing + /// + /// The child to remove public void RemoveChild(Thing child) { children.Remove(child); } - [System.NonSerialized] + /// + /// The list of children of this thing + /// + [NonSerialized] public List children = new List(); - public byte type; - public event ChangeHandler OnNameChanged; - private string _name; + /// + /// Event which is triggered when the name changes + /// + public event ChangeHandler OnNameChanged = delegate { }; + private string _name = ""; + /// + /// The name of the thing + /// public virtual string name { get => _name; set { @@ -86,11 +127,19 @@ namespace Passer.Control.Core { } } - public string modelUrl; + /// + /// An URL pointing to the location where a model of the thing can be found + /// + public string modelUrl = ""; - public byte poseUpdated = 0x00; - public event ChangeHandler OnPositionChanged; - private Spherical _position; + /// + /// Event triggered when the position has changed + /// + public event ChangeHandler OnPositionChanged = delegate { }; + private Spherical _position = Spherical.zero; + /// + /// The position of the thing in local space, in meters. + /// public Spherical position { get { return _position; } set { @@ -100,9 +149,16 @@ namespace Passer.Control.Core { } } } + public bool hasPosition = false; - public event ChangeHandler OnOrientationChanged; - private SwingTwist _orientation; + /// + /// Event triggered when the orientation has changed + /// + public event ChangeHandler OnOrientationChanged = delegate { }; + private SwingTwist _orientation = SwingTwist.zero; + /// + /// The orientation of the thing in local space + /// public SwingTwist orientation { get { return _orientation; } set { @@ -113,8 +169,14 @@ namespace Passer.Control.Core { } } - public event SphericalHandler OnLinearVelocityChanged; - private Spherical _linearVelocity; + /// + /// Event triggered when the linear velocity has changed + /// + public event SphericalHandler OnLinearVelocityChanged = delegate { }; + private Spherical _linearVelocity = Spherical.zero; + /// + /// The linear velocity of the thing in local space in meters per second + /// public Spherical linearVelocity { get => _linearVelocity; set { @@ -124,85 +186,113 @@ namespace Passer.Control.Core { } } } - public Spherical angularVelocity; + /// + /// The angular velocity of the thing in local space + /// + public Spherical angularVelocity = Spherical.zero; #if UNITY_5_3_OR_NEWER + /// + /// A reference to the representation of the thing in Unity + /// [NonSerialized] - public Unity.Thing component; + public Unity.Thing component = null; #endif #endregion Properties #region Init - // public virtual void Init(bool invokeEvent = false) { - // if (invokeEvent) - // InvokeNewThing(this); - // } - - public Thing(bool invokeEvent = false) { + /// + /// Create a new thing for the given participant + /// + /// The participant for which this thing is created + /// True when a new thing event should be triggered + public Thing(RemoteParticipant participant, bool invokeEvent = false) { + this.participant = participant; if (invokeEvent) InvokeNewThing(this); } - public Thing(RemoteParticipant sender, byte networkId, byte thingId, byte thingType = 0) { - this.participant = sender; + /// + /// Create a new thing for the given participant + /// + /// The participant for which this thing is created + /// The network ID of the thing + /// The ID of the thing + /// The type of thing + public Thing(RemoteParticipant participant, byte networkId, byte thingId, byte thingType = 0) { + this.participant = participant; this.id = thingId; this.type = thingType; this.networkId = networkId; - //this.Init(); - //OnNewThing?.Invoke(this); - //Thing.Add(this); } - public virtual void CreateComponent() {} + /// + /// Function which can be used to create components in external engines. + /// + /// Currently this is used to create GameObjects in Unity + public virtual void CreateComponent() { +#if UNITY_5_3_OR_NEWER + this.component = Unity.Thing.Create(this); + this.component.core = this; +#endif + } #endregion Init #region Update #if UNITY_5_3_OR_NEWER + /// + /// Convience function for use in Unity which removes the need for a currentTime argument + /// public void Update() { Update((ulong)UnityEngine.Time.time * 1000); } #endif + /// + /// Update this thing + /// + /// The current time in milliseconds public virtual void Update(ulong currentTime) { // should recurse over children... } + /// + /// Function used to generate binary data for this thing + /// + /// a byte array with the binary data + /// @sa Passer::RoboidControl::BinaryMsg public virtual byte[] GenerateBinary() { return new byte[0]; } + /// + /// Function used to process binary data received for this thing + /// + /// The binary data public virtual void ProcessBinary(byte[] bytes) { - //if (sensor != null) - // sensor.ProcessBytes(bytes); } #endregion Update - // Experimental - - // public float stressLevel = 0; - - // protected delegate void ReceptorFunc(Sensor sensor); - // protected void SetupReceptor(Sensor sensor, ReceptorFunc receptor) { - // sensor.Signaller += (sensor => Receptor(receptor, sensor)); - // } - // protected void Receptor(ReceptorFunc receptor, Sensor sensor) { - // if (sensor.signalStrength <= stressLevel) - // return; - - // receptor(sensor); - // } - - //---------- All Things - - private static readonly List allThings = new(); - - public delegate void ThingHandler(Thing t); - public static event ThingHandler OnNewThing; + /// + /// Event triggered when a new thing has been created + /// + public static event ThingHandler OnNewThing = delegate { }; + /// + /// Trigger the creation for the given thing + /// + /// The created thing public static void InvokeNewThing(Thing thing) { - OnNewThing?.Invoke(thing); + OnNewThing?.Invoke(thing); } + /// + /// Check if the thing has the given properaties + /// + /// The thing to check + /// The network ID to compare to + /// The thing ID to compare to + /// True when the thing has the given properties public static bool IsThing(Thing thing, byte networkId, byte thingId) { if (thing == null) return false; diff --git a/ThingMsg.cs b/ThingMsg.cs deleted file mode 100644 index 723c720..0000000 --- a/ThingMsg.cs +++ /dev/null @@ -1,45 +0,0 @@ -namespace Passer.Control.Core { - - public class ThingMsg : IMessage { - public const byte length = 5; - public const byte id = 0x80; - public byte networkId; - public byte thingId; - public byte thingType; - public byte parentId; - - public ThingMsg(byte networkId, Thing thing) { - this.networkId = networkId; - this.thingId = thing.id; - this.thingType = thing.type; - if (thing.parent != null) - this.parentId = thing.parent.id; - else - this.parentId = 0; - } - public ThingMsg(byte networkId, byte thingId, byte thingType, byte parentId) { - this.networkId = networkId; - this.thingId = thingId; - this.thingType = thingType; - this.parentId = parentId; - } - public ThingMsg(byte[] buffer) { - uint ix = 1; - this.networkId = buffer[ix++]; - this.thingId = buffer[ix++]; - this.thingType = buffer[ix++]; - this.parentId = buffer[ix]; - } - - public override byte Serialize(ref byte[] buffer) { - byte ix = 0; - buffer[ix++] = ThingMsg.id; - buffer[ix++] = this.networkId; - buffer[ix++] = this.thingId; - buffer[ix++] = this.thingType; - buffer[ix++] = this.parentId; - return ThingMsg.length; - } - } - -} \ No newline at end of file diff --git a/ThingMsg.cs.meta b/ThingMsg.cs.meta deleted file mode 100644 index 0c7dce0..0000000 --- a/ThingMsg.cs.meta +++ /dev/null @@ -1,2 +0,0 @@ -fileFormatVersion: 2 -guid: 718e148be3eeb65498334ed008747482 \ No newline at end of file diff --git a/Unity/DebugConsole.cs b/Unity/DebugConsole.cs index cb4de46..5b4e4f6 100644 --- a/Unity/DebugConsole.cs +++ b/Unity/DebugConsole.cs @@ -3,7 +3,7 @@ using System.IO; using System.Text; using UnityEngine; -namespace Passer.Control.Unity { +namespace Passer.RoboidControl.Unity { public class UnityLogWriter : TextWriter { public override void Write(char value) { diff --git a/Unity/DistanceSensor.cs b/Unity/DistanceSensor.cs index ac74b78..f8cc98a 100644 --- a/Unity/DistanceSensor.cs +++ b/Unity/DistanceSensor.cs @@ -2,31 +2,51 @@ using System.Collections; using UnityEngine; -namespace Passer.Control.Unity { +namespace Passer.RoboidControl.Unity { + /// + /// The Unity representation of a distance sensor + /// public class DistanceSensor : Thing { - public new Core.DistanceSensor core { - get => (Core.DistanceSensor)base.core; + /// + /// The core distance sensor + /// + public new RoboidControl.DistanceSensor core { + get => (RoboidControl.DistanceSensor)base.core; set => base.core = value; } + /// + /// Start the Unity representation + /// protected virtual void Start() { - if (core == null) - SetCoreThing(new Core.DistanceSensor()); + if (core == null) { + SiteServer siteServer = FindAnyObjectByType(); + SetCoreThing(new RoboidControl.DistanceSensor(siteServer.site)); + } StartCoroutine(MeasureDistance()); } - public static DistanceSensor Create(Core.Thing parent) { + /// + /// Create the Unity representation of the distance sensor + /// + /// The parent of the core distance sensor + /// The Unity representation of the distance sensor + public static DistanceSensor Create(RoboidControl.DistanceSensor core) { GameObject distanceObj = new("Distance sensor"); DistanceSensor component = distanceObj.AddComponent(); - if (parent != null && parent.component != null) - distanceObj.transform.SetParent(parent.component.transform, false); + if (core.parent != null && core.parent.component != null) + distanceObj.transform.SetParent(core.parent.component.transform, false); return component; } + /// + /// Periodically measure the distance + /// + /// IEnumerator MeasureDistance() { while (Application.isPlaying) { if (Physics.Raycast(this.transform.position, this.transform.forward, out RaycastHit hitInfo, 2.0f)) { diff --git a/Unity/SiteServer.cs b/Unity/SiteServer.cs index 6431eb3..ce665de 100644 --- a/Unity/SiteServer.cs +++ b/Unity/SiteServer.cs @@ -3,34 +3,36 @@ using System; using System.Collections.Generic; using UnityEngine; -namespace Passer.Control.Unity { +namespace Passer.RoboidControl.Unity { public class SiteServer : MonoBehaviour { - public Core.SiteServer site; + public RoboidControl.SiteServer site; - public Queue thingQueue = new(); + public Queue thingQueue = new(); protected virtual void Awake() { Console.SetOut(new UnityLogWriter()); site = new(7681); - Core.Thing.OnNewThing += HandleNewThing; + RoboidControl.Thing.OnNewThing += HandleNewThing; + + //site.Register(RoboidControl.Thing.Type.TouchSensor); } void OnApplicationQuit() { site.Close(); } - public void HandleNewThing(Core.Thing thing) { + public void HandleNewThing(RoboidControl.Thing thing) { + // Debug.Log("Handle New thing event"); site.Add(thing, false); thingQueue.Enqueue(thing); } protected virtual void Update() { site.Update((ulong)(Time.time * 1000)); - if (thingQueue.TryDequeue(out Core.Thing thing)) { + while (thingQueue.TryDequeue(out RoboidControl.Thing thing)) thing.CreateComponent(); - } } } diff --git a/Unity/Thing.cs b/Unity/Thing.cs index a280ca2..8515edb 100644 --- a/Unity/Thing.cs +++ b/Unity/Thing.cs @@ -1,14 +1,27 @@ #if UNITY_5_3_OR_NEWER +using System.Collections; using UnityEngine; +using UnityEngine.Networking; -namespace Passer.Control.Unity { +namespace Passer.RoboidControl.Unity { + /// + /// The representation of a Thing in Unity + /// public class Thing : MonoBehaviour { + /// + /// The core C# thing + /// [field: SerializeField] - public Core.Thing core {get; set; } + public RoboidControl.Thing core { get; set; } - protected void SetCoreThing(Core.Thing thing) { + private string modelUrl = null; + + /// + /// Set the core C# thing + /// + protected void SetCoreThing(RoboidControl.Thing thing) { core = thing; core.component = this; @@ -18,21 +31,82 @@ namespace Passer.Control.Unity { return; } siteServer.site.Add(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); + Thing component = gameObj.AddComponent(); + + component.core = core; + if (core.parent != null && core.parent.component != null) + gameObj.transform.SetParent(core.parent.component.transform, false); + + if (core.position != null) + gameObj.transform.localPosition = core.position.ToVector3(); + + return component; + } + + /// + /// Update the Unity representation + /// protected virtual void Update() { if (core == null) return; if (core.linearVelocity != null) { Vector3 direction = Quaternion.AngleAxis(core.linearVelocity.direction.horizontal, Vector3.up) * Vector3.forward; - this.transform.Translate(core.linearVelocity.distance * Time.deltaTime * direction); + this.transform.Translate(core.linearVelocity.distance * Time.deltaTime * direction, Space.Self); } if (core.angularVelocity != null) { Vector3 angularVelocity = core.angularVelocity.ToVector3(); - this.transform.rotation *= Quaternion.Euler(angularVelocity * Time.deltaTime); + this.transform.localRotation *= Quaternion.Euler(angularVelocity * Time.deltaTime); + } + + if (core.hasPosition) + this.transform.localPosition = core.position.ToVector3(); + //this.transform.localRotation = core.orientation.ToQuaternion(); + + if (!string.IsNullOrEmpty(core.modelUrl) && this.modelUrl == null) { + string extension = core.modelUrl.Substring(core.modelUrl.LastIndexOf(".")); + if (extension == ".jpg" || extension == ".png") { + StartCoroutine(LoadJPG()); + } + + this.modelUrl = core.modelUrl; } } + + private IEnumerator LoadJPG() { + UnityWebRequest request = UnityWebRequestTexture.GetTexture(core.modelUrl); + yield return request.SendWebRequest(); + + if (request.result == UnityWebRequest.Result.Success) { + Texture2D texture = ((DownloadHandlerTexture)request.downloadHandler).texture; + float aspectRatio = (float)texture.width / (float)texture.height; + + GameObject modelQuad = GameObject.CreatePrimitive(PrimitiveType.Quad); + Collider c = modelQuad.GetComponent(); + c.enabled = false; + Destroy(c); + modelQuad.transform.SetParent(this.transform, false); + modelQuad.transform.localEulerAngles = new(90, -90, 0); + modelQuad.transform.localScale = new Vector3(aspectRatio, 1, 1) / 5; + Material quadMaterial = new(Shader.Find("Unlit/Transparent")) { + mainTexture = texture + }; + modelQuad.GetComponent().material = quadMaterial; + } + else { + Debug.LogError("Failed to load image: " + request.error); + } + } + + } } diff --git a/Unity/TouchSensor.cs b/Unity/TouchSensor.cs new file mode 100644 index 0000000..62f6b51 --- /dev/null +++ b/Unity/TouchSensor.cs @@ -0,0 +1,72 @@ +#if UNITY_5_3_OR_NEWER +using UnityEngine; + +namespace Passer.RoboidControl.Unity { + + /// + /// The Unity representation of the TouchSensor + /// + public class TouchSensor : Thing { + + /// + /// The core touch sensor + /// + public RoboidControl.TouchSensor coreSensor { + get => (RoboidControl.TouchSensor)base.core; + } + + /// + /// Start the Unity represention + /// + protected virtual void Start() { + if (core == null) { + SiteServer siteServer = FindAnyObjectByType(); + SetCoreThing(new RoboidControl.TouchSensor(siteServer.site)); + } + } + + /// + /// Create the Unity representation + /// + /// The core touch sensor + /// The Unity representation of the touch sensor + public static TouchSensor Create(RoboidControl.TouchSensor core) { + GameObject gameObj = core.name != null ? + new(core.name) : + new("Touch Sensor"); + TouchSensor component = gameObj.AddComponent(); + Rigidbody rb = gameObj.AddComponent(); + rb.isKinematic = true; + + SphereCollider collider = gameObj.AddComponent(); + collider.radius = 0.01F; + collider.isTrigger = true; + + component.core = core; + if (core.parent != null && core.parent.component != null) + gameObj.transform.SetParent(core.parent.component.transform, false); + + if (core.position != null) + gameObj.transform.localPosition = core.position.ToVector3(); + + return component; + } + + private void OnTriggerEnter(Collider other) { + if (other.isTrigger) + return; + if (this.transform.root == other.transform.root) + return; + + // Debug.Log($"touched {other.gameObject.name}"); + this.coreSensor.touchedSomething = true; + } + private void OnTriggerExit(Collider other) { + if (other.isTrigger) + return; + + this.coreSensor.touchedSomething = false; + } + } +} +#endif \ No newline at end of file diff --git a/float16.cs.meta b/float16.cs.meta deleted file mode 100644 index a22c5ad..0000000 --- a/float16.cs.meta +++ /dev/null @@ -1,2 +0,0 @@ -fileFormatVersion: 2 -guid: 620fef383ba64a44995a234a71b2f189 \ No newline at end of file diff --git a/test.sln.meta b/test.sln.meta deleted file mode 100644 index 606fb16..0000000 --- a/test.sln.meta +++ /dev/null @@ -1,7 +0,0 @@ -fileFormatVersion: 2 -guid: d5d87461365fd8a4da528aa84a49e62c -DefaultImporter: - externalObjects: {} - userData: - assetBundleName: - assetBundleVariant: diff --git a/test/UnitTest1.cs b/test/UnitTest1.cs index 00fd906..f32d4b8 100644 --- a/test/UnitTest1.cs +++ b/test/UnitTest1.cs @@ -3,7 +3,7 @@ using System; using System.Threading; using NUnit.Framework; -using Passer.Control.Core; +using Passer.RoboidControl; namespace ControlCore.test { public class Tests { @@ -13,7 +13,7 @@ namespace ControlCore.test { [Test] public void Test_Participant() { - Participant participant = new("127.0.0.1", 7681); + Participant participant = new Participant("127.0.0.1", 7682); ulong milliseconds = (ulong)DateTimeOffset.UtcNow.ToUnixTimeMilliseconds(); ulong startTime = milliseconds; @@ -29,7 +29,7 @@ namespace ControlCore.test { [Test] public void Test_SiteServer() { - SiteServer siteServer = new(7681); + SiteServer siteServer = new SiteServer(7681); ulong milliseconds = (ulong)DateTimeOffset.UtcNow.ToUnixTimeMilliseconds(); ulong startTime = milliseconds; @@ -45,8 +45,8 @@ namespace ControlCore.test { [Test] public void Test_SiteParticipant() { - SiteServer siteServer = new(7681); - Participant participant = new("127.0.0.1", 7681); + SiteServer siteServer = new SiteServer(7681); + Participant participant = new Participant("127.0.0.1", 7681); ulong milliseconds = (ulong)DateTimeOffset.UtcNow.ToUnixTimeMilliseconds(); ulong startTime = milliseconds; @@ -63,9 +63,9 @@ namespace ControlCore.test { [Test] public void Test_ThingMsg() { - SiteServer siteServer = new(); - Participant participant = new("127.0.0.1"); - Thing thing = new() { + SiteServer siteServer = new SiteServer(); + Participant participant = new Participant("127.0.0.1"); + Thing thing = new Thing(participant) { name = "First Thing", modelUrl = "https://passer.life/extras/ant.jpg" }; diff --git a/test/UnitTest1.cs.meta b/test/UnitTest1.cs.meta deleted file mode 100644 index 8ee19d8..0000000 --- a/test/UnitTest1.cs.meta +++ /dev/null @@ -1,2 +0,0 @@ -fileFormatVersion: 2 -guid: 743b128a79ef8414fa29d7bb3b9e2ac8 \ No newline at end of file diff --git a/test/test.csproj b/test/test.csproj index b0f97b9..1c81788 100644 --- a/test/test.csproj +++ b/test/test.csproj @@ -1,24 +1,15 @@  - net9.0 - latest - enable - enable + net5.0 false true - - - - - - - - - + + + diff --git a/test/test.csproj.meta b/test/test.csproj.meta deleted file mode 100644 index 1040180..0000000 --- a/test/test.csproj.meta +++ /dev/null @@ -1,7 +0,0 @@ -fileFormatVersion: 2 -guid: 92729868a8379c04197dcb80d0276a63 -DefaultImporter: - externalObjects: {} - userData: - assetBundleName: - assetBundleVariant: