From d88d3a879de3056144056fc60c122e0adb1c8407 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Mon, 17 Feb 2025 08:59:22 +0100 Subject: [PATCH 01/20] Fix merge error --- Unity/DistanceSensor.cs | 1 - 1 file changed, 1 deletion(-) diff --git a/Unity/DistanceSensor.cs b/Unity/DistanceSensor.cs index b7a8d0d..ac74b78 100644 --- a/Unity/DistanceSensor.cs +++ b/Unity/DistanceSensor.cs @@ -7,7 +7,6 @@ namespace Passer.Control.Unity { public class DistanceSensor : Thing { public new Core.DistanceSensor core { - get => (Core.DistanceSensor)base.core; get => (Core.DistanceSensor)base.core; set => base.core = value; } From 6551c653c3fefffc82912c5891b90268d8453d51 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Mon, 17 Feb 2025 12:56:39 +0100 Subject: [PATCH 02/20] Touch & smell sensors --- Sensors/TouchSensor.cs | 16 ++++++++++++ Unity/TouchSensor.cs | 57 ++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 73 insertions(+) create mode 100644 Sensors/TouchSensor.cs create mode 100644 Unity/TouchSensor.cs diff --git a/Sensors/TouchSensor.cs b/Sensors/TouchSensor.cs new file mode 100644 index 0000000..ac2a660 --- /dev/null +++ b/Sensors/TouchSensor.cs @@ -0,0 +1,16 @@ + +namespace Passer.Control.Core { + public class TouchSensor : Thing { + public Thing touchedThing = null; + + public TouchSensor(bool invokeEvent = true) : base(invokeEvent) { + } + +#if UNITY_5_3_OR_NEWER + public override void CreateComponent() { + this.component = Unity.TouchSensor.Create(this); + this.component.core = this; + } +#endif + } +} \ No newline at end of file diff --git a/Unity/TouchSensor.cs b/Unity/TouchSensor.cs new file mode 100644 index 0000000..b2aac45 --- /dev/null +++ b/Unity/TouchSensor.cs @@ -0,0 +1,57 @@ +#if UNITY_5_3_OR_NEWER +using System.Collections; +using UnityEngine; + +namespace Passer.Control.Unity { + + public class TouchSensor : Thing { + + public Core.TouchSensor coreSensor { + get => (Core.TouchSensor)base.core; + } + + protected virtual void Start() { + if (core == null) + SetCoreThing(new Core.TouchSensor()); + + //StartCoroutine(MeasureDistance()); + } + bool update = false; + + protected override void Update() { + base.Update(); + this.update= false; + } + + public static TouchSensor Create(Core.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) { + Debug.Log("Touch!"); + this.coreSensor.touchedThing = other.transform.GetComponentInParent().core; + } + private void OnTriggerExit(Collider other) { + this.coreSensor.touchedThing = null; + } + } +} +#endif \ No newline at end of file From 923cb317af593c1f7a3157e6fa7c6299c67351e5 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Mon, 17 Feb 2025 16:02:29 +0100 Subject: [PATCH 03/20] Grabbing food --- Sensors/TouchSensor.cs | 3 ++- Unity/TouchSensor.cs | 21 +++++++++------------ 2 files changed, 11 insertions(+), 13 deletions(-) diff --git a/Sensors/TouchSensor.cs b/Sensors/TouchSensor.cs index ac2a660..1553ee8 100644 --- a/Sensors/TouchSensor.cs +++ b/Sensors/TouchSensor.cs @@ -1,7 +1,8 @@ namespace Passer.Control.Core { public class TouchSensor : Thing { - public Thing touchedThing = null; + //public Thing touchedThing = null; + public bool touchedSomething = false; public TouchSensor(bool invokeEvent = true) : base(invokeEvent) { } diff --git a/Unity/TouchSensor.cs b/Unity/TouchSensor.cs index b2aac45..4cf41b8 100644 --- a/Unity/TouchSensor.cs +++ b/Unity/TouchSensor.cs @@ -1,5 +1,4 @@ #if UNITY_5_3_OR_NEWER -using System.Collections; using UnityEngine; namespace Passer.Control.Unity { @@ -14,13 +13,6 @@ namespace Passer.Control.Unity { if (core == null) SetCoreThing(new Core.TouchSensor()); - //StartCoroutine(MeasureDistance()); - } - bool update = false; - - protected override void Update() { - base.Update(); - this.update= false; } public static TouchSensor Create(Core.TouchSensor core) { @@ -30,7 +22,7 @@ namespace Passer.Control.Unity { TouchSensor component = gameObj.AddComponent(); Rigidbody rb = gameObj.AddComponent(); rb.isKinematic = true; - + SphereCollider collider = gameObj.AddComponent(); collider.radius = 0.01F; collider.isTrigger = true; @@ -46,11 +38,16 @@ namespace Passer.Control.Unity { } private void OnTriggerEnter(Collider other) { - Debug.Log("Touch!"); - this.coreSensor.touchedThing = other.transform.GetComponentInParent().core; + if (other.isTrigger) + return; + + this.coreSensor.touchedSomething = true; } private void OnTriggerExit(Collider other) { - this.coreSensor.touchedThing = null; + if (other.isTrigger) + return; + + this.coreSensor.touchedSomething = false; } } } From 86ff02a1100cc0f5f4dd2d335716e0f6678caf96 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Tue, 18 Feb 2025 15:05:07 +0100 Subject: [PATCH 04/20] Working ants --- Unity/SiteServer.cs | 5 ++--- Unity/TouchSensor.cs | 5 ++++- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/Unity/SiteServer.cs b/Unity/SiteServer.cs index 6431eb3..f809360 100644 --- a/Unity/SiteServer.cs +++ b/Unity/SiteServer.cs @@ -28,9 +28,8 @@ namespace Passer.Control.Unity { protected virtual void Update() { site.Update((ulong)(Time.time * 1000)); - if (thingQueue.TryDequeue(out Core.Thing thing)) { - thing.CreateComponent(); - } + while (thingQueue.TryDequeue(out Core.Thing thing)) + thing.CreateComponent(); } } diff --git a/Unity/TouchSensor.cs b/Unity/TouchSensor.cs index 4cf41b8..a666f6b 100644 --- a/Unity/TouchSensor.cs +++ b/Unity/TouchSensor.cs @@ -40,7 +40,10 @@ namespace Passer.Control.Unity { 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) { From 5059dfbe6e972bb9fdbf15ef2a5505607a74bda6 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 19 Feb 2025 08:53:07 +0100 Subject: [PATCH 05/20] Fix Unity references --- LinearAlgebra/Angle.cs | 12 +++++--- LinearAlgebra/Matrix.cs | 58 +++++++++++++++++++------------------- LinearAlgebra/Spherical.cs | 24 ++++++++-------- LinearAlgebra/Vector3.cs | 22 +++++++++++---- 4 files changed, 65 insertions(+), 51 deletions(-) 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/Matrix.cs b/LinearAlgebra/Matrix.cs index c2e6334..efa4200 100644 --- a/LinearAlgebra/Matrix.cs +++ b/LinearAlgebra/Matrix.cs @@ -1,11 +1,10 @@ using System; using System.Diagnostics; -using Vector3 = UnityEngine.Vector3; -using Vector2 = UnityEngine.Vector2; +using Passer.LinearAlgebra; public readonly struct Slice { - public int start {get;} - public int stop {get;} + public int start { get; } + public int stop { get; } public Slice(int start, int stop) { this.start = start; this.stop = stop; @@ -50,7 +49,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}, @@ -137,12 +136,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 + ); } @@ -263,14 +262,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; @@ -278,18 +277,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]); } } @@ -461,12 +460,12 @@ public class Matrix { } // Vector-matrix multiplication - public static Vector3 MultiplyMatrixVector3(float[,] A, Vector3 v) { - return new Vector3() { - x = A[0, 0] * v.x + A[0, 1] * v.y + A[0, 2] * v.z, - y = A[1, 0] * v.x + A[1, 1] * v.y + A[1, 2] * v.z, - z = A[2, 0] * v.x + A[2, 1] * v.y + A[2, 2] * v.z - }; + public static Vector3Float MultiplyMatrixVector3(float[,] A, Vector3Float v) { + return new Vector3Float( + A[0, 0] * v.x + A[0, 1] * v.y + A[0, 2] * v.z, + A[1, 0] * v.x + A[1, 1] * v.y + A[1, 2] * v.z, + A[2, 0] * v.x + A[2, 1] * v.y + A[2, 2] * v.z + ); } public static float[,] MultiplyMatrixScalar(float[,] A, float s) { @@ -492,16 +491,17 @@ public class Matrix { return column; } - public static Vector3 GetRow3(float[,] M, int rowIx) { + public static Vector3Float GetRow3(float[,] M, int rowIx) { int cols = M.GetLength(1); - Vector3 row = new(); - row.x = M[rowIx, 0]; - row.y = M[rowIx, 1]; - row.z = M[rowIx, 2]; + Vector3Float row = new( + M[rowIx, 0], + M[rowIx, 1], + M[rowIx, 2] + ); return row; } - public static void SetRow3(float[,] M, int rowIx, Vector3 v) { + public static void SetRow3(float[,] M, int rowIx, Vector3Float v) { M[rowIx, 0] = v.x; M[rowIx, 1] = v.y; M[rowIx, 2] = v.z; diff --git a/LinearAlgebra/Spherical.cs b/LinearAlgebra/Spherical.cs index e9c2b5b..2b86a66 100644 --- a/LinearAlgebra/Spherical.cs +++ b/LinearAlgebra/Spherical.cs @@ -1,5 +1,3 @@ -using UnityEngine; - namespace Passer.LinearAlgebra { public class Spherical { public float distance; @@ -17,30 +15,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/Vector3.cs b/LinearAlgebra/Vector3.cs index c0bc6dd..c7ab61a 100644 --- a/LinearAlgebra/Vector3.cs +++ b/LinearAlgebra/Vector3.cs @@ -1,3 +1,7 @@ +#if UNITY_5_3_OR_NEWER +using Passer.LinearAlgebra.Vector3Float = UnityEngine.Vector3 +#else + namespace Passer.LinearAlgebra { public class Vector3Of { @@ -10,12 +14,20 @@ 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 { - public Vector3Int(int x, int y, int z) : base(x, y, z) { } + public class Vector3Int(int x, int y, int z) : Vector3Of(x, y, z) { } - public class Vector3Float : Vector3Of { - public Vector3Float(float x, float y, float z) : base(x, y, z) { } + public class Vector3Float(float x, float y, float z) : Vector3Of(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 From 0f844f5fad764e47bcd3c5634239e30492405fce Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 19 Feb 2025 10:01:22 +0100 Subject: [PATCH 06/20] Fixed errors and warnings in generic C# --- .editorconfig | 6 +- LinearAlgebra/Matrix.cs | 286 +++++++++++++++++------------------ Messages/CustomMsg.cs | 5 +- Messages/DestroyMsg.cs | 29 ++++ Messages/InvestigateMsg.cs | 46 ++++++ Messages/Messages.cs | 214 -------------------------- Messages/NameMsg.cs | 4 +- Messages/PoseMsg.cs | 109 +++++++++++++ Messages/TextMsg.cs | 22 +++ Participant.cs | 26 ++-- RemoteParticipant.cs | 13 +- Sensors/DistanceSensor.cs | 4 +- Sensors/TemperatureSensor.cs | 5 +- Sensors/TouchSensor.cs | 8 +- SiteServer.cs | 18 ++- Thing.cs | 43 +++--- test/UnitTest1.cs | 2 +- 17 files changed, 416 insertions(+), 424 deletions(-) create mode 100644 Messages/DestroyMsg.cs create mode 100644 Messages/InvestigateMsg.cs create mode 100644 Messages/PoseMsg.cs create mode 100644 Messages/TextMsg.cs 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/LinearAlgebra/Matrix.cs b/LinearAlgebra/Matrix.cs index efa4200..9603048 100644 --- a/LinearAlgebra/Matrix.cs +++ b/LinearAlgebra/Matrix.cs @@ -344,179 +344,179 @@ public class Matrix1 { } } -public class Matrix { - private readonly uint rows = 0; - private readonly uint cols = 0; - private float[] data; +// public class Matrix { +// private readonly uint rows = 0; +// private readonly uint cols = 0; +// private float[] data; - public Matrix(uint rows, uint cols) { - this.rows = rows; - this.cols = cols; - } +// public Matrix(uint rows, uint cols) { +// this.rows = rows; +// this.cols = cols; +// } - public static float[,] Diagonal(float[] v) { - float[,] r = new float[v.Length, v.Length]; - for (int i = 0; i < v.Length; i++) { - r[i, i] = v[i]; - } - return r; - } +// public static float[,] Diagonal(float[] v) { +// float[,] r = new float[v.Length, v.Length]; +// for (int i = 0; i < v.Length; i++) { +// r[i, i] = v[i]; +// } +// return r; +// } - public static float[,] Transpose(float[,] m) { - int rows = m.GetLength(0); - int cols = m.GetLength(1); - float[,] r = new float[cols, rows]; - for (uint rowIx = 0; rowIx < rows; rowIx++) { - for (uint colIx = 0; colIx < cols; colIx++) - r[colIx, rowIx] = m[rowIx, colIx]; - } - return r; - // double checked code - } +// public static float[,] Transpose(float[,] m) { +// int rows = m.GetLength(0); +// int cols = m.GetLength(1); +// float[,] r = new float[cols, rows]; +// for (uint rowIx = 0; rowIx < rows; rowIx++) { +// for (uint colIx = 0; colIx < cols; colIx++) +// r[colIx, rowIx] = m[rowIx, colIx]; +// } +// return r; +// // double checked code +// } - public static void NegateColumn(float[,] m, uint colIx) { - for (uint rowIx = 0; rowIx < m.GetLength(0); rowIx++) { - m[rowIx, colIx] = -m[rowIx, colIx]; - } - } +// public static void NegateColumn(float[,] m, uint colIx) { +// for (uint rowIx = 0; rowIx < m.GetLength(0); rowIx++) { +// m[rowIx, colIx] = -m[rowIx, colIx]; +// } +// } - public static float Determinant(float[,] matrix) { - int n = matrix.GetLength(0); - if (n != matrix.GetLength(1)) - throw new System.ArgumentException("Matrix must be square."); +// public static float Determinant(float[,] matrix) { +// int n = matrix.GetLength(0); +// if (n != matrix.GetLength(1)) +// throw new System.ArgumentException("Matrix must be square."); - if (n == 1) - return matrix[0, 0]; // Base case for 1x1 matrix +// if (n == 1) +// return matrix[0, 0]; // Base case for 1x1 matrix - if (n == 2) // Base case for 2x2 matrix - return matrix[0, 0] * matrix[1, 1] - matrix[0, 1] * matrix[1, 0]; +// if (n == 2) // Base case for 2x2 matrix +// return matrix[0, 0] * matrix[1, 1] - matrix[0, 1] * matrix[1, 0]; - float det = 0; - for (int col = 0; col < n; col++) - det += (col % 2 == 0 ? 1 : -1) * matrix[0, col] * Determinant(Minor(matrix, 0, col)); +// float det = 0; +// for (int col = 0; col < n; col++) +// det += (col % 2 == 0 ? 1 : -1) * matrix[0, col] * Determinant(Minor(matrix, 0, col)); - return det; - } +// return det; +// } - // Helper function to compute the minor of a matrix - private static float[,] Minor(float[,] matrix, int rowToRemove, int colToRemove) { - int n = matrix.GetLength(0); - float[,] minor = new float[n - 1, n - 1]; +// // Helper function to compute the minor of a matrix +// private static float[,] Minor(float[,] matrix, int rowToRemove, int colToRemove) { +// int n = matrix.GetLength(0); +// float[,] minor = new float[n - 1, n - 1]; - int r = 0, c = 0; - for (int i = 0; i < n; i++) { - if (i == rowToRemove) continue; +// int r = 0, c = 0; +// for (int i = 0; i < n; i++) { +// if (i == rowToRemove) continue; - c = 0; - for (int j = 0; j < n; j++) { - if (j == colToRemove) continue; +// c = 0; +// for (int j = 0; j < n; j++) { +// if (j == colToRemove) continue; - minor[r, c] = matrix[i, j]; - c++; - } - r++; - } +// minor[r, c] = matrix[i, j]; +// c++; +// } +// r++; +// } - return minor; - } +// return minor; +// } - public static float[,] MultiplyMatrices(float[,] A, float[,] B) { - int rowsA = A.GetLength(0); - int colsA = A.GetLength(1); - int rowsB = B.GetLength(0); - int colsB = B.GetLength(1); +// public static float[,] MultiplyMatrices(float[,] A, float[,] B) { +// int rowsA = A.GetLength(0); +// int colsA = A.GetLength(1); +// int rowsB = B.GetLength(0); +// int colsB = B.GetLength(1); - if (colsA != rowsB) - throw new System.ArgumentException("Number of columns in A must match number of rows in B."); +// if (colsA != rowsB) +// throw new System.ArgumentException("Number of columns in A must match number of rows in B."); - float[,] result = new float[rowsA, colsB]; +// float[,] result = new float[rowsA, colsB]; - for (int i = 0; i < rowsA; i++) { - for (int j = 0; j < colsB; j++) { - float sum = 0.0f; - for (int k = 0; k < colsA; k++) - sum += A[i, k] * B[k, j]; +// for (int i = 0; i < rowsA; i++) { +// for (int j = 0; j < colsB; j++) { +// float sum = 0.0f; +// for (int k = 0; k < colsA; k++) +// sum += A[i, k] * B[k, j]; - result[i, j] = sum; - } - } +// result[i, j] = sum; +// } +// } - return result; - // double checked code - } +// return result; +// // double checked code +// } - public static float[] MultiplyMatrixVector(float[,] A, float[] v) { - int rows = A.GetLength(0); - int cols = A.GetLength(1); - float[] result = new float[rows]; +// public static float[] MultiplyMatrixVector(float[,] A, float[] v) { +// int rows = A.GetLength(0); +// int cols = A.GetLength(1); +// float[] result = new float[rows]; - for (int i = 0; i < rows; i++) { - for (int j = 0; j < cols; j++) { - result[i] += A[i, j] * v[j]; - } - } +// for (int i = 0; i < rows; i++) { +// for (int j = 0; j < cols; j++) { +// result[i] += A[i, j] * v[j]; +// } +// } - return result; - } +// return result; +// } - // Vector-matrix multiplication - public static Vector3Float MultiplyMatrixVector3(float[,] A, Vector3Float v) { - return new Vector3Float( - A[0, 0] * v.x + A[0, 1] * v.y + A[0, 2] * v.z, - A[1, 0] * v.x + A[1, 1] * v.y + A[1, 2] * v.z, - A[2, 0] * v.x + A[2, 1] * v.y + A[2, 2] * v.z - ); - } +// // Vector-matrix multiplication +// public static Vector3Float MultiplyMatrixVector3(float[,] A, Vector3Float v) { +// return new Vector3Float( +// A[0, 0] * v.x + A[0, 1] * v.y + A[0, 2] * v.z, +// A[1, 0] * v.x + A[1, 1] * v.y + A[1, 2] * v.z, +// A[2, 0] * v.x + A[2, 1] * v.y + A[2, 2] * v.z +// ); +// } - public static float[,] MultiplyMatrixScalar(float[,] A, float s) { - int rows = A.GetLength(0); - int cols = A.GetLength(1); - float[,] result = new float[rows, cols]; +// public static float[,] MultiplyMatrixScalar(float[,] A, float s) { +// int rows = A.GetLength(0); +// int cols = A.GetLength(1); +// float[,] result = new float[rows, cols]; - for (int i = 0; i < rows; i++) { - for (int j = 0; j < cols; j++) { - result[i, j] += A[i, j] * s; - } - } +// for (int i = 0; i < rows; i++) { +// for (int j = 0; j < cols; j++) { +// result[i, j] += A[i, j] * s; +// } +// } - return result; - } +// return result; +// } - public static float[] GetColumn(float[,] M, int col) { - int rows = M.GetLength(0); - float[] column = new float[rows]; - for (int i = 0; i < rows; i++) { - column[i] = M[i, col]; - } - return column; - } +// public static float[] GetColumn(float[,] M, int col) { +// int rows = M.GetLength(0); +// float[] column = new float[rows]; +// for (int i = 0; i < rows; i++) { +// column[i] = M[i, col]; +// } +// return column; +// } - public static Vector3Float GetRow3(float[,] M, int rowIx) { - int cols = M.GetLength(1); - Vector3Float row = new( - M[rowIx, 0], - M[rowIx, 1], - M[rowIx, 2] - ); - return row; - } +// public static Vector3Float GetRow3(float[,] M, int rowIx) { +// int cols = M.GetLength(1); +// Vector3Float row = new( +// M[rowIx, 0], +// M[rowIx, 1], +// M[rowIx, 2] +// ); +// return row; +// } - public static void SetRow3(float[,] M, int rowIx, Vector3Float v) { - M[rowIx, 0] = v.x; - M[rowIx, 1] = v.y; - M[rowIx, 2] = v.z; - } +// public static void SetRow3(float[,] M, int rowIx, Vector3Float v) { +// M[rowIx, 0] = v.x; +// M[rowIx, 1] = v.y; +// M[rowIx, 2] = v.z; +// } - public static float Dot(float[] a, float[] b) { - float sum = 0; - for (int i = 0; i < a.Length; i++) sum += a[i] * b[i]; - return sum; - } +// public static float Dot(float[] a, float[] b) { +// float sum = 0; +// for (int i = 0; i < a.Length; i++) sum += a[i] * b[i]; +// return sum; +// } - public static float[,] IdentityMatrix(int size) { - float[,] I = new float[size, size]; - for (int i = 0; i < size; i++) I[i, i] = 1.0f; - return I; - } +// public static float[,] IdentityMatrix(int size) { +// float[,] I = new float[size, size]; +// for (int i = 0; i < size; i++) I[i, i] = 1.0f; +// return I; +// } -} \ No newline at end of file +// } \ No newline at end of file diff --git a/Messages/CustomMsg.cs b/Messages/CustomMsg.cs index 75e4311..e3c7acb 100644 --- a/Messages/CustomMsg.cs +++ b/Messages/CustomMsg.cs @@ -1,3 +1,4 @@ +#nullable enable namespace Passer.Control.Core { public class CustomMsg : IMessage { @@ -23,11 +24,13 @@ namespace Passer.Control.Core { public CustomMsg(byte networkId, Thing thing) : base() { this.networkId = networkId; this.thingId = thing.id; + this.bytes = []; } public override byte Serialize(ref byte[] buffer) { - if (bytes == null) + if (bytes.Length == 0) return 0; + byte ix = 0; buffer[ix++] = CustomMsg.Id; buffer[ix++] = this.networkId; diff --git a/Messages/DestroyMsg.cs b/Messages/DestroyMsg.cs new file mode 100644 index 0000000..1701a53 --- /dev/null +++ b/Messages/DestroyMsg.cs @@ -0,0 +1,29 @@ + +namespace Passer.Control.Core { + + 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; + } + } + +} \ No newline at end of file diff --git a/Messages/InvestigateMsg.cs b/Messages/InvestigateMsg.cs new file mode 100644 index 0000000..7c6af47 --- /dev/null +++ b/Messages/InvestigateMsg.cs @@ -0,0 +1,46 @@ +namespace Passer.Control.Core { + + 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; + + } + } + +} \ No newline at end of file diff --git a/Messages/Messages.cs b/Messages/Messages.cs index 83ba4e5..c07368e 100644 --- a/Messages/Messages.cs +++ b/Messages/Messages.cs @@ -1,7 +1,3 @@ -using System.IO; -using System.Threading.Tasks; - -using Passer.LinearAlgebra; namespace Passer.Control.Core { public class IMessage { @@ -40,214 +36,4 @@ namespace Passer.Control.Core { } } - #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/Messages/NameMsg.cs b/Messages/NameMsg.cs index 3d99f18..cb709d2 100644 --- a/Messages/NameMsg.cs +++ b/Messages/NameMsg.cs @@ -8,7 +8,7 @@ namespace Passer.Control.Core { public byte networkId; public byte thingId; public byte len; - public string? name = null; + public string name = ""; public NameMsg(byte networkId, Thing thing) { this.networkId = networkId; @@ -29,7 +29,7 @@ namespace Passer.Control.Core { } public override byte Serialize(ref byte[] buffer) { - if (this.name == null) + if (this.name.Length == 0) return 0; byte ix = 0; diff --git a/Messages/PoseMsg.cs b/Messages/PoseMsg.cs new file mode 100644 index 0000000..9ea0748 --- /dev/null +++ b/Messages/PoseMsg.cs @@ -0,0 +1,109 @@ +using Passer.LinearAlgebra; + +namespace Passer.Control.Core { + + 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 = Spherical.zero; + public SwingTwist orientation = SwingTwist.zero; + public Spherical linearVelocity = Spherical.zero; + public Spherical angularVelocity = Spherical.zero; + + 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; + if (this.orientation != null) + this.poseType |= Pose_Orientation; + } + 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); + } + } + +} \ No newline at end of file diff --git a/Messages/TextMsg.cs b/Messages/TextMsg.cs new file mode 100644 index 0000000..10f14c6 --- /dev/null +++ b/Messages/TextMsg.cs @@ -0,0 +1,22 @@ +namespace Passer.Control.Core { + + public class TextMsg(byte[] buffer) : IMessage(buffer) { + public const byte Id = 0xB0; + public string text = ""; + + 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; + } + } + +} \ No newline at end of file diff --git a/Participant.cs b/Participant.cs index d95d39c..2e19888 100644 --- a/Participant.cs +++ b/Participant.cs @@ -1,3 +1,4 @@ +#nullable enable using System; using System.Collections.Generic; using System.Collections.Concurrent; @@ -13,8 +14,8 @@ namespace Passer.Control.Core { //public byte networkId = 0; public string name = "Participant"; - public IPEndPoint endPoint = null; - public UdpClient udpClient = null; + public IPEndPoint? endPoint = null; + public UdpClient? udpClient = null; public string broadcastIpAddress = "255.255.255.255"; public readonly ConcurrentQueue messageQueue = new(); @@ -64,7 +65,7 @@ namespace Passer.Control.Core { public List senders = new(); - public RemoteParticipant GetParticipant(string ipAddress, int port) { + public RemoteParticipant? GetParticipant(string ipAddress, int port) { //Console.WriteLine($"Get Participant {ipAddress}:{port}"); foreach (RemoteParticipant sender in senders) { if (sender.ipAddress == ipAddress && sender.port == port) @@ -81,7 +82,7 @@ namespace Passer.Control.Core { return participant; } - protected readonly Dictionary> thingMsgProcessors = new(); + protected readonly Dictionary> thingMsgProcessors = new(); public delegate Thing ThingConstructor(byte networkId, byte thingId); public void Register(byte thingType, ThingConstructor constr) { @@ -93,7 +94,7 @@ namespace Passer.Control.Core { } public void Register(byte thingType) where ThingClass : Thing { - thingMsgProcessors[thingType] = (byte networkId, byte thingId) => + thingMsgProcessors[thingType] = static (byte networkId, byte thingId) => Activator.CreateInstance(typeof(ThingClass), networkId, thingId) as ThingClass; Console.WriteLine($"Registering {typeof(ThingClass)} for thing type {thingType}"); } @@ -103,17 +104,20 @@ 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); @@ -285,7 +289,7 @@ namespace Passer.Control.Core { protected virtual void Process(RemoteParticipant sender, NameMsg msg) { // Console.WriteLine($"Participant: Process name [{msg.networkId}/{msg.thingId}] {msg.name}"); - Thing thing = sender.Get(msg.networkId, msg.thingId); + Thing? thing = sender.Get(msg.networkId, msg.thingId); if (thing != null) thing.name = msg.name; } @@ -298,7 +302,7 @@ namespace Passer.Control.Core { protected virtual void Process(RemoteParticipant sender, CustomMsg msg) { // Console.WriteLine($"Participant: Process binary [{msg.networkId}/{msg.thingId}]"); - Thing thing = sender.Get(msg.networkId, msg.thingId); + Thing? thing = sender.Get(msg.networkId, msg.thingId); thing?.ProcessBinary(msg.bytes); } diff --git a/RemoteParticipant.cs b/RemoteParticipant.cs index 67d8b13..7dce8da 100644 --- a/RemoteParticipant.cs +++ b/RemoteParticipant.cs @@ -17,18 +17,19 @@ namespace Passer.Control.Core { this.port = port; } - protected readonly List things = new(); + protected readonly List things = []; - public Thing Get(byte networkId, byte thingId) { - Thing thing = things.Find(aThing => Thing.IsThing(aThing, networkId, thingId)); - // if (thing == null) - // Console.WriteLine($"Could not find thing {ipAddress}:{port}[{networkId}/{thingId}]"); + public Thing? Get(byte networkId, byte thingId) { + Thing? thing = things.Find(aThing => Thing.IsThing(aThing, networkId, thingId)); return thing; } + // if (thing == null) + // Console.WriteLine($"Could not find thing {ipAddress}:{port}[{networkId}/{thingId}]"); + public void Add(Thing thing, bool invokeEvent = true) { // Console.WriteLine($"added thing [{thing.networkId}/{thing.id}]"); - Thing foundThing = Get(thing.networkId, thing.id); + Thing? foundThing = Get(thing.networkId, thing.id); if (foundThing == null) { things.Add(thing); diff --git a/Sensors/DistanceSensor.cs b/Sensors/DistanceSensor.cs index 0845d21..b88387b 100644 --- a/Sensors/DistanceSensor.cs +++ b/Sensors/DistanceSensor.cs @@ -5,9 +5,9 @@ namespace Passer.Control.Core { public class DistanceSensor : Thing { public float distance = 0; - public DistanceSensor() : base(true) { } + public DistanceSensor(RemoteParticipant participant) : base(participant, true) { } - public DistanceSensor(byte networkId, byte thingId) : base(null, networkId, thingId, (byte)Type.TemperatureSensor) { + public DistanceSensor(RemoteParticipant participant, byte networkId, byte thingId) : base(participant, networkId, thingId, (byte)Type.TemperatureSensor) { } #if UNITY_5_3_OR_NEWER diff --git a/Sensors/TemperatureSensor.cs b/Sensors/TemperatureSensor.cs index b6e61b5..4617d47 100644 --- a/Sensors/TemperatureSensor.cs +++ b/Sensors/TemperatureSensor.cs @@ -2,11 +2,8 @@ using System; namespace Passer.Control.Core { - public class TemperatureSensor : Thing { + public class TemperatureSensor(Participant participant, byte networkId, byte thingId) : Thing(participant, networkId, thingId, (byte)Type.TemperatureSensor) { public float temp = 0; - - public TemperatureSensor(byte networkId, byte thingId) : base(null, networkId, thingId, (byte)Type.TemperatureSensor) { - } public override void ProcessBinary(byte[] bytes) { byte ix = 0; diff --git a/Sensors/TouchSensor.cs b/Sensors/TouchSensor.cs index 1553ee8..8c53470 100644 --- a/Sensors/TouchSensor.cs +++ b/Sensors/TouchSensor.cs @@ -1,12 +1,8 @@ - namespace Passer.Control.Core { - public class TouchSensor : Thing { - //public Thing touchedThing = null; + + public class TouchSensor(RemoteParticipant participant, bool invokeEvent = true) : Thing(participant, invokeEvent) { public bool touchedSomething = false; - public TouchSensor(bool invokeEvent = true) : base(invokeEvent) { - } - #if UNITY_5_3_OR_NEWER public override void CreateComponent() { this.component = Unity.TouchSensor.Create(this); diff --git a/SiteServer.cs b/SiteServer.cs index f44916b..0fbc4a4 100644 --- a/SiteServer.cs +++ b/SiteServer.cs @@ -23,10 +23,10 @@ namespace Passer.Control.Core { } 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) { @@ -40,13 +40,15 @@ namespace Passer.Control.Core { protected override void Process(RemoteParticipant sender, ThingMsg msg) { //Console.WriteLine($"SiteServer: Process thing [{msg.networkId}/{msg.thingId}]"); - Thing thing = sender.Get(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 - newThing = new Thing(sender, msg.networkId, msg.thingId, msg.thingType); + Thing? newThing = null; + if (thingMsgProcessors.TryGetValue(msg.thingType, out Func? value)) { + if (value != null) + newThing = value(msg.networkId, msg.thingId); + } + newThing ??= new Thing(sender, msg.networkId, msg.thingId, msg.thingType); + sender.Add(newThing); } } diff --git a/Thing.cs b/Thing.cs index c78851a..34e61a5 100644 --- a/Thing.cs +++ b/Thing.cs @@ -41,16 +41,16 @@ namespace Passer.Control.Core { public byte networkId; public byte id; - public event ChangeHandler OnParentChanged; - private Thing _parent; - public Thing parent { + public event ChangeHandler OnParentChanged = delegate {}; + private Thing? _parent; + public Thing? parent { get => _parent; set { if (_parent == value) return; if (value == null) { - _parent.RemoveChild(this); + _parent?.RemoveChild(this); _parent = null; } else { @@ -74,8 +74,8 @@ namespace Passer.Control.Core { public List children = new List(); public byte type; - public event ChangeHandler OnNameChanged; - private string _name; + public event ChangeHandler OnNameChanged = delegate {}; + private string _name = ""; public virtual string name { get => _name; set { @@ -86,11 +86,11 @@ namespace Passer.Control.Core { } } - public string modelUrl; + public string modelUrl = ""; public byte poseUpdated = 0x00; - public event ChangeHandler OnPositionChanged; - private Spherical _position; + public event ChangeHandler OnPositionChanged = delegate {}; + private Spherical _position = Spherical.zero; public Spherical position { get { return _position; } set { @@ -101,8 +101,8 @@ namespace Passer.Control.Core { } } - public event ChangeHandler OnOrientationChanged; - private SwingTwist _orientation; + public event ChangeHandler OnOrientationChanged = delegate {}; + private SwingTwist _orientation = SwingTwist.zero; public SwingTwist orientation { get { return _orientation; } set { @@ -113,8 +113,8 @@ namespace Passer.Control.Core { } } - public event SphericalHandler OnLinearVelocityChanged; - private Spherical _linearVelocity; + public event SphericalHandler OnLinearVelocityChanged = delegate {}; + private Spherical _linearVelocity = Spherical.zero; public Spherical linearVelocity { get => _linearVelocity; set { @@ -124,7 +124,7 @@ namespace Passer.Control.Core { } } } - public Spherical angularVelocity; + public Spherical angularVelocity = Spherical.zero; #if UNITY_5_3_OR_NEWER [NonSerialized] @@ -135,12 +135,8 @@ namespace Passer.Control.Core { #region Init - // public virtual void Init(bool invokeEvent = false) { - // if (invokeEvent) - // InvokeNewThing(this); - // } - - public Thing(bool invokeEvent = false) { + public Thing(RemoteParticipant participant, bool invokeEvent = false) { + this.participant = participant; if (invokeEvent) InvokeNewThing(this); } @@ -149,9 +145,6 @@ namespace Passer.Control.Core { this.id = thingId; this.type = thingType; this.networkId = networkId; - //this.Init(); - //OnNewThing?.Invoke(this); - //Thing.Add(this); } public virtual void CreateComponent() {} @@ -195,10 +188,10 @@ namespace Passer.Control.Core { //---------- All Things - private static readonly List allThings = new(); + private static readonly List allThings = []; public delegate void ThingHandler(Thing t); - public static event ThingHandler OnNewThing; + public static event ThingHandler OnNewThing = delegate {}; public static void InvokeNewThing(Thing thing) { OnNewThing?.Invoke(thing); } diff --git a/test/UnitTest1.cs b/test/UnitTest1.cs index 00fd906..51c6baa 100644 --- a/test/UnitTest1.cs +++ b/test/UnitTest1.cs @@ -65,7 +65,7 @@ namespace ControlCore.test { public void Test_ThingMsg() { SiteServer siteServer = new(); Participant participant = new("127.0.0.1"); - Thing thing = new() { + Thing thing = new(participant) { name = "First Thing", modelUrl = "https://passer.life/extras/ant.jpg" }; From 4044b86a9d0f76c48106324b0a2a7f3c924a5caa Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 19 Feb 2025 11:04:56 +0100 Subject: [PATCH 07/20] Unity Compatibility --- LinearAlgebra/Matrix.cs | 3 +++ LinearAlgebra/Spherical.cs | 5 +++++ LinearAlgebra/Vector3.cs | 5 +---- Messages/CustomMsg.cs | 2 +- Messages/DestroyMsg.cs | 2 ++ Messages/InvestigateMsg.cs | 3 +++ Messages/Messages.cs | 3 +++ Messages/PoseMsg.cs | 2 ++ Messages/TextMsg.cs | 6 +++++- RemoteParticipant.cs | 3 ++- Sensors/TemperatureSensor.cs | 4 +++- SiteServer.cs | 1 + Thing.cs | 5 +++-- Unity/DistanceSensor.cs | 6 ++++-- Unity/TouchSensor.cs | 6 ++++-- 15 files changed, 42 insertions(+), 14 deletions(-) diff --git a/LinearAlgebra/Matrix.cs b/LinearAlgebra/Matrix.cs index 9603048..95978be 100644 --- a/LinearAlgebra/Matrix.cs +++ b/LinearAlgebra/Matrix.cs @@ -1,6 +1,9 @@ using System; using System.Diagnostics; using Passer.LinearAlgebra; +#if UNITY_5_3_OR_NEWER +using Vector3Float = UnityEngine.Vector3; +#endif public readonly struct Slice { public int start { get; } diff --git a/LinearAlgebra/Spherical.cs b/LinearAlgebra/Spherical.cs index 2b86a66..5670c6b 100644 --- a/LinearAlgebra/Spherical.cs +++ b/LinearAlgebra/Spherical.cs @@ -1,3 +1,8 @@ +using System; +#if UNITY_5_3_OR_NEWER +using Vector3Float = UnityEngine.Vector3; +#endif + namespace Passer.LinearAlgebra { public class Spherical { public float distance; diff --git a/LinearAlgebra/Vector3.cs b/LinearAlgebra/Vector3.cs index c7ab61a..4b629ac 100644 --- a/LinearAlgebra/Vector3.cs +++ b/LinearAlgebra/Vector3.cs @@ -1,9 +1,6 @@ -#if UNITY_5_3_OR_NEWER -using Passer.LinearAlgebra.Vector3Float = UnityEngine.Vector3 -#else +#if !UNITY_5_3_OR_NEWER namespace Passer.LinearAlgebra { - public class Vector3Of { public T x; public T y; diff --git a/Messages/CustomMsg.cs b/Messages/CustomMsg.cs index e3c7acb..d089b73 100644 --- a/Messages/CustomMsg.cs +++ b/Messages/CustomMsg.cs @@ -24,7 +24,7 @@ namespace Passer.Control.Core { public CustomMsg(byte networkId, Thing thing) : base() { this.networkId = networkId; this.thingId = thing.id; - this.bytes = []; + this.bytes = new byte[0]; } public override byte Serialize(ref byte[] buffer) { diff --git a/Messages/DestroyMsg.cs b/Messages/DestroyMsg.cs index 1701a53..1298113 100644 --- a/Messages/DestroyMsg.cs +++ b/Messages/DestroyMsg.cs @@ -1,3 +1,5 @@ +using System.IO; +using System.Threading.Tasks; namespace Passer.Control.Core { diff --git a/Messages/InvestigateMsg.cs b/Messages/InvestigateMsg.cs index 7c6af47..39d4c2b 100644 --- a/Messages/InvestigateMsg.cs +++ b/Messages/InvestigateMsg.cs @@ -1,3 +1,6 @@ +using System.IO; +using System.Threading.Tasks; + namespace Passer.Control.Core { public class InvestigateMsg : IMessage { diff --git a/Messages/Messages.cs b/Messages/Messages.cs index c07368e..fe160cf 100644 --- a/Messages/Messages.cs +++ b/Messages/Messages.cs @@ -1,3 +1,6 @@ +using System.Threading.Tasks; +using System.IO; + namespace Passer.Control.Core { public class IMessage { diff --git a/Messages/PoseMsg.cs b/Messages/PoseMsg.cs index 9ea0748..6cdcb0c 100644 --- a/Messages/PoseMsg.cs +++ b/Messages/PoseMsg.cs @@ -1,3 +1,5 @@ +using System.IO; +using System.Threading.Tasks; using Passer.LinearAlgebra; namespace Passer.Control.Core { diff --git a/Messages/TextMsg.cs b/Messages/TextMsg.cs index 10f14c6..b28ac57 100644 --- a/Messages/TextMsg.cs +++ b/Messages/TextMsg.cs @@ -1,6 +1,10 @@ +using System.IO; +using System.Threading.Tasks; + namespace Passer.Control.Core { - public class TextMsg(byte[] buffer) : IMessage(buffer) { + public class TextMsg : IMessage { + public TextMsg(byte[] buffer) : base(buffer) {} public const byte Id = 0xB0; public string text = ""; diff --git a/RemoteParticipant.cs b/RemoteParticipant.cs index 7dce8da..8d8341f 100644 --- a/RemoteParticipant.cs +++ b/RemoteParticipant.cs @@ -1,3 +1,4 @@ +#nullable enable using System; using System.Collections.Generic; @@ -17,7 +18,7 @@ namespace Passer.Control.Core { this.port = port; } - protected readonly List things = []; + protected readonly List things = new(); public Thing? Get(byte networkId, byte thingId) { Thing? thing = things.Find(aThing => Thing.IsThing(aThing, networkId, thingId)); diff --git a/Sensors/TemperatureSensor.cs b/Sensors/TemperatureSensor.cs index 4617d47..bb8ed1e 100644 --- a/Sensors/TemperatureSensor.cs +++ b/Sensors/TemperatureSensor.cs @@ -2,9 +2,11 @@ using System; namespace Passer.Control.Core { - public class TemperatureSensor(Participant participant, byte networkId, byte thingId) : Thing(participant, networkId, thingId, (byte)Type.TemperatureSensor) { + public class TemperatureSensor : Thing { public float temp = 0; + public TemperatureSensor(Participant participant, byte networkId, byte thingId) : base(participant, networkId, thingId, (byte)Type.TemperatureSensor) {} + public override void ProcessBinary(byte[] bytes) { byte ix = 0; this.temp = LowLevelMessages.ReceiveFloat16(bytes, ref ix); diff --git a/SiteServer.cs b/SiteServer.cs index 0fbc4a4..8c80049 100644 --- a/SiteServer.cs +++ b/SiteServer.cs @@ -1,3 +1,4 @@ +#nullable enable using System; using System.Collections.Generic; using System.Net; diff --git a/Thing.cs b/Thing.cs index 34e61a5..3570382 100644 --- a/Thing.cs +++ b/Thing.cs @@ -1,3 +1,4 @@ +#nullable enable using System; using System.Collections.Generic; using Passer.LinearAlgebra; @@ -128,7 +129,7 @@ namespace Passer.Control.Core { #if UNITY_5_3_OR_NEWER [NonSerialized] - public Unity.Thing component; + public Unity.Thing? component = null; #endif #endregion Properties @@ -188,7 +189,7 @@ namespace Passer.Control.Core { //---------- All Things - private static readonly List allThings = []; + // private static readonly List allThings = new(); public delegate void ThingHandler(Thing t); public static event ThingHandler OnNewThing = delegate {}; diff --git a/Unity/DistanceSensor.cs b/Unity/DistanceSensor.cs index ac74b78..b28340d 100644 --- a/Unity/DistanceSensor.cs +++ b/Unity/DistanceSensor.cs @@ -12,8 +12,10 @@ namespace Passer.Control.Unity { } protected virtual void Start() { - if (core == null) - SetCoreThing(new Core.DistanceSensor()); + if (core == null) { + SiteServer siteServer = FindAnyObjectByType(); + SetCoreThing(new Core.DistanceSensor(siteServer.site)); + } StartCoroutine(MeasureDistance()); } diff --git a/Unity/TouchSensor.cs b/Unity/TouchSensor.cs index a666f6b..fe31b1b 100644 --- a/Unity/TouchSensor.cs +++ b/Unity/TouchSensor.cs @@ -10,8 +10,10 @@ namespace Passer.Control.Unity { } protected virtual void Start() { - if (core == null) - SetCoreThing(new Core.TouchSensor()); + if (core == null) { + SiteServer siteServer = FindAnyObjectByType(); + SetCoreThing(new Core.TouchSensor(siteServer.site)); + } } From 0a75c7f4fd6c30027531723fbf1400a3f4f988d8 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 19 Feb 2025 11:21:12 +0100 Subject: [PATCH 08/20] Downgraded to C# 9 --- ControlCore.csproj | 3 +-- LinearAlgebra/Vector3.cs | 9 ++++++--- test/test.csproj | 8 +------- 3 files changed, 8 insertions(+), 12 deletions(-) diff --git a/ControlCore.csproj b/ControlCore.csproj index 9f3c622..537dba9 100644 --- a/ControlCore.csproj +++ b/ControlCore.csproj @@ -3,8 +3,7 @@ false false - net9.0 - enable + net5.0 enable diff --git a/LinearAlgebra/Vector3.cs b/LinearAlgebra/Vector3.cs index 4b629ac..0cc4d7b 100644 --- a/LinearAlgebra/Vector3.cs +++ b/LinearAlgebra/Vector3.cs @@ -1,5 +1,6 @@ - #if !UNITY_5_3_OR_NEWER +using System; + namespace Passer.LinearAlgebra { public class Vector3Of { public T x; @@ -17,9 +18,11 @@ namespace Passer.LinearAlgebra { // } } - public class Vector3Int(int x, int y, int z) : Vector3Of(x, y, z) { + public class Vector3Int : Vector3Of { + public Vector3Int(int x, int y, int z) : base(x, y, z) { } } - public class Vector3Float(float x, float y, float z) : Vector3Of(x, y, z) { + 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); diff --git a/test/test.csproj b/test/test.csproj index b0f97b9..6fe6bad 100644 --- a/test/test.csproj +++ b/test/test.csproj @@ -1,9 +1,7 @@  - net9.0 - latest - enable + net5.0 enable false true @@ -17,10 +15,6 @@ - - - - From dcccef221b999eb9cabe377b803fa8d92536ef63 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 19 Feb 2025 12:23:55 +0100 Subject: [PATCH 09/20] Make the unit tests work again (2 are still failing) --- ControlCore.csproj | 8 ++---- LinearAlgebra/Direction.cs | 12 ++++---- LinearAlgebra/Quat32.cs | 2 +- LinearAlgebra/Spherical.cs | 4 +-- LinearAlgebra/SwingTwist.cs | 4 +-- Messages/CustomMsg.cs | 1 - Messages/DestroyMsg.cs | 2 +- Messages/InvestigateMsg.cs | 2 +- Messages/LowLevelMessages.cs | 8 +++--- Messages/NameMsg.cs | 2 -- Messages/PoseMsg.cs | 4 +-- Messages/TextMsg.cs | 2 +- ModelUrlMsg.cs | 4 +-- Participant.cs | 35 ++++++++++++----------- RemoteParticipant.cs | 9 +++--- SiteServer.cs | 12 ++++---- Thing.cs | 5 ++-- float16.cs | 55 ++++++++++++++++-------------------- test/UnitTest1.cs | 14 ++++----- test/test.csproj | 9 ++---- 20 files changed, 87 insertions(+), 107 deletions(-) diff --git a/ControlCore.csproj b/ControlCore.csproj index 537dba9..feb0b62 100644 --- a/ControlCore.csproj +++ b/ControlCore.csproj @@ -4,15 +4,11 @@ false false net5.0 - enable - - - - - + + 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/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 5670c6b..be0c38a 100644 --- a/LinearAlgebra/Spherical.cs +++ b/LinearAlgebra/Spherical.cs @@ -8,8 +8,8 @@ namespace Passer.LinearAlgebra { 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; 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/Messages/CustomMsg.cs b/Messages/CustomMsg.cs index d089b73..cf06fff 100644 --- a/Messages/CustomMsg.cs +++ b/Messages/CustomMsg.cs @@ -1,4 +1,3 @@ -#nullable enable namespace Passer.Control.Core { public class CustomMsg : IMessage { diff --git a/Messages/DestroyMsg.cs b/Messages/DestroyMsg.cs index 1298113..9a07eda 100644 --- a/Messages/DestroyMsg.cs +++ b/Messages/DestroyMsg.cs @@ -21,7 +21,7 @@ namespace Passer.Control.Core { return false; byte[] buffer = await Receive(dataStream, packetSize); - DestroyMsg msg = new(buffer); + DestroyMsg msg = new DestroyMsg(buffer); client.messageQueue.Enqueue(msg); return true; diff --git a/Messages/InvestigateMsg.cs b/Messages/InvestigateMsg.cs index 39d4c2b..b6f9765 100644 --- a/Messages/InvestigateMsg.cs +++ b/Messages/InvestigateMsg.cs @@ -37,7 +37,7 @@ namespace Passer.Control.Core { return false; byte[] buffer = await Receive(dataStream, packetSize); - InvestigateMsg msg = new(buffer); + InvestigateMsg msg = new InvestigateMsg(buffer); //UnityEngine.Debug.Log($"Receive investigate [{msg.networkId}/{msg.thingId}]"); client.messageQueue.Enqueue(msg); diff --git a/Messages/LowLevelMessages.cs b/Messages/LowLevelMessages.cs index 055096b..591a25d 100644 --- a/Messages/LowLevelMessages.cs +++ b/Messages/LowLevelMessages.cs @@ -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/NameMsg.cs b/Messages/NameMsg.cs index cb709d2..70c8d9d 100644 --- a/Messages/NameMsg.cs +++ b/Messages/NameMsg.cs @@ -1,5 +1,3 @@ -#nullable enable - namespace Passer.Control.Core { public class NameMsg : IMessage { diff --git a/Messages/PoseMsg.cs b/Messages/PoseMsg.cs index 6cdcb0c..14c0a34 100644 --- a/Messages/PoseMsg.cs +++ b/Messages/PoseMsg.cs @@ -70,12 +70,12 @@ namespace Passer.Control.Core { } public static bool Send(Participant client, byte thingId, Spherical position, SwingTwist orientation) { - PoseMsg msg = new(client.networkId, thingId, position, orientation); + PoseMsg msg = new PoseMsg(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); + PoseMsg msg = new PoseMsg(buffer); // Do no process poses with nwid == 0 (== local) if (msg.networkId == 0) diff --git a/Messages/TextMsg.cs b/Messages/TextMsg.cs index b28ac57..3959116 100644 --- a/Messages/TextMsg.cs +++ b/Messages/TextMsg.cs @@ -16,7 +16,7 @@ namespace Passer.Control.Core { public static async Task Receive(Stream dataStream, Participant client, byte packetSize) { byte[] buffer = await Receive(dataStream, packetSize); - TextMsg msg = new(buffer); + TextMsg msg = new TextMsg(buffer); client.messageQueue.Enqueue(msg); return true; diff --git a/ModelUrlMsg.cs b/ModelUrlMsg.cs index a3c02ba..b1c712c 100644 --- a/ModelUrlMsg.cs +++ b/ModelUrlMsg.cs @@ -1,5 +1,3 @@ -#nullable enable - namespace Passer.Control.Core { public class ModelUrlMsg : IMessage { @@ -7,7 +5,7 @@ namespace Passer.Control.Core { public const byte length = 4; public byte networkId; public byte thingId; - public string? url = null; + public string url = null; public ModelUrlMsg(byte networkId, Thing thing) { this.networkId = networkId; diff --git a/Participant.cs b/Participant.cs index 2e19888..a8a650d 100644 --- a/Participant.cs +++ b/Participant.cs @@ -1,4 +1,3 @@ -#nullable enable using System; using System.Collections.Generic; using System.Collections.Concurrent; @@ -14,11 +13,11 @@ namespace Passer.Control.Core { //public byte networkId = 0; public string name = "Participant"; - public IPEndPoint? endPoint = null; - public UdpClient? udpClient = null; + 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 @@ -46,10 +45,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); } @@ -63,9 +64,9 @@ namespace Passer.Control.Core { this.port = port; } - public List senders = new(); + public List senders = new List(); - public RemoteParticipant? GetParticipant(string ipAddress, int port) { + public RemoteParticipant GetParticipant(string ipAddress, int port) { //Console.WriteLine($"Get Participant {ipAddress}:{port}"); foreach (RemoteParticipant sender in senders) { if (sender.ipAddress == ipAddress && sender.port == port) @@ -75,14 +76,14 @@ 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 void Register(byte thingType, ThingConstructor constr) { @@ -94,7 +95,7 @@ namespace Passer.Control.Core { } public void Register(byte thingType) where ThingClass : Thing { - thingMsgProcessors[thingType] = static (byte networkId, byte thingId) => + thingMsgProcessors[thingType] = (byte networkId, byte thingId) => Activator.CreateInstance(typeof(ThingClass), networkId, thingId) as ThingClass; Console.WriteLine($"Registering {typeof(ThingClass)} for thing type {thingType}"); } @@ -115,13 +116,13 @@ namespace Passer.Control.Core { // We can receive our own publish (broadcast) packages. How do we recognize them???? // It is hard to determine our source port string ipAddress = this.endPoint.Address.ToString(); - RemoteParticipant? remoteParticipant = GetParticipant(ipAddress, this.endPoint.Port); + RemoteParticipant remoteParticipant = GetParticipant(ipAddress, this.endPoint.Port); if (remoteParticipant == null) 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; @@ -177,7 +178,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; @@ -289,7 +290,7 @@ namespace Passer.Control.Core { protected virtual void Process(RemoteParticipant sender, NameMsg msg) { // Console.WriteLine($"Participant: Process name [{msg.networkId}/{msg.thingId}] {msg.name}"); - Thing? thing = sender.Get(msg.networkId, msg.thingId); + Thing thing = sender.Get(msg.networkId, msg.thingId); if (thing != null) thing.name = msg.name; } @@ -302,7 +303,7 @@ namespace Passer.Control.Core { protected virtual void Process(RemoteParticipant sender, CustomMsg msg) { // Console.WriteLine($"Participant: Process binary [{msg.networkId}/{msg.thingId}]"); - Thing? thing = sender.Get(msg.networkId, msg.thingId); + Thing thing = sender.Get(msg.networkId, msg.thingId); thing?.ProcessBinary(msg.bytes); } diff --git a/RemoteParticipant.cs b/RemoteParticipant.cs index 8d8341f..aed929b 100644 --- a/RemoteParticipant.cs +++ b/RemoteParticipant.cs @@ -1,4 +1,3 @@ -#nullable enable using System; using System.Collections.Generic; @@ -18,10 +17,10 @@ namespace Passer.Control.Core { this.port = port; } - protected readonly List things = new(); + protected readonly List things = new List(); - public Thing? Get(byte networkId, byte thingId) { - Thing? thing = things.Find(aThing => Thing.IsThing(aThing, networkId, thingId)); + public Thing Get(byte networkId, byte thingId) { + Thing thing = things.Find(aThing => Thing.IsThing(aThing, networkId, thingId)); return thing; } @@ -30,7 +29,7 @@ namespace Passer.Control.Core { public void Add(Thing thing, bool invokeEvent = true) { // Console.WriteLine($"added thing [{thing.networkId}/{thing.id}]"); - Thing? foundThing = Get(thing.networkId, thing.id); + Thing foundThing = Get(thing.networkId, thing.id); if (foundThing == null) { things.Add(thing); diff --git a/SiteServer.cs b/SiteServer.cs index 8c80049..d54f3af 100644 --- a/SiteServer.cs +++ b/SiteServer.cs @@ -1,4 +1,3 @@ -#nullable enable using System; using System.Collections.Generic; using System.Net; @@ -20,7 +19,7 @@ namespace Passer.Control.Core { this.udpClient = new UdpClient(port); // for receiving this.udpClient.BeginReceive( new AsyncCallback(result => ReceiveUDP(result)), - new Tuple(this.udpClient, new(IPAddress.Any, port))); + new Tuple(this.udpClient, new IPEndPoint(IPAddress.Any, port))); } public void Close() { @@ -41,14 +40,15 @@ namespace Passer.Control.Core { protected override void Process(RemoteParticipant sender, ThingMsg msg) { //Console.WriteLine($"SiteServer: Process thing [{msg.networkId}/{msg.thingId}]"); - Thing? thing = sender.Get(msg.networkId, msg.thingId); + Thing thing = sender.Get(msg.networkId, msg.thingId); if (thing == null) { - Thing? newThing = null; - if (thingMsgProcessors.TryGetValue(msg.thingType, out Func? value)) { + Thing newThing = null; + if (thingMsgProcessors.TryGetValue(msg.thingType, out Func value)) { if (value != null) newThing = value(msg.networkId, msg.thingId); } - newThing ??= new Thing(sender, msg.networkId, msg.thingId, msg.thingType); + if (newThing == null) + newThing = new Thing(sender, msg.networkId, msg.thingId, msg.thingType); sender.Add(newThing); } diff --git a/Thing.cs b/Thing.cs index 3570382..6454eae 100644 --- a/Thing.cs +++ b/Thing.cs @@ -1,4 +1,3 @@ -#nullable enable using System; using System.Collections.Generic; using Passer.LinearAlgebra; @@ -43,8 +42,8 @@ namespace Passer.Control.Core { public byte id; public event ChangeHandler OnParentChanged = delegate {}; - private Thing? _parent; - public Thing? parent { + private Thing _parent; + public Thing parent { get => _parent; set { if (_parent == value) diff --git a/float16.cs b/float16.cs index b934651..c1885c3 100644 --- a/float16.cs +++ b/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/test/UnitTest1.cs b/test/UnitTest1.cs index 51c6baa..66633d7 100644 --- a/test/UnitTest1.cs +++ b/test/UnitTest1.cs @@ -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(participant) { + 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/test.csproj b/test/test.csproj index 6fe6bad..1c81788 100644 --- a/test/test.csproj +++ b/test/test.csproj @@ -2,17 +2,14 @@ net5.0 - enable false true - - - - - + + + From a7f6aecb5a5ef977ebc5ad4d59a1396573743346 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 19 Feb 2025 12:27:38 +0100 Subject: [PATCH 10/20] Fix Unity errors --- LinearAlgebra/Vector3.cs | 1 - SiteServer.cs | 1 - Thing.cs | 3 +-- 3 files changed, 1 insertion(+), 4 deletions(-) diff --git a/LinearAlgebra/Vector3.cs b/LinearAlgebra/Vector3.cs index 9349b99..0cc4d7b 100644 --- a/LinearAlgebra/Vector3.cs +++ b/LinearAlgebra/Vector3.cs @@ -1,7 +1,6 @@ #if !UNITY_5_3_OR_NEWER using System; -#if !UNITY_5_3_OR_NEWER namespace Passer.LinearAlgebra { public class Vector3Of { public T x; diff --git a/SiteServer.cs b/SiteServer.cs index 3282444..d54f3af 100644 --- a/SiteServer.cs +++ b/SiteServer.cs @@ -1,4 +1,3 @@ -#nullable enable using System; using System.Collections.Generic; using System.Net; diff --git a/Thing.cs b/Thing.cs index fcf85e6..34f6a65 100644 --- a/Thing.cs +++ b/Thing.cs @@ -1,4 +1,3 @@ -#nullable enable using System; using System.Collections.Generic; using Passer.LinearAlgebra; @@ -129,7 +128,7 @@ namespace Passer.Control.Core { #if UNITY_5_3_OR_NEWER [NonSerialized] - public Unity.Thing? component = null; + public Unity.Thing component = null; #endif #endregion Properties From 9e85419e35233598ba347f4af7da6f5ec033d1d2 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 19 Feb 2025 13:08:32 +0100 Subject: [PATCH 11/20] Namespace -> RoboidControl, added doc --- DoxyGen/Doxyfile | 6 +- EchoStream.cs | 175 ------------------------- float16.cs => LinearAlgebra/float16.cs | 0 Messages/BinaryMsg.cs | 84 ++++++++++++ Messages/ClientMsg.cs | 23 ---- Messages/CustomMsg.cs | 53 -------- Messages/DestroyMsg.cs | 2 +- Messages/InvestigateMsg.cs | 2 +- Messages/LowLevelMessages.cs | 2 +- Messages/Messages.cs | 2 +- Messages/NameMsg.cs | 2 +- Messages/NetworkIdMsg.cs | 2 +- Messages/ParticipantMsg.cs | 51 +++++++ Messages/PoseMsg.cs | 2 +- Messages/TextMsg.cs | 2 +- Messages/ThingMsg.cs | 90 +++++++++++++ ModelUrlMsg.cs | 2 +- Participant.cs | 24 ++-- RemoteParticipant.cs | 46 +++++-- Sensors/DistanceSensor.cs | 2 +- Sensors/TemperatureSensor.cs | 2 +- Sensors/TouchSensor.cs | 2 +- SiteServer.cs | 14 +- Thing.cs | 9 +- ThingMsg.cs | 45 ------- float16.cs.meta | 2 - test/UnitTest1.cs | 2 +- 27 files changed, 310 insertions(+), 338 deletions(-) delete mode 100644 EchoStream.cs rename float16.cs => LinearAlgebra/float16.cs (100%) create mode 100644 Messages/BinaryMsg.cs delete mode 100644 Messages/ClientMsg.cs delete mode 100644 Messages/CustomMsg.cs create mode 100644 Messages/ParticipantMsg.cs create mode 100644 Messages/ThingMsg.cs delete mode 100644 ThingMsg.cs delete mode 100644 float16.cs.meta diff --git a/DoxyGen/Doxyfile b/DoxyGen/Doxyfile index 071d8c0..6a000ae 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 @@ -68,7 +68,7 @@ PROJECT_LOGO = //intranet/home/Afbeeldingen/PasserVR/Logos/Logo3NameRi # 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/float16.cs b/LinearAlgebra/float16.cs similarity index 100% rename from float16.cs rename to LinearAlgebra/float16.cs diff --git a/Messages/BinaryMsg.cs b/Messages/BinaryMsg.cs new file mode 100644 index 0000000..f9ef94e --- /dev/null +++ b/Messages/BinaryMsg.cs @@ -0,0 +1,84 @@ +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(); + } + /// + /// Create the message for receiving + /// + /// The byte array to parse + 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++]; + } + + /// + /// Serialize the message into a byte array for sending + /// + /// The buffer to serilize into + /// The length of the message in the buffer + public override byte Serialize(ref byte[] buffer) { + if (buffer.Length < BinaryMsg.length + buffer.Length || 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 cf06fff..0000000 --- a/Messages/CustomMsg.cs +++ /dev/null @@ -1,53 +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; - this.bytes = new byte[0]; - } - - public override byte Serialize(ref byte[] buffer) { - if (bytes.Length == 0) - 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 index 9a07eda..1564a25 100644 --- a/Messages/DestroyMsg.cs +++ b/Messages/DestroyMsg.cs @@ -1,7 +1,7 @@ using System.IO; using System.Threading.Tasks; -namespace Passer.Control.Core { +namespace Passer.RoboidControl { public class DestroyMsg : IMessage { public const byte Id = 0x20; diff --git a/Messages/InvestigateMsg.cs b/Messages/InvestigateMsg.cs index b6f9765..3f55640 100644 --- a/Messages/InvestigateMsg.cs +++ b/Messages/InvestigateMsg.cs @@ -1,7 +1,7 @@ using System.IO; using System.Threading.Tasks; -namespace Passer.Control.Core { +namespace Passer.RoboidControl { public class InvestigateMsg : IMessage { public const byte Id = 0x81; diff --git a/Messages/LowLevelMessages.cs b/Messages/LowLevelMessages.cs index 591a25d..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 { diff --git a/Messages/Messages.cs b/Messages/Messages.cs index fe160cf..1e6e6ca 100644 --- a/Messages/Messages.cs +++ b/Messages/Messages.cs @@ -1,7 +1,7 @@ using System.Threading.Tasks; using System.IO; -namespace Passer.Control.Core { +namespace Passer.RoboidControl { public class IMessage { public IMessage() { } diff --git a/Messages/NameMsg.cs b/Messages/NameMsg.cs index 70c8d9d..8bddcbe 100644 --- a/Messages/NameMsg.cs +++ b/Messages/NameMsg.cs @@ -1,4 +1,4 @@ -namespace Passer.Control.Core { +namespace Passer.RoboidControl { public class NameMsg : IMessage { public const byte Id = 0x91; // 145 diff --git a/Messages/NetworkIdMsg.cs b/Messages/NetworkIdMsg.cs index 6b65913..c1cf080 100644 --- a/Messages/NetworkIdMsg.cs +++ b/Messages/NetworkIdMsg.cs @@ -1,4 +1,4 @@ -namespace Passer.Control.Core { +namespace Passer.RoboidControl { public class NetworkIdMsg : IMessage { public const byte Id = 0xA1; diff --git a/Messages/ParticipantMsg.cs b/Messages/ParticipantMsg.cs new file mode 100644 index 0000000..5f83a4e --- /dev/null +++ b/Messages/ParticipantMsg.cs @@ -0,0 +1,51 @@ +namespace Passer.RoboidControl { + + /// + /// A participant messages notifies other participants of its presence + /// + 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 + /// + /// + 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 index 14c0a34..3a9ee8f 100644 --- a/Messages/PoseMsg.cs +++ b/Messages/PoseMsg.cs @@ -2,7 +2,7 @@ using System.IO; using System.Threading.Tasks; using Passer.LinearAlgebra; -namespace Passer.Control.Core { +namespace Passer.RoboidControl { public class PoseMsg : IMessage { public const byte Id = 0x10; diff --git a/Messages/TextMsg.cs b/Messages/TextMsg.cs index 3959116..abad8f3 100644 --- a/Messages/TextMsg.cs +++ b/Messages/TextMsg.cs @@ -1,7 +1,7 @@ using System.IO; using System.Threading.Tasks; -namespace Passer.Control.Core { +namespace Passer.RoboidControl { public class TextMsg : IMessage { public TextMsg(byte[] buffer) : base(buffer) {} 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 b/ModelUrlMsg.cs index b1c712c..8f82e47 100644 --- a/ModelUrlMsg.cs +++ b/ModelUrlMsg.cs @@ -1,4 +1,4 @@ -namespace Passer.Control.Core { +namespace Passer.RoboidControl { public class ModelUrlMsg : IMessage { public const byte Id = 0x90; // (144) Model URL diff --git a/Participant.cs b/Participant.cs index a8a650d..a9bce80 100644 --- a/Participant.cs +++ b/Participant.cs @@ -4,13 +4,15 @@ 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; @@ -148,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 @@ -160,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) { @@ -189,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) { @@ -232,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)); @@ -253,8 +255,8 @@ namespace Passer.Control.Core { case PoseMsg.Id: // 0x10 / 16 // 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); @@ -271,7 +273,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}"); @@ -301,7 +303,7 @@ namespace Passer.Control.Core { protected virtual void Process(PoseMsg msg) { } - 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); diff --git a/RemoteParticipant.cs b/RemoteParticipant.cs index 0a7ee81..3f2bc02 100644 --- a/RemoteParticipant.cs +++ b/RemoteParticipant.cs @@ -1,33 +1,63 @@ -#nullable enable using System; using System.Collections.Generic; -namespace Passer.Control.Core { +namespace Passer.RoboidControl { + /// + /// A reference to a participant, possible 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; } + /// + /// The things reported by this participant + /// protected readonly List things = new List(); + /// + /// Get a thing with the given ids + /// + /// The networkId 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) + // Console.WriteLine($"Could not find thing {ipAddress}:{port}[{networkId}/{thingId}]"); return thing; } - // if (thing == null) - // Console.WriteLine($"Could not find thing {ipAddress}:{port}[{networkId}/{thingId}]"); - + /// + /// 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/DistanceSensor.cs b/Sensors/DistanceSensor.cs index b88387b..ed2b369 100644 --- a/Sensors/DistanceSensor.cs +++ b/Sensors/DistanceSensor.cs @@ -1,6 +1,6 @@ using System; -namespace Passer.Control.Core { +namespace Passer.RoboidControl.Core { public class DistanceSensor : Thing { public float distance = 0; diff --git a/Sensors/TemperatureSensor.cs b/Sensors/TemperatureSensor.cs index bb8ed1e..09a311d 100644 --- a/Sensors/TemperatureSensor.cs +++ b/Sensors/TemperatureSensor.cs @@ -1,6 +1,6 @@ using System; -namespace Passer.Control.Core { +namespace Passer.RoboidControl.Core { public class TemperatureSensor : Thing { public float temp = 0; diff --git a/Sensors/TouchSensor.cs b/Sensors/TouchSensor.cs index 6e03864..fb86613 100644 --- a/Sensors/TouchSensor.cs +++ b/Sensors/TouchSensor.cs @@ -1,4 +1,4 @@ -namespace Passer.Control.Core { +namespace Passer.RoboidControl.Core { public class TouchSensor : Thing { public bool touchedSomething = false; diff --git a/SiteServer.cs b/SiteServer.cs index d54f3af..2154c12 100644 --- a/SiteServer.cs +++ b/SiteServer.cs @@ -3,10 +3,17 @@ 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 { + /// + /// Create a new site server + /// + /// public SiteServer(int port = 7681) { this.name = "Site Server"; @@ -22,6 +29,9 @@ namespace Passer.Control.Core { new Tuple(this.udpClient, new IPEndPoint(IPAddress.Any, port))); } + /// + /// Close the site + /// public void Close() { this.udpClient?.Close(); } @@ -29,7 +39,7 @@ namespace Passer.Control.Core { 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)); diff --git a/Thing.cs b/Thing.cs index 34f6a65..e1712da 100644 --- a/Thing.cs +++ b/Thing.cs @@ -2,18 +2,21 @@ 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 + /// + /// Predefined thing types + /// public enum Type { - Undeterment, + Undetermined, // Sensor Switch, DistanceSensor, 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/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/UnitTest1.cs b/test/UnitTest1.cs index 66633d7..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 { From a1d89ff1f78688c188145db1d2b460bb0d0d26a4 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 19 Feb 2025 14:50:09 +0100 Subject: [PATCH 12/20] Updated namespaces --- Sensors/TouchSensor.cs | 2 +- ThingMsg.cs.meta | 2 -- Unity/DistanceSensor.cs | 4 ++-- Unity/SiteServer.cs | 14 +++++++------- Unity/Thing.cs | 6 +++--- Unity/TouchSensor.cs | 10 +++++----- 6 files changed, 18 insertions(+), 20 deletions(-) delete mode 100644 ThingMsg.cs.meta diff --git a/Sensors/TouchSensor.cs b/Sensors/TouchSensor.cs index fb86613..c05b59b 100644 --- a/Sensors/TouchSensor.cs +++ b/Sensors/TouchSensor.cs @@ -1,4 +1,4 @@ -namespace Passer.RoboidControl.Core { +namespace Passer.RoboidControl { public class TouchSensor : Thing { public bool touchedSomething = false; 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/DistanceSensor.cs b/Unity/DistanceSensor.cs index b28340d..dc934eb 100644 --- a/Unity/DistanceSensor.cs +++ b/Unity/DistanceSensor.cs @@ -2,7 +2,7 @@ using System.Collections; using UnityEngine; -namespace Passer.Control.Unity { +namespace Passer.RoboidControl.Unity { public class DistanceSensor : Thing { @@ -20,7 +20,7 @@ namespace Passer.Control.Unity { StartCoroutine(MeasureDistance()); } - public static DistanceSensor Create(Core.Thing parent) { + public static DistanceSensor Create(RoboidControl.Thing parent) { GameObject distanceObj = new("Distance sensor"); DistanceSensor component = distanceObj.AddComponent(); if (parent != null && parent.component != null) diff --git a/Unity/SiteServer.cs b/Unity/SiteServer.cs index f809360..fcc32b1 100644 --- a/Unity/SiteServer.cs +++ b/Unity/SiteServer.cs @@ -3,32 +3,32 @@ 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()); + //Console.SetOut(new UnityLogWriter()); site = new(7681); - Core.Thing.OnNewThing += HandleNewThing; + RoboidControl.Thing.OnNewThing += HandleNewThing; } void OnApplicationQuit() { site.Close(); } - public void HandleNewThing(Core.Thing thing) { + public void HandleNewThing(RoboidControl.Thing thing) { site.Add(thing, false); thingQueue.Enqueue(thing); } protected virtual void Update() { site.Update((ulong)(Time.time * 1000)); - while (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..aa03f00 100644 --- a/Unity/Thing.cs +++ b/Unity/Thing.cs @@ -1,14 +1,14 @@ #if UNITY_5_3_OR_NEWER using UnityEngine; -namespace Passer.Control.Unity { +namespace Passer.RoboidControl.Unity { public class Thing : MonoBehaviour { [field: SerializeField] - public Core.Thing core {get; set; } + public RoboidControl.Thing core {get; set; } - protected void SetCoreThing(Core.Thing thing) { + protected void SetCoreThing(RoboidControl.Thing thing) { core = thing; core.component = this; diff --git a/Unity/TouchSensor.cs b/Unity/TouchSensor.cs index fe31b1b..bd9c8d6 100644 --- a/Unity/TouchSensor.cs +++ b/Unity/TouchSensor.cs @@ -1,23 +1,23 @@ #if UNITY_5_3_OR_NEWER using UnityEngine; -namespace Passer.Control.Unity { +namespace Passer.RoboidControl.Unity { public class TouchSensor : Thing { - public Core.TouchSensor coreSensor { - get => (Core.TouchSensor)base.core; + public RoboidControl.TouchSensor coreSensor { + get => (RoboidControl.TouchSensor)base.core; } protected virtual void Start() { if (core == null) { SiteServer siteServer = FindAnyObjectByType(); - SetCoreThing(new Core.TouchSensor(siteServer.site)); + SetCoreThing(new RoboidControl.TouchSensor(siteServer.site)); } } - public static TouchSensor Create(Core.TouchSensor core) { + public static TouchSensor Create(RoboidControl.TouchSensor core) { GameObject gameObj = core.name != null ? new(core.name) : new("Touch Sensor"); From 7b21331afb7f396a6955ebbe59424b23142596d8 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 19 Feb 2025 15:57:44 +0100 Subject: [PATCH 13/20] Completed core documentation --- .gitignore | 2 +- ControlCore.csproj.meta | 7 -- Messages/BinaryMsg.cs | 13 +-- Messages/DestroyMsg.cs | 49 +++++++--- Messages/InvestigateMsg.cs | 50 +++++++--- Messages/Messages.cs | 62 +++++++----- Messages/NameMsg.cs | 37 ++++++- Messages/NetworkIdMsg.cs | 21 ++++ Messages/PoseMsg.cs | 142 ++++++++++++++++++--------- Messages/TextMsg.cs | 55 +++++++++-- ModelUrlMsg.cs.meta | 2 - Participant.cs | 4 +- README.md | 11 ++- RemoteParticipant.cs | 1 - Sensors.meta | 8 -- Sensors/DistanceSensor.cs | 27 +++++- Sensors/TemperatureSensor.cs | 26 ++++- Sensors/TemperatureSensor.cs.meta | 2 - Sensors/TouchSensor.cs | 13 +++ Thing.cs | 155 +++++++++++++++++++++++------- test.sln.meta | 7 -- test/UnitTest1.cs.meta | 2 - test/test.csproj.meta | 7 -- 23 files changed, 502 insertions(+), 201 deletions(-) delete mode 100644 ControlCore.csproj.meta delete mode 100644 ModelUrlMsg.cs.meta delete mode 100644 Sensors.meta delete mode 100644 Sensors/TemperatureSensor.cs.meta delete mode 100644 test.sln.meta delete mode 100644 test/UnitTest1.cs.meta delete mode 100644 test/test.csproj.meta 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.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/Messages/BinaryMsg.cs b/Messages/BinaryMsg.cs index f9ef94e..e72c6fd 100644 --- a/Messages/BinaryMsg.cs +++ b/Messages/BinaryMsg.cs @@ -47,10 +47,7 @@ namespace Passer.RoboidControl { this.thingId = thing.id; this.bytes = System.Array.Empty(); } - /// - /// Create the message for receiving - /// - /// The byte array to parse + /// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer) public BinaryMsg(byte[] buffer) { byte ix = 1; this.networkId = buffer[ix++]; @@ -61,13 +58,9 @@ namespace Passer.RoboidControl { this.bytes[bytesIx] = buffer[ix++]; } - /// - /// Serialize the message into a byte array for sending - /// - /// The buffer to serilize into - /// The length of the message in the buffer + /// @copydoc Passer::RoboidControl::IMessage::Serialize public override byte Serialize(ref byte[] buffer) { - if (buffer.Length < BinaryMsg.length + buffer.Length || bytes.Length == 0) + if (buffer.Length < BinaryMsg.length + this.bytes.Length || this.bytes.Length == 0) return 0; byte ix = 0; diff --git a/Messages/DestroyMsg.cs b/Messages/DestroyMsg.cs index 1564a25..e5d0a8e 100644 --- a/Messages/DestroyMsg.cs +++ b/Messages/DestroyMsg.cs @@ -3,29 +3,52 @@ using System.Threading.Tasks; 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) { } - public override void Deserialize(byte[] buffer) { - this.networkId = buffer[0]; - this.thingId = buffer[1]; + /// @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; } - 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 DestroyMsg(buffer); - - client.messageQueue.Enqueue(msg); - return true; - } } } \ No newline at end of file diff --git a/Messages/InvestigateMsg.cs b/Messages/InvestigateMsg.cs index 3f55640..43a6d60 100644 --- a/Messages/InvestigateMsg.cs +++ b/Messages/InvestigateMsg.cs @@ -3,18 +3,40 @@ using System.Threading.Tasks; 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; @@ -22,28 +44,28 @@ namespace Passer.RoboidControl { buffer[ix++] = this.thingId; return ix; } - public override void Deserialize(byte[] buffer) { - uint ix = 0; - this.networkId = buffer[ix++]; - this.thingId = buffer[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; + // 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 InvestigateMsg(buffer); - //UnityEngine.Debug.Log($"Receive investigate [{msg.networkId}/{msg.thingId}]"); + // byte[] buffer = await Receive(dataStream, packetSize); + // InvestigateMsg msg = new InvestigateMsg(buffer); + // //UnityEngine.Debug.Log($"Receive investigate [{msg.networkId}/{msg.thingId}]"); - client.messageQueue.Enqueue(msg); - return true; + // client.messageQueue.Enqueue(msg); + // return true; - } + // } } } \ No newline at end of file diff --git a/Messages/Messages.cs b/Messages/Messages.cs index 1e6e6ca..078c8b2 100644 --- a/Messages/Messages.cs +++ b/Messages/Messages.cs @@ -3,40 +3,54 @@ using System.IO; 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 virtual void Deserialize(byte[] buffer) { } - public static bool SendMsg(Participant client, IMessage msg) { - msg.Serialize(ref client.buffer); - return client.SendBuffer(client.buffer.Length); - } + // public bool SendTo(Participant client) { + // 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 bool SendMsg(Participant client, IMessage msg) { + // msg.Serialize(ref client.buffer); + // return client.SendBuffer(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; - } + // 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; + // } } } diff --git a/Messages/NameMsg.cs b/Messages/NameMsg.cs index 8bddcbe..6193c37 100644 --- a/Messages/NameMsg.cs +++ b/Messages/NameMsg.cs @@ -1,23 +1,56 @@ 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 + /// 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; + /// + /// 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++]; @@ -26,8 +59,9 @@ namespace Passer.RoboidControl { 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.Length == 0) + if (buffer.Length < NameMsg.length + this.name.Length || this.name.Length == 0) return 0; byte ix = 0; @@ -40,6 +74,7 @@ namespace Passer.RoboidControl { 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 c1cf080..fc095d0 100644 --- a/Messages/NetworkIdMsg.cs +++ b/Messages/NetworkIdMsg.cs @@ -1,18 +1,39 @@ 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/PoseMsg.cs b/Messages/PoseMsg.cs index 3a9ee8f..e31953e 100644 --- a/Messages/PoseMsg.cs +++ b/Messages/PoseMsg.cs @@ -4,23 +4,73 @@ 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; + /// + /// bitpattern 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 veloicty 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) { this.networkId = networkId; this.thingId = thingId; @@ -34,8 +84,10 @@ namespace Passer.RoboidControl { if (this.orientation != null) this.poseType |= Pose_Orientation; } + /// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer) public PoseMsg(byte[] buffer) : base(buffer) { } + /// @copydoc Passer::RoboidControl::IMessage::Serialize public override byte Serialize(ref byte[] buffer) { byte ix = 0; buffer[ix++] = PoseMsg.Id; @@ -53,59 +105,59 @@ namespace Passer.RoboidControl { 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++]; + // 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); - } + // 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 PoseMsg(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 PoseMsg(buffer); + // public static bool Send(Participant client, byte thingId, Spherical position, SwingTwist orientation) { + // PoseMsg msg = new PoseMsg(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 PoseMsg(buffer); - // Do no process poses with nwid == 0 (== local) - if (msg.networkId == 0) - return true; + // // Do no process poses with nwid == 0 (== local) + // if (msg.networkId == 0) + // return true; - client.messageQueue.Enqueue(msg); - return true; - } + // client.messageQueue.Enqueue(msg); + // return true; + // } - public static bool SendTo(Participant participant, Thing thing) { - if (participant == null || thing == null) - return false; + // 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; + // 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); + // 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); - } + // return participant.SendBuffer(ix); + // } } } \ No newline at end of file diff --git a/Messages/TextMsg.cs b/Messages/TextMsg.cs index abad8f3..7d353e6 100644 --- a/Messages/TextMsg.cs +++ b/Messages/TextMsg.cs @@ -3,24 +3,59 @@ using System.Threading.Tasks; namespace Passer.RoboidControl { + /// + /// Message for sending generic text. + /// public class TextMsg : IMessage { - public TextMsg(byte[] buffer) : base(buffer) {} + /// + /// 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 = ""; - 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); + /// + /// Create a new message for sending + /// + /// The text to send + public TextMsg(string text) { + this.text = text; } - public static async Task Receive(Stream dataStream, Participant client, byte packetSize) { - byte[] buffer = await Receive(dataStream, packetSize); - TextMsg msg = new TextMsg(buffer); + /// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer) + public TextMsg(byte[] buffer) : base(buffer) { } - client.messageQueue.Enqueue(msg); - return true; + /// @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; } + // 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 TextMsg(buffer); + + // client.messageQueue.Enqueue(msg); + // return true; + // } } } \ 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 a9bce80..bebc1f3 100644 --- a/Participant.cs +++ b/Participant.cs @@ -318,7 +318,9 @@ namespace Passer.RoboidControl { 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 44c15f4..3f2bc02 100644 --- a/RemoteParticipant.cs +++ b/RemoteParticipant.cs @@ -1,4 +1,3 @@ -#nullable enable using System; using System.Collections.Generic; 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 ed2b369..2c1db64 100644 --- a/Sensors/DistanceSensor.cs +++ b/Sensors/DistanceSensor.cs @@ -1,22 +1,39 @@ -using System; - -namespace Passer.RoboidControl.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; + /// + /// 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.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 09a311d..edb9750 100644 --- a/Sensors/TemperatureSensor.cs +++ b/Sensors/TemperatureSensor.cs @@ -1,16 +1,32 @@ -using System; +//using System; -namespace Passer.RoboidControl.Core { +namespace Passer.RoboidControl { + /// + /// A temperature sensor + /// public class TemperatureSensor : Thing { - public float temp = 0; + /// + /// 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 index c05b59b..72d0350 100644 --- a/Sensors/TouchSensor.cs +++ b/Sensors/TouchSensor.cs @@ -1,12 +1,25 @@ 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; } #if UNITY_5_3_OR_NEWER + /// @copydoc Passer::RoboidControl::Thing::CreateComponent public override void CreateComponent() { this.component = Unity.TouchSensor.Create(this); this.component.core = this; diff --git a/Thing.cs b/Thing.cs index e1712da..6e40df1 100644 --- a/Thing.cs +++ b/Thing.cs @@ -31,21 +31,41 @@ namespace Passer.RoboidControl { 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 + /// + /// The participant to which this thing belongs + /// public RemoteParticipant participant; public delegate void ChangeHandler(); public delegate void SphericalHandler(Spherical v); + /// + /// The network ID of this thing. + /// public byte networkId; + /// + /// The ID of this thing + /// public byte id; - public event ChangeHandler OnParentChanged = delegate {}; + /// + /// 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 { @@ -63,22 +83,38 @@ namespace Passer.RoboidControl { } } + /// + /// 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 + /// + /// 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 = delegate {}; + /// + /// 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 { @@ -89,11 +125,19 @@ namespace Passer.RoboidControl { } } + /// + /// 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 = delegate {}; + /// + /// 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 { @@ -104,8 +148,14 @@ namespace Passer.RoboidControl { } } - public event ChangeHandler OnOrientationChanged = delegate {}; + /// + /// 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 { @@ -116,8 +166,14 @@ namespace Passer.RoboidControl { } } - public event SphericalHandler OnLinearVelocityChanged = delegate {}; + /// + /// 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 { @@ -127,9 +183,15 @@ namespace Passer.RoboidControl { } } } + /// + /// 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 = null; #endif @@ -138,67 +200,92 @@ namespace Passer.RoboidControl { #region Init + /// + /// 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; } - 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() { } #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 = delegate {}; + /// + /// 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/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.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.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: From 383f0c179303e58d859ffe4fb4c05170bed3b1ed Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 19 Feb 2025 16:49:54 +0100 Subject: [PATCH 14/20] Init Unity documentation --- Unity/Thing.cs | 14 +++++++++++++- Unity/TouchSensor.cs | 3 +++ 2 files changed, 16 insertions(+), 1 deletion(-) diff --git a/Unity/Thing.cs b/Unity/Thing.cs index aa03f00..7ad3e12 100644 --- a/Unity/Thing.cs +++ b/Unity/Thing.cs @@ -2,12 +2,21 @@ using UnityEngine; namespace Passer.RoboidControl.Unity { - + + /// + /// The representation of a Thing in Unity + /// public class Thing : MonoBehaviour { + /// + /// The core C# thing + /// [field: SerializeField] public RoboidControl.Thing core {get; set; } + /// + /// Set the core C# thing + /// protected void SetCoreThing(RoboidControl.Thing thing) { core = thing; core.component = this; @@ -20,6 +29,9 @@ namespace Passer.RoboidControl.Unity { siteServer.site.Add(thing); } + /// + /// Update the Unity representation + /// protected virtual void Update() { if (core == null) return; diff --git a/Unity/TouchSensor.cs b/Unity/TouchSensor.cs index bd9c8d6..0655f9f 100644 --- a/Unity/TouchSensor.cs +++ b/Unity/TouchSensor.cs @@ -3,6 +3,9 @@ using UnityEngine; namespace Passer.RoboidControl.Unity { + /// + /// The Unity representation of the TouchSensor + /// public class TouchSensor : Thing { public RoboidControl.TouchSensor coreSensor { From cd9b4a1e9e7e322b1dfbb09ede120c95dae3a52e Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 19 Feb 2025 17:01:00 +0100 Subject: [PATCH 15/20] namespace change, added doc --- Sensors/DistanceSensor.cs | 2 +- SiteServer.cs | 1 - Unity/DistanceSensor.cs | 24 +++++++++++++++++++++--- Unity/TouchSensor.cs | 14 ++++++++++++-- 4 files changed, 34 insertions(+), 7 deletions(-) diff --git a/Sensors/DistanceSensor.cs b/Sensors/DistanceSensor.cs index 2c1db64..3967ae5 100644 --- a/Sensors/DistanceSensor.cs +++ b/Sensors/DistanceSensor.cs @@ -26,7 +26,7 @@ namespace Passer.RoboidControl { #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 diff --git a/SiteServer.cs b/SiteServer.cs index 2154c12..868aacf 100644 --- a/SiteServer.cs +++ b/SiteServer.cs @@ -1,5 +1,4 @@ using System; -using System.Collections.Generic; using System.Net; using System.Net.Sockets; diff --git a/Unity/DistanceSensor.cs b/Unity/DistanceSensor.cs index 8a9af74..f8cc98a 100644 --- a/Unity/DistanceSensor.cs +++ b/Unity/DistanceSensor.cs @@ -4,13 +4,22 @@ using UnityEngine; namespace Passer.RoboidControl.Unity { + /// + /// The Unity representation of a distance sensor + /// public class DistanceSensor : Thing { + /// + /// 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) { SiteServer siteServer = FindAnyObjectByType(); @@ -20,15 +29,24 @@ namespace Passer.RoboidControl.Unity { StartCoroutine(MeasureDistance()); } - public static DistanceSensor Create(RoboidControl.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/TouchSensor.cs b/Unity/TouchSensor.cs index 0655f9f..31dd50a 100644 --- a/Unity/TouchSensor.cs +++ b/Unity/TouchSensor.cs @@ -8,18 +8,28 @@ namespace Passer.RoboidControl.Unity { /// 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) : @@ -48,7 +58,7 @@ namespace Passer.RoboidControl.Unity { if (this.transform.root == other.transform.root) return; - Debug.Log($"touched {other.gameObject.name}"); + // Debug.Log($"touched {other.gameObject.name}"); this.coreSensor.touchedSomething = true; } private void OnTriggerExit(Collider other) { From 122ac3360b27580c5ded45dd748426255e4c8df0 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 19 Feb 2025 17:02:30 +0100 Subject: [PATCH 16/20] Remove #nullable --- RemoteParticipant.cs | 1 - 1 file changed, 1 deletion(-) diff --git a/RemoteParticipant.cs b/RemoteParticipant.cs index 44c15f4..3f2bc02 100644 --- a/RemoteParticipant.cs +++ b/RemoteParticipant.cs @@ -1,4 +1,3 @@ -#nullable enable using System; using System.Collections.Generic; From b628a0c718efc3f3be3b5014cda6402793cb71bf Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 19 Feb 2025 17:21:39 +0100 Subject: [PATCH 17/20] Updated Doxyfile --- DoxyGen/Doxyfile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/DoxyGen/Doxyfile b/DoxyGen/Doxyfile index 6a000ae..ae7a013 100644 --- a/DoxyGen/Doxyfile +++ b/DoxyGen/Doxyfile @@ -61,7 +61,7 @@ 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 From 8369450d222e304da8eb2d9e345bd8dd894c917f Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Fri, 21 Feb 2025 15:49:41 +0100 Subject: [PATCH 18/20] Improved documentation --- Messages/DestroyMsg.cs | 3 -- Messages/InvestigateMsg.cs | 25 ---------- Messages/Messages.cs | 31 ------------ ModelUrlMsg.cs => Messages/ModelUrlMsg.cs | 33 ++++++++++++- Messages/NameMsg.cs | 2 +- Messages/ParticipantMsg.cs | 4 +- Messages/PoseMsg.cs | 58 +---------------------- Messages/TextMsg.cs | 18 +------ RemoteParticipant.cs | 6 +-- Thing.cs | 46 +++++++++--------- 10 files changed, 65 insertions(+), 161 deletions(-) rename ModelUrlMsg.cs => Messages/ModelUrlMsg.cs (54%) diff --git a/Messages/DestroyMsg.cs b/Messages/DestroyMsg.cs index e5d0a8e..660c142 100644 --- a/Messages/DestroyMsg.cs +++ b/Messages/DestroyMsg.cs @@ -1,6 +1,3 @@ -using System.IO; -using System.Threading.Tasks; - namespace Passer.RoboidControl { /// diff --git a/Messages/InvestigateMsg.cs b/Messages/InvestigateMsg.cs index 43a6d60..d822fec 100644 --- a/Messages/InvestigateMsg.cs +++ b/Messages/InvestigateMsg.cs @@ -1,6 +1,3 @@ -using System.IO; -using System.Threading.Tasks; - namespace Passer.RoboidControl { /// @@ -44,28 +41,6 @@ namespace Passer.RoboidControl { 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 InvestigateMsg(buffer); - // //UnityEngine.Debug.Log($"Receive investigate [{msg.networkId}/{msg.thingId}]"); - - // client.messageQueue.Enqueue(msg); - // return true; - - // } } } \ No newline at end of file diff --git a/Messages/Messages.cs b/Messages/Messages.cs index 078c8b2..d75e62b 100644 --- a/Messages/Messages.cs +++ b/Messages/Messages.cs @@ -1,6 +1,3 @@ -using System.Threading.Tasks; -using System.IO; - namespace Passer.RoboidControl { /// @@ -23,34 +20,6 @@ namespace Passer.RoboidControl { /// 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; - // } } } diff --git a/ModelUrlMsg.cs b/Messages/ModelUrlMsg.cs similarity index 54% rename from ModelUrlMsg.cs rename to Messages/ModelUrlMsg.cs index 8f82e47..01b6527 100644 --- a/ModelUrlMsg.cs +++ b/Messages/ModelUrlMsg.cs @@ -1,22 +1,52 @@ 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; + /// + /// 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++]; @@ -26,6 +56,7 @@ namespace Passer.RoboidControl { 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 6193c37..57de3aa 100644 --- a/Messages/NameMsg.cs +++ b/Messages/NameMsg.cs @@ -9,7 +9,7 @@ namespace Passer.RoboidControl { /// public const byte Id = 0x91; // 145 /// - /// The length of the message + /// The length of the message without the name string /// public const byte length = 4; /// diff --git a/Messages/ParticipantMsg.cs b/Messages/ParticipantMsg.cs index 5f83a4e..6ba3228 100644 --- a/Messages/ParticipantMsg.cs +++ b/Messages/ParticipantMsg.cs @@ -3,6 +3,8 @@ 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 @@ -20,7 +22,7 @@ namespace Passer.RoboidControl { /// /// Create a new message for sending /// - /// + /// The network ID known by the participant public ParticipantMsg(byte networkId) { this.networkId = networkId; } diff --git a/Messages/PoseMsg.cs b/Messages/PoseMsg.cs index e31953e..a43b40d 100644 --- a/Messages/PoseMsg.cs +++ b/Messages/PoseMsg.cs @@ -1,5 +1,3 @@ -using System.IO; -using System.Threading.Tasks; using Passer.LinearAlgebra; namespace Passer.RoboidControl { @@ -27,7 +25,7 @@ namespace Passer.RoboidControl { public byte thingId; /// - /// bitpattern stating which pose components are available + /// Bit pattern stating which pose components are available /// public byte poseType; /// @@ -60,7 +58,7 @@ namespace Passer.RoboidControl { /// public Spherical linearVelocity = Spherical.zero; /// - /// The angular veloicty of the thing in local space + /// The angular velocity of the thing in local space /// public Spherical angularVelocity = Spherical.zero; @@ -105,59 +103,7 @@ namespace Passer.RoboidControl { 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 PoseMsg(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 PoseMsg(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); - // } } } \ No newline at end of file diff --git a/Messages/TextMsg.cs b/Messages/TextMsg.cs index 7d353e6..c2dcf16 100644 --- a/Messages/TextMsg.cs +++ b/Messages/TextMsg.cs @@ -1,10 +1,7 @@ -using System.IO; -using System.Threading.Tasks; - namespace Passer.RoboidControl { /// - /// Message for sending generic text. + /// Message for sending generic text /// public class TextMsg : IMessage { /// @@ -43,19 +40,6 @@ namespace Passer.RoboidControl { buffer[ix++] = (byte)this.text[textIx]; return ix; } - // 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 TextMsg(buffer); - - // client.messageQueue.Enqueue(msg); - // return true; - // } } } \ No newline at end of file diff --git a/RemoteParticipant.cs b/RemoteParticipant.cs index 3f2bc02..64b9fb4 100644 --- a/RemoteParticipant.cs +++ b/RemoteParticipant.cs @@ -4,7 +4,7 @@ using System.Collections.Generic; namespace Passer.RoboidControl { /// - /// A reference to a participant, possible on a remote location + /// A reference to a participant, possibly on a remote location /// public class RemoteParticipant { /// @@ -28,7 +28,7 @@ namespace Passer.RoboidControl { /// /// Create a new remote participant /// - /// The ip address of the participant + /// The IP address of the participant /// The UDP port of the participant public RemoteParticipant(string ipAddress, int port) { this.ipAddress = ipAddress; @@ -43,7 +43,7 @@ namespace Passer.RoboidControl { /// /// Get a thing with the given ids /// - /// The networkId of the thing + /// 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) { diff --git a/Thing.cs b/Thing.cs index 6e40df1..43f1c02 100644 --- a/Thing.cs +++ b/Thing.cs @@ -12,6 +12,28 @@ namespace Passer.RoboidControl { #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 /// @@ -37,27 +59,6 @@ namespace Passer.RoboidControl { /// public byte type; - #endregion Types - - #region Properties - - /// - /// The participant to which this thing belongs - /// - public RemoteParticipant participant; - - public delegate void ChangeHandler(); - public delegate void SphericalHandler(Spherical v); - - /// - /// The network ID of this thing. - /// - public byte networkId; - /// - /// The ID of this thing - /// - public byte id; - /// /// Event which is triggered when the parent changes /// @@ -96,7 +97,7 @@ namespace Passer.RoboidControl { /// /// Remove the given thing as a child of this thing /// - /// + /// The child to remove public void RemoveChild(Thing child) { children.Remove(child); } @@ -266,7 +267,6 @@ namespace Passer.RoboidControl { #endregion Update - public delegate void ThingHandler(Thing t); /// /// Event triggered when a new thing has been created /// From eca5e698cdaccdbe85e34435bbc5484411b88abe Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Mon, 24 Feb 2025 12:54:25 +0100 Subject: [PATCH 19/20] First UDP communication is working --- Participant.cs | 16 ++++++++-------- RemoteParticipant.cs | 1 - SiteServer.cs | 23 ++++++++++++++--------- Thing.cs | 7 ++++++- Unity/DebugConsole.cs | 2 +- Unity/SiteServer.cs | 5 +++-- Unity/Thing.cs | 21 +++++++++++++++++++-- 7 files changed, 51 insertions(+), 24 deletions(-) diff --git a/Participant.cs b/Participant.cs index bebc1f3..cacb27a 100644 --- a/Participant.cs +++ b/Participant.cs @@ -34,9 +34,9 @@ namespace Passer.RoboidControl { /// 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 @@ -85,11 +85,11 @@ namespace Passer.RoboidControl { return participant; } - protected readonly Dictionary> thingMsgProcessors = new Dictionary>(); + 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 { @@ -97,8 +97,8 @@ namespace Passer.RoboidControl { } 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}"); } diff --git a/RemoteParticipant.cs b/RemoteParticipant.cs index 44c15f4..3f2bc02 100644 --- a/RemoteParticipant.cs +++ b/RemoteParticipant.cs @@ -1,4 +1,3 @@ -#nullable enable using System; using System.Collections.Generic; diff --git a/SiteServer.cs b/SiteServer.cs index 868aacf..23679c9 100644 --- a/SiteServer.cs +++ b/SiteServer.cs @@ -9,23 +9,25 @@ namespace Passer.RoboidControl { /// public class SiteServer : Participant { + public SiteServer(int port = 7681) : this("0.0.0.0", port) {} + /// /// Create a new site server /// /// - public SiteServer(int port = 7681) { + 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 + this.endPoint = new IPEndPoint(IPAddress.Parse(ipAddress), port); // for sending Console.Write($"Prepare receive on port {port}"); this.udpClient = new UdpClient(port); // for receiving this.udpClient.BeginReceive( new AsyncCallback(result => ReceiveUDP(result)), - new Tuple(this.udpClient, new IPEndPoint(IPAddress.Any, port))); + new Tuple(this.udpClient, new(IPAddress.Any, port))); } /// @@ -48,17 +50,20 @@ namespace Passer.RoboidControl { 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 = null; - if (thingMsgProcessors.TryGetValue(msg.thingType, out Func value)) { + if (thingMsgProcessors.TryGetValue(msg.thingType, out Func value)) { + Console.WriteLine("Found thing message processor"); if (value != null) - newThing = value(msg.networkId, msg.thingId); + newThing = value(sender, msg.networkId, msg.thingId); } - if (newThing == null) + if (newThing == null) { newThing = new Thing(sender, msg.networkId, msg.thingId, msg.thingType); - + Console.WriteLine("Created generic new core thing"); + } + sender.Add(newThing); } } diff --git a/Thing.cs b/Thing.cs index 6e40df1..8730e7e 100644 --- a/Thing.cs +++ b/Thing.cs @@ -228,7 +228,12 @@ namespace Passer.RoboidControl { /// Function which can be used to create components in external engines. /// /// Currently this is used to create GameObjects in Unity - public virtual void CreateComponent() { } + public virtual void CreateComponent() { +#if UNITY_5_3_OR_NEWER + this.component = Unity.Thing.Create(this); + this.component.core = this; +#endif + } #endregion Init 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/SiteServer.cs b/Unity/SiteServer.cs index fcc32b1..ae16757 100644 --- a/Unity/SiteServer.cs +++ b/Unity/SiteServer.cs @@ -11,7 +11,7 @@ namespace Passer.RoboidControl.Unity { public Queue thingQueue = new(); protected virtual void Awake() { - //Console.SetOut(new UnityLogWriter()); + Console.SetOut(new UnityLogWriter()); site = new(7681); RoboidControl.Thing.OnNewThing += HandleNewThing; @@ -22,6 +22,7 @@ namespace Passer.RoboidControl.Unity { } public void HandleNewThing(RoboidControl.Thing thing) { + Debug.Log("Handle New thing event"); site.Add(thing, false); thingQueue.Enqueue(thing); } @@ -29,7 +30,7 @@ namespace Passer.RoboidControl.Unity { protected virtual void Update() { site.Update((ulong)(Time.time * 1000)); while (thingQueue.TryDequeue(out RoboidControl.Thing thing)) - thing.CreateComponent(); + thing.CreateComponent(); } } diff --git a/Unity/Thing.cs b/Unity/Thing.cs index 7ad3e12..2d81238 100644 --- a/Unity/Thing.cs +++ b/Unity/Thing.cs @@ -2,7 +2,7 @@ using UnityEngine; namespace Passer.RoboidControl.Unity { - + /// /// The representation of a Thing in Unity /// @@ -12,7 +12,7 @@ namespace Passer.RoboidControl.Unity { /// The core C# thing /// [field: SerializeField] - public RoboidControl.Thing core {get; set; } + public RoboidControl.Thing core { get; set; } /// /// Set the core C# thing @@ -29,6 +29,23 @@ namespace Passer.RoboidControl.Unity { 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 /// From 28d3a98bea156585c804417f43fe49a3b09d88fb Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Mon, 24 Feb 2025 17:42:09 +0100 Subject: [PATCH 20/20] Arduino Ant random walk --- Messages/PoseMsg.cs | 34 ++++++++++++++++++++++++----- Participant.cs | 25 ++++++++++++++++++++- Sensors/TouchSensor.cs | 5 +++++ SiteServer.cs | 13 +++++++++-- Thing.cs | 2 ++ Unity/SiteServer.cs | 4 +++- Unity/Thing.cs | 49 ++++++++++++++++++++++++++++++++++++++++-- Unity/TouchSensor.cs | 2 +- 8 files changed, 122 insertions(+), 12 deletions(-) diff --git a/Messages/PoseMsg.cs b/Messages/PoseMsg.cs index a43b40d..4f1320d 100644 --- a/Messages/PoseMsg.cs +++ b/Messages/PoseMsg.cs @@ -69,21 +69,45 @@ namespace Passer.RoboidControl { /// 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) { + public PoseMsg(byte networkId, byte thingId, Spherical position, SwingTwist orientation, Spherical linearVelocity = null, Spherical angularVelocity = null) { this.networkId = networkId; this.thingId = thingId; - this.position = position; - this.orientation = orientation; - 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) { } + 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) { diff --git a/Participant.cs b/Participant.cs index cacb27a..349f50d 100644 --- a/Participant.cs +++ b/Participant.cs @@ -253,6 +253,7 @@ namespace Passer.RoboidControl { 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 BinaryMsg.Id: // 0xB1 / 177 @@ -299,9 +300,31 @@ namespace Passer.RoboidControl { 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, BinaryMsg msg) { // Console.WriteLine($"Participant: Process binary [{msg.networkId}/{msg.thingId}]"); diff --git a/Sensors/TouchSensor.cs b/Sensors/TouchSensor.cs index 72d0350..f6bc873 100644 --- a/Sensors/TouchSensor.cs +++ b/Sensors/TouchSensor.cs @@ -18,9 +18,14 @@ namespace Passer.RoboidControl { 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; } diff --git a/SiteServer.cs b/SiteServer.cs index 23679c9..9bd1ae9 100644 --- a/SiteServer.cs +++ b/SiteServer.cs @@ -9,7 +9,7 @@ namespace Passer.RoboidControl { /// public class SiteServer : Participant { - public SiteServer(int port = 7681) : this("0.0.0.0", port) {} + public SiteServer(int port = 7681) : this("0.0.0.0", port) { } /// /// Create a new site server @@ -28,6 +28,8 @@ namespace Passer.RoboidControl { this.udpClient.BeginReceive( new AsyncCallback(result => ReceiveUDP(result)), new Tuple(this.udpClient, new(IPAddress.Any, port))); + + Register(Thing.Type.TouchSensor); } /// @@ -55,7 +57,7 @@ namespace Passer.RoboidControl { if (thing == null) { Thing newThing = null; if (thingMsgProcessors.TryGetValue(msg.thingType, out Func value)) { - Console.WriteLine("Found thing message processor"); + // Console.WriteLine("Found thing message processor"); if (value != null) newThing = value(sender, msg.networkId, msg.thingId); } @@ -63,6 +65,13 @@ namespace Passer.RoboidControl { 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 95dc1b1..92d929d 100644 --- a/Thing.cs +++ b/Thing.cs @@ -44,6 +44,7 @@ namespace Passer.RoboidControl { DistanceSensor, DirectionalSensor, TemperatureSensor, + TouchSensor, // Motor ControlledMotor, UncontrolledMotor, @@ -148,6 +149,7 @@ namespace Passer.RoboidControl { } } } + public bool hasPosition = false; /// /// Event triggered when the orientation has changed diff --git a/Unity/SiteServer.cs b/Unity/SiteServer.cs index ae16757..ce665de 100644 --- a/Unity/SiteServer.cs +++ b/Unity/SiteServer.cs @@ -15,6 +15,8 @@ namespace Passer.RoboidControl.Unity { site = new(7681); RoboidControl.Thing.OnNewThing += HandleNewThing; + + //site.Register(RoboidControl.Thing.Type.TouchSensor); } void OnApplicationQuit() { @@ -22,7 +24,7 @@ namespace Passer.RoboidControl.Unity { } public void HandleNewThing(RoboidControl.Thing thing) { - Debug.Log("Handle New thing event"); + // Debug.Log("Handle New thing event"); site.Add(thing, false); thingQueue.Enqueue(thing); } diff --git a/Unity/Thing.cs b/Unity/Thing.cs index 2d81238..8515edb 100644 --- a/Unity/Thing.cs +++ b/Unity/Thing.cs @@ -1,5 +1,7 @@ #if UNITY_5_3_OR_NEWER +using System.Collections; using UnityEngine; +using UnityEngine.Networking; namespace Passer.RoboidControl.Unity { @@ -14,6 +16,8 @@ namespace Passer.RoboidControl.Unity { [field: SerializeField] public RoboidControl.Thing core { get; set; } + private string modelUrl = null; + /// /// Set the core C# thing /// @@ -27,6 +31,7 @@ namespace Passer.RoboidControl.Unity { return; } siteServer.site.Add(thing); + } public static Thing Create(RoboidControl.Thing core) { @@ -55,13 +60,53 @@ namespace Passer.RoboidControl.Unity { 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 index 31dd50a..62f6b51 100644 --- a/Unity/TouchSensor.cs +++ b/Unity/TouchSensor.cs @@ -18,7 +18,7 @@ namespace Passer.RoboidControl.Unity { /// /// Start the Unity represention /// - protected virtual void Start() { + protected virtual void Start() { if (core == null) { SiteServer siteServer = FindAnyObjectByType(); SetCoreThing(new RoboidControl.TouchSensor(siteServer.site));