RoboidControl/RoboidControl-csharp
RoboidControl/RoboidControl-csharp
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Merge commit '28d3a98bea156585c804417f43fe49a3b09d88fb'

Browse Source
This commit is contained in:
Pascal Serrarens 2025-02-25 11:25:31 +01:00
commit 7d2ffeb11c
52 changed files with 1304 additions and 861 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
.editorconfig
View File

@ -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
# Suppress warnings everywhere
dotnet_diagnostic.IDE1006.severity = none
dotnet_diagnostic.IDE0130.severity = none

2
.gitignore vendored
View File

@ -4,4 +4,4 @@ test/bin
test/obj
/bin
/obj
*.meta
**.meta

11
ControlCore.csproj
View File

@ -3,17 +3,12 @@
<PropertyGroup>
<GenerateAssemblyInfo>false</GenerateAssemblyInfo>
<GenerateTargetFrameworkAttribute>false</GenerateTargetFrameworkAttribute>
<TargetFramework>net9.0</TargetFramework>
<ImplicitUsings>enable</ImplicitUsings>
<Nullable>enable</Nullable>
<TargetFramework>net5.0</TargetFramework>
</PropertyGroup>
<ItemGroup>
<PackageReference Include="coverlet.collector" Version="6.0.2" />
<PackageReference Include="Microsoft.NET.Test.Sdk" Version="17.12.0" />
<PackageReference Include="NUnit" Version="3.13.3" />
<PackageReference Include="NUnit.Analyzers" Version="4.3.0" />
<PackageReference Include="NUnit3TestAdapter" Version="4.6.0" />
<PackageReference Include="Microsoft.NET.Test.Sdk" Version="17.13.0" />
<PackageReference Include="NUnit" Version="3.13.2" />
</ItemGroup>
</Project>

7
ControlCore.csproj.meta
View File

@ -1,7 +0,0 @@
fileFormatVersion: 2
guid: 7f964f406734cf74097d61697e5cafc9
DefaultImporter:
externalObjects: {}
userData:
assetBundleName:
assetBundleVariant:

8
DoxyGen/Doxyfile
View File

@ -42,7 +42,7 @@ DOXYFILE_ENCODING = UTF-8
# title of most generated pages and in a few other places.
# The default value is: My Project.
PROJECT_NAME = "Control Core for C#"
PROJECT_NAME = "Roboid Control for C#"
# The PROJECT_NUMBER tag can be used to enter a project or revision number. This
# could be handy for archiving the generated documentation or if some version
@ -61,14 +61,14 @@ PROJECT_BRIEF =
# pixels and the maximum width should not exceed 200 pixels. Doxygen will copy
# the logo to the output directory.
PROJECT_LOGO = //intranet/home/Afbeeldingen/PasserVR/Logos/Logo3NameRight100.png
PROJECT_LOGO = //intranet/home/Afbeeldingen/PasserVR/Logos/PasserLife/PasserLifeLogoLeft_300.png
# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) path
# into which the generated documentation will be written. If a relative path is
# entered, it will be relative to the location where doxygen was started. If
# left blank the current directory will be used.
OUTPUT_DIRECTORY = //intranet/web/passer_life/apis/ControlCore/Csharp/
OUTPUT_DIRECTORY = //intranet/web/roboidcontrol_doc/Csharp/
# If the CREATE_SUBDIRS tag is set to YES then doxygen will create up to 4096
# sub-directories (in 2 levels) under the output directory of each output format
@ -777,7 +777,7 @@ MAX_INITIALIZER_LINES = 30
# list will mention the files that were used to generate the documentation.
# The default value is: YES.
SHOW_USED_FILES = YES
SHOW_USED_FILES = NO
# Set the SHOW_FILES tag to NO to disable the generation of the Files page. This
# will remove the Files entry from the Quick Index and from the Folder Tree View

175
EchoStream.cs
View File

@ -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<T>, which is what we want
private readonly BlockingCollection<byte[]> _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<byte[]>(_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<int> 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();
}
}
*/

12
LinearAlgebra/Angle.cs
View File

@ -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;
}
}

12
LinearAlgebra/Direction.cs
View File

@ -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) {

39
LinearAlgebra/Matrix.cs
View File

@ -1,7 +1,10 @@
using System;
using System.Diagnostics;
using Vector3 = UnityEngine.Vector3;
using Vector2 = UnityEngine.Vector2;
using Passer.LinearAlgebra;
#if UNITY_5_3_OR_NEWER
using Vector3Float = UnityEngine.Vector3;
using Vector2Float = UnityEngine.Vector2;
#endif
using Quaternion = UnityEngine.Quaternion;
public readonly struct Slice {
@ -60,7 +63,7 @@ public class Matrix2 {
return new Matrix2(resultData);
}
public static Matrix2 SkewMatrix(Vector3 v) {
public static Matrix2 SkewMatrix(Vector3Float v) {
float[,] result = new float[3, 3] {
{0, -v.z, v.y},
{v.z, 0, -v.x},
@ -69,16 +72,16 @@ public class Matrix2 {
return new Matrix2(result);
}
public Vector3 GetRow3(int rowIx) {
public Vector3Float GetRow3(int rowIx) {
uint cols = this.nCols;
Vector3 row = new() {
Vector3Float row = new() {
x = this.data[rowIx, 0],
y = this.data[rowIx, 1],
z = this.data[rowIx, 2]
};
return row;
}
public void SetRow3(int rowIx, Vector3 v) {
public void SetRow3(int rowIx, Vector3Float v) {
this.data[rowIx, 0] = v.x;
this.data[rowIx, 1] = v.y;
this.data[rowIx, 2] = v.z;
@ -174,12 +177,12 @@ public class Matrix2 {
return new Matrix1(result);
}
public static Vector3 operator *(Matrix2 A, Vector3 v) {
return new Vector3() {
x = A.data[0, 0] * v.x + A.data[0, 1] * v.y + A.data[0, 2] * v.z,
y = A.data[1, 0] * v.x + A.data[1, 1] * v.y + A.data[1, 2] * v.z,
z = A.data[2, 0] * v.x + A.data[2, 1] * v.y + A.data[2, 2] * v.z
};
public static Vector3Float operator *(Matrix2 A, Vector3Float v) {
return new Vector3Float(
A.data[0, 0] * v.x + A.data[0, 1] * v.y + A.data[0, 2] * v.z,
A.data[1, 0] * v.x + A.data[1, 1] * v.y + A.data[1, 2] * v.z,
A.data[2, 0] * v.x + A.data[2, 1] * v.y + A.data[2, 2] * v.z
);
}
public static Matrix2 operator *(Matrix2 A, float s) {
@ -360,14 +363,14 @@ public class Matrix1 {
return new Matrix1(magnitude);
}
public static Matrix1 FromVector2(Vector2 v) {
public static Matrix1 FromVector2(Vector2Float v) {
float[] result = new float[2];
result[0] = v.x;
result[1] = v.y;
return new Matrix1(result);
}
public static Matrix1 FromVector3(Vector3 v) {
public static Matrix1 FromVector3(Vector3Float v) {
float[] result = new float[3];
result[0] = v.x;
result[1] = v.y;
@ -384,18 +387,18 @@ public class Matrix1 {
return new Matrix1(result);
}
public Vector2 vector2 {
public Vector2Float vector2 {
get {
if (this.magnitude != 2)
throw new System.ArgumentException("Matrix1 must be of size 2");
return new Vector2(this.data[0], this.data[1]);
return new Vector2Float(this.data[0], this.data[1]);
}
}
public Vector3 vector3 {
public Vector3Float vector3 {
get {
if (this.magnitude != 3)
throw new System.ArgumentException("Matrix1 must be of size 3");
return new Vector3(this.data[0], this.data[1], this.data[2]);
return new Vector3Float(this.data[0], this.data[1], this.data[2]);
}
}
public Quaternion quaternion {

2
LinearAlgebra/Quat32.cs
View File

@ -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,

31
LinearAlgebra/Spherical.cs
View File

@ -1,12 +1,15 @@
using UnityEngine;
using System;
#if UNITY_5_3_OR_NEWER
using Vector3Float = UnityEngine.Vector3;
#endif
namespace Passer.LinearAlgebra {
public class Spherical {
public float distance;
public Direction direction;
public static Spherical zero = new(0, 0, 0);
public static Spherical forward = new(1, 0, 0);
public static Spherical zero = new Spherical(0, 0, 0);
public static Spherical forward = new Spherical(1, 0, 0);
public Spherical(float distance, float horizontal, float vertical) {
this.distance = distance;
@ -17,30 +20,30 @@ namespace Passer.LinearAlgebra {
this.direction = direction;
}
public static Spherical FromVector3(Vector3 v) {
public static Spherical FromVector3(Vector3Float v) {
float distance = v.magnitude;
if (distance == 0.0f)
return new Spherical(distance, 0, 0);
else {
float verticalAngle = (Mathf.PI / 2 - Mathf.Acos(v.y / distance)) * Mathf.Rad2Deg;
float horizontalAngle = Mathf.Atan2(v.x, v.z) * Mathf.Rad2Deg;
float verticalAngle = (float)((Angle.pi / 2 - Math.Acos(v.y / distance)) * Angle.Rad2Deg);
float horizontalAngle = (float) Math.Atan2(v.x, v.z) * Angle.Rad2Deg;
return new Spherical(distance, horizontalAngle, verticalAngle);
}
}
public Vector3 ToVector3() {
float verticalRad = (UnityEngine.Mathf.PI / 2) - this.direction.vertical * UnityEngine.Mathf.Deg2Rad;
float horizontalRad = this.direction.horizontal * UnityEngine.Mathf.Deg2Rad;
float cosVertical = UnityEngine.Mathf.Cos(verticalRad);
float sinVertical = UnityEngine.Mathf.Sin(verticalRad);
float cosHorizontal = UnityEngine.Mathf.Cos(horizontalRad);
float sinHorizontal = UnityEngine.Mathf.Sin(horizontalRad);
public Vector3Float ToVector3() {
float verticalRad = (Angle.pi / 2) - this.direction.vertical * Angle.Deg2Rad;
float horizontalRad = this.direction.horizontal * Angle.Deg2Rad;
float cosVertical = (float)Math.Cos(verticalRad);
float sinVertical = (float)Math.Sin(verticalRad);
float cosHorizontal = (float)Math.Cos(horizontalRad);
float sinHorizontal = (float)Math.Sin(horizontalRad);
float x = this.distance * sinVertical * sinHorizontal;
float y = this.distance * cosVertical;
float z = this.distance * sinVertical * cosHorizontal;
Vector3 v = new Vector3(x, y, z);
Vector3Float v = new Vector3Float(x, y, z);
return v;
}
}

4
LinearAlgebra/SwingTwist.cs
View File

@ -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;
}
}

14
LinearAlgebra/Vector3.cs
View File

@ -1,5 +1,7 @@
namespace Passer.LinearAlgebra {
#if !UNITY_5_3_OR_NEWER
using System;
namespace Passer.LinearAlgebra {
public class Vector3Of<T> {
public T x;
public T y;
@ -10,6 +12,10 @@ namespace Passer.LinearAlgebra {
this.y = y;
this.z = z;
}
// public uint magnitude {
// get => (float)Math.Sqrt(this.x * this.x + this.y * this.y + this.z * this.z);
// }
}
public class Vector3Int : Vector3Of<int> {
@ -17,5 +23,11 @@ namespace Passer.LinearAlgebra {
}
public class Vector3Float : Vector3Of<float> {
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);
}
}
}
#endif

55
float16.cs → LinearAlgebra/float16.cs
View File

@ -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

77
Messages/BinaryMsg.cs Normal file
View File

@ -0,0 +1,77 @@
namespace Passer.RoboidControl {
/// <summary>
/// A message containing binary data for custom communication
/// </summary>
public class BinaryMsg : IMessage {
/// <summary>
/// The message ID
/// </summary>
public const byte Id = 0xB1;
/// <summary>
/// The length of the message, excluding the binary data
/// </summary>
/// For the total size of the message this.bytes.Length should be added to this value.
public const byte length = 2;
/// <summary>
/// The network ID identifying the thing
/// </summary>
public byte networkId;
/// <summary>
/// The ID of the thing
/// </summary>
public byte thingId;
/// <summary>
/// The binary data
/// </summary>
public byte[] bytes;
/// <summary>
/// Create a new message for sending
/// </summary>
/// <param name="networkId">The netowork ID of the thing</param>
/// <param name="thingId">The ID of the thing</param>
/// <param name="bytes">The binary data for the thing</param>
public BinaryMsg(byte networkId, byte thingId, byte[] bytes) : base() {
this.networkId = networkId;
this.thingId = thingId;
this.bytes = bytes;
}
/// <summary>
/// Create an empty message for sending
/// </summary>
/// <param name="networkId">The netowork ID of the thing</param>
/// <param name="thingId">The ID of the thing</param>
public BinaryMsg(byte networkId, Thing thing) : base() {
this.networkId = networkId;
this.thingId = thing.id;
this.bytes = System.Array.Empty<byte>();
}
/// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer)
public BinaryMsg(byte[] buffer) {
byte ix = 1;
this.networkId = buffer[ix++];
this.thingId = buffer[ix++];
byte length = (byte)(buffer.Length - ix);
this.bytes = new byte[length];
for (uint bytesIx = 0; bytesIx < length; bytesIx++)
this.bytes[bytesIx] = buffer[ix++];
}
/// @copydoc Passer::RoboidControl::IMessage::Serialize
public override byte Serialize(ref byte[] buffer) {
if (buffer.Length < BinaryMsg.length + this.bytes.Length || this.bytes.Length == 0)
return 0;
byte ix = 0;
buffer[ix++] = BinaryMsg.Id;
buffer[ix++] = this.networkId;
buffer[ix++] = this.thingId;
foreach (byte b in bytes)
buffer[ix++] = b;
return ix;
}
}
}

23
Messages/ClientMsg.cs
View File

@ -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;
}
}
}

51
Messages/CustomMsg.cs
View File

@ -1,51 +0,0 @@
namespace Passer.Control.Core {
public class CustomMsg : IMessage {
public const byte Id = 0xB1;
public byte networkId;
public byte thingId;
public byte[] bytes;
public CustomMsg(byte[] buffer) {
byte ix = 1;
this.networkId = buffer[ix++];
this.thingId = buffer[ix++];
byte length = (byte)(buffer.Length - ix);
this.bytes = new byte[length];
for (uint bytesIx = 0; bytesIx < length; bytesIx++)
this.bytes[bytesIx] = buffer[ix++];
}
public CustomMsg(byte networkId, byte thingId, byte[] bytes) : base() {
this.networkId = networkId;
this.thingId = thingId;
this.bytes = bytes;
}
public CustomMsg(byte networkId, Thing thing) : base() {
this.networkId = networkId;
this.thingId = thing.id;
}
public override byte Serialize(ref byte[] buffer) {
if (bytes == null)
return 0;
byte ix = 0;
buffer[ix++] = CustomMsg.Id;
buffer[ix++] = this.networkId;
buffer[ix++] = this.thingId;
//buffer[ix++] = (byte)bytes.Length;
foreach (byte b in bytes)
buffer[ix++] = b;
return ix;
}
//public static async Task<bool> Receive(Stream dataStream, Participant client, byte packetSize) {
// byte[] buffer = await Receive(dataStream, packetSize);
// CustomMsg msg = new(buffer);
// client.messageQueue.Enqueue(msg);
// return true;
//}
}
}

51
Messages/DestroyMsg.cs Normal file
View File

@ -0,0 +1,51 @@
namespace Passer.RoboidControl {
/// <summary>
/// Message notifiying that a Thing no longer exists
/// </summary>
public class DestroyMsg : IMessage {
/// <summary>
/// The message ID
/// </summary>
public const byte Id = 0x20;
/// <summary>
/// The length of the message
/// </summary>
public const byte length = 3;
/// <summary>
/// The network ID of the thing
/// </summary>
public byte networkId;
/// <summary>
/// The ID of the thing
/// </summary>
public byte thingId;
/// <summary>
/// Create a message for sending
/// </summary>
/// <param name="networkId">The network ID of the thing</param>
/// <param name="thingId">The ID of the thing</param>
public DestroyMsg(byte networkId, byte thingId) {
this.networkId = networkId;
this.thingId = thingId;
}
/// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer)
public DestroyMsg(byte[] buffer) : base(buffer) { }
/// @copydoc Passer::RoboidControl::IMessage::Serialize
public override byte Serialize(ref byte[] buffer) {
if (buffer.Length < DestroyMsg.length)
return 0;
byte ix = 0;
buffer[ix++] = DestroyMsg.Id;
buffer[ix++] = this.networkId;
buffer[ix++] = this.thingId;
return ix;
}
}
}

46
Messages/InvestigateMsg.cs Normal file
View File

@ -0,0 +1,46 @@
namespace Passer.RoboidControl {
/// <summary>
/// Message to request details for a Thing
/// </summary>
public class InvestigateMsg : IMessage {
/// <summary>
/// The message Id
/// </summary>
public const byte Id = 0x81;
/// <summary>
/// The length of the message
/// </summary>
public const byte length = 3;
/// <summary>
/// The network ID of the thing
/// </summary>
public byte networkId;
/// <summary>
/// The ID of the thing
/// </summary>
public byte thingId;
/// <summary>
/// Create a new message for sending
/// </summary>
/// <param name="networkId">The network ID for the thing</param>
/// <param name="thingId">The ID of the thing</param>
public InvestigateMsg(byte networkId, byte thingId) {
this.networkId = networkId;
this.thingId = thingId;
}
/// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer)
public InvestigateMsg(byte[] buffer) : base(buffer) { }
/// @copydoc Passer::RoboidControl::IMessage::Serialize
public override byte Serialize(ref byte[] buffer) {
byte ix = 0;
buffer[ix++] = InvestigateMsg.Id;
buffer[ix++] = this.networkId;
buffer[ix++] = this.thingId;
return ix;
}
}
}

10
Messages/LowLevelMessages.cs
View File

@ -1,6 +1,6 @@
using Passer.LinearAlgebra;
namespace Passer.Control.Core
namespace Passer.RoboidControl
{
public class LowLevelMessages
{
@ -17,7 +17,7 @@ namespace Passer.Control.Core
float distance = ReceiveFloat16(data, ref ix);
float horizontal = ReceiveAngle8(data, ref ix);
float vertical = ReceiveAngle8(data, ref ix);
Spherical v = new(distance, horizontal, vertical);
Spherical v = new Spherical(distance, horizontal, vertical);
return v;
}
@ -48,7 +48,7 @@ namespace Passer.Control.Core
}
public static Quat32 ReceiveQuat32(byte[] data, ref byte ix)
{
Quat32 q = new(
Quat32 q = new Quat32(
(data[ix++] - 128.0F) / 127.0F,
(data[ix++] - 128.0F) / 127.0F,
(data[ix++] - 128.0F) / 127.0F,
@ -84,7 +84,7 @@ namespace Passer.Control.Core
public static void SendFloat16(byte[] data, ref byte ix, float f)
{
float16 f16 = new(f);
float16 f16 = new float16(f);
ushort binary = f16.GetBinary();
data[ix++] = (byte)(binary >> 8);
data[ix++] = (byte)(binary & 255);
@ -99,7 +99,7 @@ namespace Passer.Control.Core
public static float ReceiveFloat16(byte[] data, ref byte ix)
{
ushort value = (ushort)(data[ix++] << 8 | data[ix++]);
float16 f16 = new();
float16 f16 = new float16();
f16.SetBinary(value);
float f = f16.toFloat();
return f;

258
Messages/Messages.cs
View File

@ -1,253 +1,25 @@
using System.IO;
using System.Threading.Tasks;
using Passer.LinearAlgebra;
namespace Passer.Control.Core {
namespace Passer.RoboidControl {
/// <summary>
/// Root structure for all communcation messages
/// </summary>
public class IMessage {
public IMessage() { }
/// <summary>
/// Create a message for receiving
/// </summary>
/// <param name="buffer">The byte array to parse</param>
public IMessage(byte[] buffer) {
Deserialize(buffer);
//Deserialize(buffer);
}
/// <summary>
/// Serialize the message into a byte array for sending
/// </summary>
/// <param name="buffer">The buffer to serilize into</param>
/// <returns>The length of the message in the buffer</returns>
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<byte[]> Receive(Stream dataStream, byte packetSize) {
byte[] buffer = new byte[packetSize - 1]; // without msgId
int byteCount = dataStream.Read(buffer, 0, packetSize - 1);
while (byteCount < packetSize - 1) {
// not all bytes have been read, wait and try again
await Task.Delay(1);
byteCount += dataStream.Read(buffer, byteCount, packetSize - 1 - byteCount);
}
return buffer;
}
}
#region Investigate
public class InvestigateMsg : IMessage {
public const byte Id = 0x81;
public const byte length = 3;
public byte networkId;
public byte thingId;
public InvestigateMsg(byte networkId, byte thingId) {
this.networkId = networkId;
this.thingId = thingId;
}
public InvestigateMsg(byte[] buffer) : base(buffer) { }
public override byte Serialize(ref byte[] buffer) {
byte ix = 0;
buffer[ix++] = InvestigateMsg.Id;
buffer[ix++] = this.networkId;
buffer[ix++] = this.thingId;
return ix;
}
public override void Deserialize(byte[] buffer) {
uint ix = 0;
this.networkId = buffer[ix++];
this.thingId = buffer[ix++];
}
//public static bool Send(Participant client, CoreThing thing) {
// InvestigateMsg msg = new(thing.networkId, thing.id);
// return SendMsg(client, msg);
//}
public static async Task<bool> 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<bool> 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<bool> 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<bool> 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
}

39
ModelUrlMsg.cs → Messages/ModelUrlMsg.cs
View File

@ -1,24 +1,52 @@
#nullable enable
namespace Passer.Control.Core {
namespace Passer.RoboidControl {
/// <summary>
/// Message for communicating the URL for a model of the thing
/// </summary>
public class ModelUrlMsg : IMessage {
/// <summary>
/// The message ID
/// </summary>
public const byte Id = 0x90; // (144) Model URL
/// <summary>
/// The length of the message without th URL itself
/// </summary>
public const byte length = 4;
/// <summary>
/// The network ID of the thing
/// </summary>
public byte networkId;
/// <summary>
/// The ID of the thing
/// </summary>
public byte thingId;
public string? url = null;
/// <summary>
/// The URL of the model
/// </summary>
public string url = null;
/// <summary>
/// Create a new message for sending
/// </summary>
/// <param name="networkId">The network ID of the thing</param>
/// <param name="thing">The thing for which to send the model URL</param>
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) {
/// <summary>
/// Create a new message for sending
/// </summary>
/// <param name="networkId">The network ID of the thing</param>
/// <param name="thingId">The ID of the thing</param>
/// <param name="url">The URL to send</param>
public ModelUrlMsg(byte networkId, byte thingId, string url) {
this.networkId = networkId;
this.thingId = thingId;
this.url = url;
}
/// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer)
public ModelUrlMsg(byte[] buffer) {
byte ix = 1;
this.networkId = buffer[ix++];
@ -28,6 +56,7 @@ namespace Passer.Control.Core {
url = System.Text.Encoding.UTF8.GetString(buffer, (int)ix, strlen);
}
/// @copydoc Passer::RoboidControl::IMessage::Serialize
public override byte Serialize(ref byte[] buffer) {
if (this.url == null)
return 0;

43
Messages/NameMsg.cs
View File

@ -1,25 +1,56 @@
#nullable enable
namespace Passer.Control.Core {
namespace Passer.RoboidControl {
/// <summary>
/// Message for communicating the name of a thing
/// </summary>
public class NameMsg : IMessage {
/// <summary>
/// The message ID
/// </summary>
public const byte Id = 0x91; // 145
/// <summary>
/// The length of the message without the name string
/// </summary>
public const byte length = 4;
/// <summary>
/// The network ID of the thing
/// </summary>
public byte networkId;
/// <summary>
/// The ID of the thing
/// </summary>
public byte thingId;
/// <summary>
/// The length of the name, excluding null terminator
/// </summary>
public byte len;
public string? name = null;
/// <summary>
/// The name of the thing, not terminated with a null character
/// </summary>
public string name = "";
/// <summary>
/// Create a new message for sending
/// </summary>
/// <param name="networkId">The network ID of the thing</param>
/// <param name="thing">The thing</param>
public NameMsg(byte networkId, Thing thing) {
this.networkId = networkId;
this.thingId = thing.id;
this.name = thing.name;
}
/// <summary>
/// Create a new message for sending
/// </summary>
/// <param name="networkId">The network ID of the thing</param>
/// <param name="thingId">The ID of the thing</param>
/// <param name="name">The name of the thing</param>
public NameMsg(byte networkId, byte thingId, string name) {
this.networkId = networkId;
this.thingId = thingId;
this.name = name;
}
/// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer)
public NameMsg(byte[] buffer) {
byte ix = 1;
this.networkId = buffer[ix++];
@ -28,8 +59,9 @@ namespace Passer.Control.Core {
this.name = System.Text.Encoding.UTF8.GetString(buffer, (int)ix, strlen);
}
/// @copydoc Passer::RoboidControl::IMessage::Serialize
public override byte Serialize(ref byte[] buffer) {
if (this.name == null)
if (buffer.Length < NameMsg.length + this.name.Length || this.name.Length == 0)
return 0;
byte ix = 0;
@ -42,6 +74,7 @@ namespace Passer.Control.Core {
buffer[ix++] = (byte)nameLength;
for (int nameIx = 0; nameIx < nameLength; nameIx++)
buffer[ix++] = (byte)this.name[nameIx];
return ix;
}
}

23
Messages/NetworkIdMsg.cs
View File

@ -1,18 +1,39 @@
namespace Passer.Control.Core {
namespace Passer.RoboidControl {
/// <summary>
/// A message communicating the network ID for that participant
/// </summary>
public class NetworkIdMsg : IMessage {
/// <summary>
/// The message ID
/// </summary>
public const byte Id = 0xA1;
/// <summary>
/// The length of the message
/// </summary>
public const byte length = 2;
/// <summary>
/// The network ID for the participant
/// </summary>
public byte networkId;
/// <summary>
/// Create a new message for sending
/// </summary>
/// <param name="networkId">The network ID for the participant</param>
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;

53
Messages/ParticipantMsg.cs Normal file
View File

@ -0,0 +1,53 @@
namespace Passer.RoboidControl {
/// <summary>
/// A participant messages notifies other participants of its presence
/// </summary>
/// 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 {
/// <summary>
/// The message ID
/// </summary>
public const byte Id = 0xA0;
/// <summary>
/// The length of the message
/// </summary>
public const byte length = 2;
/// <summary>
/// The network ID known by the participant
/// </summary>
public byte networkId;
/// <summary>
/// Create a new message for sending
/// </summary>
/// <param name="networkId">The network ID known by the participant</param>
public ParticipantMsg(byte networkId) {
this.networkId = networkId;
}
/// <summary>
/// Create a message for receiving
/// </summary>
/// <param name="buffer">The byte array to parse</param>
public ParticipantMsg(byte[] buffer) {
this.networkId = buffer[1];
}
/// <summary>
/// Serialize the message into a byte array
/// </summary>
/// <param name="buffer">The buffer to serialize into</param>
/// <returns>The length of the message in the buffer</returns>
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;
}
}
}

133
Messages/PoseMsg.cs Normal file
View File

@ -0,0 +1,133 @@
using Passer.LinearAlgebra;
namespace Passer.RoboidControl {
/// <summary>
/// Message to communicate the pose of the thing
/// </summary>
/// 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 {
/// <summary>
/// The message ID
/// </summary>
public const byte Id = 0x10;
/// <summary>
/// The length of the message
/// </summary>
public const byte length = 4 + 4 + 4;
/// <summary>
/// The network ID of the thing
/// </summary>
public byte networkId;
/// <summary>
/// The ID of the thing
/// </summary>
public byte thingId;
/// <summary>
/// Bit pattern stating which pose components are available
/// </summary>
public byte poseType;
/// <summary>
/// Bit pattern for a pose with position
/// </summary>
public const byte Pose_Position = 0x01;
/// <summary>
/// Bit pattern for a pose with orientation
/// </summary>
public const byte Pose_Orientation = 0x02;
/// <summary>
/// Bit pattern for a pose with linear velocity
/// </summary>
public const byte Pose_LinearVelocity = 0x04;
/// <summary>
/// Bit pattern for a pose with angular velocity
/// </summary>
public const byte Pose_AngularVelocity = 0x08;
/// <summary>
/// The position of the thing in local space in meters
/// </summary>
public Spherical position = Spherical.zero;
/// <summary>
/// The orientation of the thing in local space
/// </summary>
public SwingTwist orientation = SwingTwist.zero;
/// <summary>
/// The linear velocity of the thing in local space in meters per second
/// </summary>
public Spherical linearVelocity = Spherical.zero;
/// <summary>
/// The angular velocity of the thing in local space
/// </summary>
public Spherical angularVelocity = Spherical.zero;
/// <summary>
/// Create a new message for sending
/// </summary>
/// <param name="networkId">The network ID of the thing</param>
/// <param name="thingId">The ID of the thing</param>
/// <param name="position">The position of the thing in local space in meters</param>
/// <param name="orientation">The orientation of the thing in local space</param>
public PoseMsg(byte networkId, byte thingId, Spherical position, SwingTwist orientation, Spherical linearVelocity = null, Spherical angularVelocity = null) {
this.networkId = networkId;
this.thingId = thingId;
this.poseType = 0;
if (this.position != null)
this.poseType |= Pose_Position;
if (this.orientation != null)
this.poseType |= Pose_Orientation;
if (this.linearVelocity != null)
this.poseType |= Pose_LinearVelocity;
if (this.angularVelocity != null)
this.poseType |= Pose_AngularVelocity;
this.position = position;
this.orientation = orientation;
this.linearVelocity = linearVelocity;
this.angularVelocity = angularVelocity;
}
/// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer)
public PoseMsg(byte[] buffer) : base(buffer) {
byte ix = 1;
this.networkId = buffer[ix++];
this.thingId = buffer[ix++];
this.poseType = buffer[ix++];
this.position = null;
this.orientation = null;
this.linearVelocity = null;
this.angularVelocity = null;
if ((this.poseType & Pose_Position) != 0)
this.position = LowLevelMessages.ReceiveSpherical(buffer, ref ix);
if ((this.poseType & Pose_Orientation) != 0)
this.orientation = SwingTwist.FromQuat32(LowLevelMessages.ReceiveQuat32(buffer, ref ix));
if ((this.poseType & Pose_LinearVelocity) != 0)
this.linearVelocity = LowLevelMessages.ReceiveSpherical(buffer, ref ix);
if ((this.poseType & Pose_AngularVelocity) != 0)
this.angularVelocity = LowLevelMessages.ReceiveSpherical(buffer, ref ix);
}
/// @copydoc Passer::RoboidControl::IMessage::Serialize
public override byte Serialize(ref byte[] buffer) {
byte ix = 0;
buffer[ix++] = PoseMsg.Id;
buffer[ix++] = this.networkId;
buffer[ix++] = this.thingId;
buffer[ix++] = this.poseType;
if ((poseType & Pose_Position) != 0)
LowLevelMessages.SendSpherical(buffer, ref ix, this.position);
if ((poseType & Pose_Orientation) != 0)
LowLevelMessages.SendQuat32(buffer, ref ix, this.orientation);
if ((poseType & Pose_LinearVelocity) != 0)
LowLevelMessages.SendSpherical(buffer, ref ix, this.linearVelocity);
if ((poseType & Pose_AngularVelocity) != 0)
LowLevelMessages.SendSpherical(buffer, ref ix, this.angularVelocity);
return ix;
}
}
}

45
Messages/TextMsg.cs Normal file
View File

@ -0,0 +1,45 @@
namespace Passer.RoboidControl {
/// <summary>
/// Message for sending generic text
/// </summary>
public class TextMsg : IMessage {
/// <summary>
/// The message ID
/// </summary>
public const byte Id = 0xB0;
/// <summary>
/// The length of the message without the text itself
/// </summary>
public const byte length = 2;
/// <summary>
/// The text
/// </summary>
public string text = "";
/// <summary>
/// Create a new message for sending
/// </summary>
/// <param name="text">The text to send</param>
public TextMsg(string text) {
this.text = text;
}
/// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer)
public TextMsg(byte[] buffer) : base(buffer) { }
/// @copydoc Passer::RoboidControl::IMessage::Serialize
public override byte Serialize(ref byte[] buffer) {
if (buffer.Length < TextMsg.length + this.text.Length || this.text.Length == 0)
return 0;
byte ix = 0;
buffer[ix++] = TextMsg.Id;
buffer[ix++] = (byte)this.text.Length;
for (int textIx = 0; textIx < this.text.Length; textIx++)
buffer[ix++] = (byte)this.text[textIx];
return ix;
}
}
}

90
Messages/ThingMsg.cs Normal file
View File

@ -0,0 +1,90 @@
namespace Passer.RoboidControl {
/// <summary>
/// Message providing generic information about a Thing
/// </summary>
public class ThingMsg : IMessage {
/// <summary>
/// The message ID
/// </summary>
public const byte id = 0x80;
/// <summary>
/// The length of the message
/// </summary>
public const byte length = 5;
/// <summary>
/// The network ID of the thing
/// </summary>
public byte networkId;
/// <summary>
/// The ID of the thing
/// </summary>
public byte thingId;
/// <summary>
/// The Thing.Type of the thing
/// </summary>
public byte thingType;
/// <summary>
/// The parent of the thing in the hierarachy. This is null for root Things
/// </summary>
public byte parentId;
/// <summary>
/// Create a message for sending
/// </summary>
/// <param name="networkId">The network ID of the thing</param>
/// <param name="thing">The thing</param>
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;
}
/// <summary>
/// Create a message for sending
/// </summary>
/// <param name="networkId">The network ID of the thing</param>
/// <param name="thingId">The ID of the thing</param>
/// <param name="thingType">The type of thing</param>
/// <param name="parentId">The parent of the thing</param>
public ThingMsg(byte networkId, byte thingId, byte thingType, byte parentId) {
this.networkId = networkId;
this.thingId = thingId;
this.thingType = thingType;
this.parentId = parentId;
}
/// <summary>
/// Create a message for receiving
/// </summary>
/// <param name="buffer">The byte array to parse</param>
public ThingMsg(byte[] buffer) {
uint ix = 1;
this.networkId = buffer[ix++];
this.thingId = buffer[ix++];
this.thingType = buffer[ix++];
this.parentId = buffer[ix];
}
/// <summary>
/// Serialize the message into a byte array
/// </summary>
/// <param name="buffer">The buffer to serialize into</param>
/// <returns>The length of the message in the bufer. 0 when the buffer was too small</returns>
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;
}
}
}

2
ModelUrlMsg.cs.meta
View File

@ -1,2 +0,0 @@
fileFormatVersion: 2
guid: 711bdb0248c9f6848a6b4da15cc05db5

98
Participant.cs
View File

@ -4,20 +4,22 @@ using System.Collections.Concurrent;
using System.Net;
using System.Net.Sockets;
namespace Passer.Control.Core {
namespace Passer.RoboidControl {
/// <summary>
/// A participant is used for communcation between things
/// </summary>
public class Participant : RemoteParticipant {
public byte[] buffer = new byte[1024];
public ulong publishInterval = 3000; // = 3 seconds
//public byte networkId = 0;
public string name = "Participant";
public IPEndPoint endPoint = null;
public UdpClient udpClient = null;
public string broadcastIpAddress = "255.255.255.255";
public readonly ConcurrentQueue<IMessage> messageQueue = new();
public readonly ConcurrentQueue<IMessage> messageQueue = new ConcurrentQueue<IMessage>();
#region Init
@ -32,9 +34,9 @@ namespace Passer.Control.Core {
/// Create a participant with the give UDP port
/// </summary>
/// <param name="port">The port number on which to communicate</param>
public Participant(int port) : this() {
this.port = port;
}
// public Participant(int port) : this() {
// this.port = port;
// }
/// <summary>
/// Create a new participant for a site at the given address and port
@ -45,10 +47,12 @@ namespace Passer.Control.Core {
this.ipAddress = ipAddress;
this.port = port;
this.endPoint = new IPEndPoint(IPAddress.Parse(ipAddress), port); // for sending
this.udpClient = new UdpClient();
this.udpClient.Client.Bind(new IPEndPoint(IPAddress.Any, port)); // local port
// this.endPoint = new IPEndPoint(IPAddress.Parse(ipAddress), port); // for sending
this.udpClient = new UdpClient(port); // for receiving
this.udpClient.Client.Bind(new IPEndPoint(IPAddress.Any, port));
// this.udpClient = new UdpClient(port); // for receiving
// this.udpClient.Client.Bind(new IPEndPoint(IPAddress.Any, port));
this.udpClient.BeginReceive(new AsyncCallback(result => ReceiveUDP(result)), null);
}
@ -62,7 +66,7 @@ namespace Passer.Control.Core {
this.port = port;
}
public List<RemoteParticipant> senders = new();
public List<RemoteParticipant> senders = new List<RemoteParticipant>();
public RemoteParticipant GetParticipant(string ipAddress, int port) {
//Console.WriteLine($"Get Participant {ipAddress}:{port}");
@ -74,18 +78,18 @@ namespace Passer.Control.Core {
}
public RemoteParticipant AddParticipant(string ipAddress, int port) {
// Console.WriteLine($"New Participant {ipAddress}:{port}");
RemoteParticipant participant = new(ipAddress, port) {
RemoteParticipant participant = new RemoteParticipant(ipAddress, port) {
networkId = (byte)this.senders.Count
};
senders.Add(participant);
return participant;
}
protected readonly Dictionary<byte, Func<byte, byte, Thing>> thingMsgProcessors = new();
protected readonly Dictionary<byte, Func<RemoteParticipant, byte, byte, Thing>> thingMsgProcessors = new Dictionary<byte, Func<RemoteParticipant, byte, byte, Thing>>();
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<byte, byte, Thing>(constr);
thingMsgProcessors[thingType] = new Func<RemoteParticipant, byte, byte, Thing>(constr);
}
public void Register<ThingClass>(Thing.Type thingType) where ThingClass : Thing {
@ -93,8 +97,8 @@ namespace Passer.Control.Core {
}
public void Register<ThingClass>(byte thingType) where ThingClass : Thing {
thingMsgProcessors[thingType] = (byte networkId, byte thingId) =>
Activator.CreateInstance(typeof(ThingClass), networkId, thingId) as ThingClass;
thingMsgProcessors[thingType] = (RemoteParticipant sender, byte networkId, byte thingId) =>
Activator.CreateInstance(typeof(ThingClass), sender, networkId, thingId) as ThingClass;
Console.WriteLine($"Registering {typeof(ThingClass)} for thing type {thingType}");
}
@ -103,21 +107,24 @@ namespace Passer.Control.Core {
#region Update
protected void ReceiveUDP(IAsyncResult result) {
if (udpClient == null || this.endPoint == null)
if (this.udpClient == null || this.endPoint == null)
return;
byte[] data = udpClient.EndReceive(result, ref this.endPoint);
byte[] data = this.udpClient.EndReceive(result, ref this.endPoint);
// This does not yet take multi-packet messages into account!
if (this.endPoint == null)
return;
// We can receive our own publish (broadcast) packages. How do we recognize them????
// It is hard to determine our source port
RemoteParticipant remoteParticipant = this.GetParticipant(endPoint.Address.ToString(), endPoint.Port);
string ipAddress = this.endPoint.Address.ToString();
RemoteParticipant remoteParticipant = GetParticipant(ipAddress, this.endPoint.Port);
if (remoteParticipant == null)
remoteParticipant = this.AddParticipant(endPoint.Address.ToString(), endPoint.Port);
remoteParticipant = AddParticipant(ipAddress, this.endPoint.Port);
ReceiveData(data, remoteParticipant);
udpClient.BeginReceive(new AsyncCallback(result => ReceiveUDP(result)), null);
udpClient.BeginReceive(new AsyncCallback(callbackResult => ReceiveUDP(callbackResult)), null);
}
protected ulong nextPublishMe = 0;
@ -143,7 +150,7 @@ namespace Passer.Control.Core {
}
public virtual void Publish() {
this.Publish(new ClientMsg(this.networkId));
this.Publish(new ParticipantMsg(this.networkId));
}
#endregion Update
@ -155,7 +162,7 @@ namespace Passer.Control.Core {
this.Send(remoteParticipant, new ThingMsg(this.networkId, thing));
this.Send(remoteParticipant, new NameMsg(this.networkId, thing));
this.Send(remoteParticipant, new ModelUrlMsg(this.networkId, thing));
this.Send(remoteParticipant, new CustomMsg(this.networkId, thing));
this.Send(remoteParticipant, new BinaryMsg(this.networkId, thing));
}
public bool Send(IMessage msg) {
@ -173,7 +180,7 @@ namespace Passer.Control.Core {
if (bufferSize <= 0)
return true;
IPEndPoint participantEndpoint = new(IPAddress.Parse(remoteParticipant.ipAddress), remoteParticipant.port);
IPEndPoint participantEndpoint = new IPEndPoint(IPAddress.Parse(remoteParticipant.ipAddress), remoteParticipant.port);
Console.WriteLine($"msg to {participantEndpoint.Address.ToString()} {participantEndpoint.Port}");
this.udpClient?.Send(this.buffer, bufferSize, participantEndpoint);
return true;
@ -184,7 +191,7 @@ namespace Passer.Control.Core {
this.Publish(new ThingMsg(this.networkId, thing));
this.Publish(new NameMsg(this.networkId, thing));
this.Publish(new ModelUrlMsg(this.networkId, thing));
this.Publish(new CustomMsg(this.networkId, thing));
this.Publish(new BinaryMsg(this.networkId, thing));
}
public bool Publish(IMessage msg) {
@ -227,8 +234,8 @@ namespace Passer.Control.Core {
}
switch (msgId) {
case ClientMsg.Id: // 0xA0 / 160
this.Process(remoteParticipant, new ClientMsg(data));
case ParticipantMsg.Id: // 0xA0 / 160
this.Process(remoteParticipant, new ParticipantMsg(data));
break;
case NetworkIdMsg.Id: // 0xA1 / 161
this.Process(remoteParticipant, new NetworkIdMsg(data));
@ -246,10 +253,11 @@ namespace Passer.Control.Core {
this.Process(remoteParticipant, new ModelUrlMsg(data));
break;
case PoseMsg.Id: // 0x10 / 16
this.Process(remoteParticipant, new PoseMsg(data));
// result = await PoseMsg.Receive(dataStream, client, packetSize);
break;
case CustomMsg.Id: // 0xB1 / 177
this.Process(remoteParticipant, new CustomMsg(data));
case BinaryMsg.Id: // 0xB1 / 177
this.Process(remoteParticipant, new BinaryMsg(data));
break;
case TextMsg.Id: // 0xB0 / 176
// result = await TextMsg.Receive(dataStream, client, packetSize);
@ -266,7 +274,7 @@ namespace Passer.Control.Core {
#region Process
protected virtual void Process(RemoteParticipant sender, ClientMsg msg) { }
protected virtual void Process(RemoteParticipant sender, ParticipantMsg msg) { }
protected virtual void Process(RemoteParticipant sender, NetworkIdMsg msg) {
Console.WriteLine($"{this.name} receive network id {this.networkId} {msg.networkId}");
@ -292,11 +300,33 @@ namespace Passer.Control.Core {
protected virtual void Process(RemoteParticipant sender, ModelUrlMsg msg) {
Console.WriteLine($"Participant: Process model [{msg.networkId}/{msg.thingId}] {msg.url}");
Thing thing = sender.Get(msg.networkId, msg.thingId);
if (thing != null)
thing.modelUrl = msg.url;
}
protected virtual void Process(PoseMsg msg) { }
protected virtual void Process(RemoteParticipant sender, PoseMsg msg) {
//Console.WriteLine($"Participant: Process pose [{msg.networkId}/{msg.thingId}] {msg.poseType}");
Thing thing = sender.Get(msg.networkId, msg.thingId);
if (thing != null) {
thing.hasPosition = false;
if ((msg.poseType & PoseMsg.Pose_Position) != 0) {
thing.position = msg.position;
thing.hasPosition = true;
}
if ((msg.poseType & PoseMsg.Pose_Orientation) != 0)
thing.orientation = msg.orientation;
else
thing.orientation = null;
if ((msg.poseType & PoseMsg.Pose_LinearVelocity) != 0)
thing.linearVelocity = msg.linearVelocity;
if ((msg.poseType & PoseMsg.Pose_AngularVelocity) != 0)
thing.angularVelocity = msg.angularVelocity;
protected virtual void Process(RemoteParticipant sender, CustomMsg msg) {
}
}
protected virtual void Process(RemoteParticipant sender, BinaryMsg msg) {
// Console.WriteLine($"Participant: Process binary [{msg.networkId}/{msg.thingId}]");
Thing thing = sender.Get(msg.networkId, msg.thingId);
thing?.ProcessBinary(msg.bytes);
@ -311,7 +341,9 @@ namespace Passer.Control.Core {
if (client == this)
continue;
//UnityEngine.Debug.Log($"---> {client.ipAddress}");
IMessage.SendMsg(client, msg);
//IMessage.SendMsg(client, msg);
msg.Serialize(ref client.buffer);
client.SendBuffer(client.buffer.Length);
}
}

11
README.md
View File

@ -1,3 +1,10 @@
\mainpage Control Core for C#
\mainpage Roboid Control for C#
Control Core contains generic functionality for Controlling Things.
Roboid Control support for C# applications.
Includes support for the Unity game engine.
# Basic components
- Passer::RoboidControl::Thing
- Passer::RoboidControl::Participant
- Passer::RoboidControl::SiteServer

42
RemoteParticipant.cs
View File

@ -1,24 +1,51 @@
using System;
using System.Collections.Generic;
namespace Passer.Control.Core {
namespace Passer.RoboidControl {
/// <summary>
/// A reference to a participant, possibly on a remote location
/// </summary>
public class RemoteParticipant {
/// <summary>
/// The internet address of the participant
/// </summary>
public string ipAddress = "0.0.0.0";
/// <summary>
/// The UDP port on which the participant can be reached
/// </summary>
public int port = 0;
/// <summary>
/// The network ID of the participant
/// </summary>
public byte networkId;
public RemoteParticipant() {
}
/// <summary>
/// Default constructor
/// </summary>
public RemoteParticipant() {}
/// <summary>
/// Create a new remote participant
/// </summary>
/// <param name="ipAddress">The IP address of the participant</param>
/// <param name="port">The UDP port of the participant</param>
public RemoteParticipant(string ipAddress, int port) {
this.ipAddress = ipAddress;
this.port = port;
}
protected readonly List<Thing> things = new();
/// <summary>
/// The things reported by this participant
/// </summary>
protected readonly List<Thing> things = new List<Thing>();
/// <summary>
/// Get a thing with the given ids
/// </summary>
/// <param name="networkId">The network ID of the thing</param>
/// <param name="thingId">The ID of the thing</param>
/// <returns>The thing when it is found, null in other cases.</returns>
public Thing Get(byte networkId, byte thingId) {
Thing thing = things.Find(aThing => Thing.IsThing(aThing, networkId, thingId));
// if (thing == null)
@ -26,6 +53,11 @@ namespace Passer.Control.Core {
return thing;
}
/// <summary>
/// Add a new thing for this participant
/// </summary>
/// <param name="thing">The thing to add</param>
/// <param name="invokeEvent">Invoke an notification event when the thing has been added</param>
public void Add(Thing thing, bool invokeEvent = true) {
// Console.WriteLine($"added thing [{thing.networkId}/{thing.id}]");
Thing foundThing = Get(thing.networkId, thing.id);

8
Sensors.meta
View File

@ -1,8 +0,0 @@
fileFormatVersion: 2
guid: a5a7a42365df0d0459195576ad3bb50b
folderAsset: yes
DefaultImporter:
externalObjects: {}
userData:
assetBundleName:
assetBundleVariant:

33
Sensors/DistanceSensor.cs
View File

@ -1,22 +1,39 @@
using System;
namespace Passer.Control.Core {
namespace Passer.RoboidControl {
/// <summary>
/// A sensor measuring the distance in the forward direction
/// </summary>
public class DistanceSensor : Thing {
/// <summary>
/// The current measured distance
/// </summary>
public float distance = 0;
public DistanceSensor() : base(true) { }
public DistanceSensor(byte networkId, byte thingId) : base(null, networkId, thingId, (byte)Type.TemperatureSensor) {
/// <summary>
/// Constructor for a new distance sensor
/// </summary>
/// <param name="participant">The participant for which the sensor is needed</param>
public DistanceSensor(RemoteParticipant participant) : base(participant, true) { }
/// <summary>
/// Create a distance sensor with the given ID
/// </summary>
/// <param name="participant">The participant for with the sensor is needed</param>
/// <param name="networkId">The network ID of the sensor</param>
/// <param name="thingId">The ID of the thing</param>
public DistanceSensor(RemoteParticipant participant, byte networkId, byte thingId) : base(participant, networkId, thingId, (byte)Type.TemperatureSensor) {
}
#if UNITY_5_3_OR_NEWER
/// @copydoc Passer::RoboidControl::Thing::CreateComponent
public override void CreateComponent() {
this.component = Unity.DistanceSensor.Create(this.parent);
this.component = Unity.DistanceSensor.Create(this);
this.component.core = this;
}
#endif
/// <summary>
/// Function to extract the distance received in the binary message
/// </summary>
/// <param name="bytes">The byte array</param>
public override void ProcessBinary(byte[] bytes) {
byte ix = 0;
this.distance = LowLevelMessages.ReceiveFloat16(bytes, ref ix);

29
Sensors/TemperatureSensor.cs
View File

@ -1,17 +1,32 @@
using System;
//using System;
namespace Passer.Control.Core {
namespace Passer.RoboidControl {
/// <summary>
/// A temperature sensor
/// </summary>
public class TemperatureSensor : Thing {
public float temp = 0;
/// <summary>
/// The measured temperature
/// </summary>
public float temperature = 0;
public TemperatureSensor(byte networkId, byte thingId) : base(null, networkId, thingId, (byte)Type.TemperatureSensor) {
}
/// <summary>
/// Create a temperature sensor with the given ID
/// </summary>
/// <param name="participant">The participant for with the sensor is needed</param>
/// <param name="networkId">The network ID of the sensor</param>
/// <param name="thingId">The ID of the thing</param>
public TemperatureSensor(Participant participant, byte networkId, byte thingId) : base(participant, networkId, thingId, (byte)Type.TemperatureSensor) {}
/// <summary>
/// Function to extract the temperature received in the binary message
/// </summary>
/// <param name="bytes">The byte array</param>
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");
}
}

2
Sensors/TemperatureSensor.cs.meta
View File

@ -1,2 +0,0 @@
fileFormatVersion: 2
guid: 5cd9e2534695ec844bd23b7a48ad498a

34
Sensors/TouchSensor.cs Normal file
View File

@ -0,0 +1,34 @@
namespace Passer.RoboidControl {
/// <summary>
/// A sensor which can detect touches
/// </summary>
public class TouchSensor : Thing {
/// <summary>
/// Value which is true when the sensor is touching something, false otherwise
/// </summary>
public bool touchedSomething = false;
/// <summary>
/// Create a touch sensor
/// </summary>
/// <param name="participant">The participant for with the sensor is needed</param>
/// <param name="invokeEvent">True when the creation should trigger an event</param>
public TouchSensor(RemoteParticipant participant, bool invokeEvent = true) : base(participant, invokeEvent) {
touchedSomething = false;
}
public TouchSensor(RemoteParticipant participant, byte networkId, byte thingId) : base(participant, networkId, thingId) {
touchedSomething = false;
}
#if UNITY_5_3_OR_NEWER
/// @copydoc Passer::RoboidControl::Thing::CreateComponent
public override void CreateComponent() {
System.Console.Write("Create touch sensor component");
this.component = Unity.TouchSensor.Create(this);
this.component.core = this;
}
#endif
}
}

48
SiteServer.cs
View File

@ -1,16 +1,24 @@
using System;
using System.Collections.Generic;
using System.Net;
using System.Net.Sockets;
namespace Passer.Control.Core {
namespace Passer.RoboidControl {
/// <summary>
/// A site server is a participant which provides a shared simulated environment
/// </summary>
public class SiteServer : Participant {
public SiteServer(int port = 7681) {
public SiteServer(int port = 7681) : this("0.0.0.0", port) { }
/// <summary>
/// Create a new site server
/// </summary>
/// <param name="port"></param>
public SiteServer(string ipAddress = "0.0.0.0", int port = 7681) : base() {
this.name = "Site Server";
this.ipAddress = "0.0.0.0";
this.ipAddress = ipAddress;
this.port = port;
this.endPoint = new IPEndPoint(IPAddress.Parse(ipAddress), port); // for sending
@ -20,16 +28,21 @@ namespace Passer.Control.Core {
this.udpClient.BeginReceive(
new AsyncCallback(result => ReceiveUDP(result)),
new Tuple<UdpClient, IPEndPoint>(this.udpClient, new(IPAddress.Any, port)));
Register<TouchSensor>(Thing.Type.TouchSensor);
}
/// <summary>
/// Close the site
/// </summary>
public void Close() {
this.udpClient.Close();
this.udpClient?.Close();
}
public override void Publish() {
}
protected override void Process(RemoteParticipant sender, ClientMsg msg) {
protected override void Process(RemoteParticipant sender, ParticipantMsg msg) {
if (msg.networkId == 0) {
Console.WriteLine($"{this.name} received New Client -> {sender.networkId}");
this.Send(sender, new NetworkIdMsg(sender.networkId));
@ -39,14 +52,27 @@ namespace Passer.Control.Core {
protected override void Process(RemoteParticipant sender, NetworkIdMsg msg) { }
protected override void Process(RemoteParticipant sender, ThingMsg msg) {
//Console.WriteLine($"SiteServer: Process thing [{msg.networkId}/{msg.thingId}]");
Console.WriteLine($"SiteServer: Process thing [{msg.networkId}/{msg.thingId}]");
Thing thing = sender.Get(msg.networkId, msg.thingId);
if (thing == null) {
Thing newThing;
if (thingMsgProcessors.ContainsKey(msg.thingType))
newThing = thingMsgProcessors[msg.thingType](msg.networkId, msg.thingId);
else
Thing newThing = null;
if (thingMsgProcessors.TryGetValue(msg.thingType, out Func<RemoteParticipant, byte, byte, Thing> value)) {
// Console.WriteLine("Found thing message processor");
if (value != null)
newThing = value(sender, msg.networkId, msg.thingId);
}
if (newThing == null) {
newThing = new Thing(sender, msg.networkId, msg.thingId, msg.thingType);
Console.WriteLine("Created generic new core thing");
}
if (msg.parentId != 0) {
Thing parentThing = Get(msg.networkId, msg.parentId);
if (parentThing == null)
Console.WriteLine("Could not find parent");
else
newThing.parent = parentThing;
}
sender.Add(newThing);
}
}

224
Thing.cs
View File

@ -2,23 +2,49 @@ using System;
using System.Collections.Generic;
using Passer.LinearAlgebra;
namespace Passer.Control.Core {
namespace Passer.RoboidControl {
/// <summary>
/// A thing is the basic building block
/// A thing is the primitive building block
/// </summary>
[Serializable]
public class Thing {
#region Types
#endregion Types
#region Properties
public delegate void ChangeHandler();
public delegate void SphericalHandler(Spherical v);
public delegate void ThingHandler(Thing t);
/// <summary>
/// The participant to which this thing belongs
/// </summary>
public RemoteParticipant participant;
/// <summary>
/// The network ID of this thing.
/// </summary>
public byte networkId;
/// <summary>
/// The ID of this thing
/// </summary>
public byte id;
/// <summary>
/// Predefined thing types
/// </summary>
public enum Type {
Undeterment,
Undetermined,
// Sensor
Switch,
DistanceSensor,
DirectionalSensor,
TemperatureSensor,
TouchSensor,
// Motor
ControlledMotor,
UncontrolledMotor,
@ -28,21 +54,20 @@ namespace Passer.Control.Core {
Humanoid,
ExternalSensor
};
/// <summary>
/// The type of this thing. This can be either a Thing::Type (needs casting)
/// or a byte value for custom types.
/// </summary>
public byte type;
#endregion Types
#region Properties
public RemoteParticipant participant;
public delegate void ChangeHandler();
public delegate void SphericalHandler(Spherical v);
public byte networkId;
public byte id;
public event ChangeHandler OnParentChanged;
/// <summary>
/// Event which is triggered when the parent changes
/// </summary>
public event ChangeHandler OnParentChanged = delegate { };
private Thing _parent;
/// <summary>
/// The parent of this thing
/// </summary>
public Thing parent {
get => _parent;
set {
@ -50,7 +75,7 @@ namespace Passer.Control.Core {
return;
if (value == null) {
_parent.RemoveChild(this);
_parent?.RemoveChild(this);
_parent = null;
}
else {
@ -60,22 +85,38 @@ namespace Passer.Control.Core {
}
}
/// <summary>
/// Attach a thing as a child of this thing
/// </summary>
/// <param name="child">The thing to attach as a child</param>
public void AddChild(Thing child) {
if (children.Find(thing => thing == child) != null)
return;
child._parent = this;
children.Add(child);
}
/// <summary>
/// Remove the given thing as a child of this thing
/// </summary>
/// <param name="child">The child to remove</param>
public void RemoveChild(Thing child) {
children.Remove(child);
}
[System.NonSerialized]
/// <summary>
/// The list of children of this thing
/// </summary>
[NonSerialized]
public List<Thing> children = new List<Thing>();
public byte type;
public event ChangeHandler OnNameChanged;
private string _name;
/// <summary>
/// Event which is triggered when the name changes
/// </summary>
public event ChangeHandler OnNameChanged = delegate { };
private string _name = "";
/// <summary>
/// The name of the thing
/// </summary>
public virtual string name {
get => _name;
set {
@ -86,11 +127,19 @@ namespace Passer.Control.Core {
}
}
public string modelUrl;
/// <summary>
/// An URL pointing to the location where a model of the thing can be found
/// </summary>
public string modelUrl = "";
public byte poseUpdated = 0x00;
public event ChangeHandler OnPositionChanged;
private Spherical _position;
/// <summary>
/// Event triggered when the position has changed
/// </summary>
public event ChangeHandler OnPositionChanged = delegate { };
private Spherical _position = Spherical.zero;
/// <summary>
/// The position of the thing in local space, in meters.
/// </summary>
public Spherical position {
get { return _position; }
set {
@ -100,9 +149,16 @@ namespace Passer.Control.Core {
}
}
}
public bool hasPosition = false;
public event ChangeHandler OnOrientationChanged;
private SwingTwist _orientation;
/// <summary>
/// Event triggered when the orientation has changed
/// </summary>
public event ChangeHandler OnOrientationChanged = delegate { };
private SwingTwist _orientation = SwingTwist.zero;
/// <summary>
/// The orientation of the thing in local space
/// </summary>
public SwingTwist orientation {
get { return _orientation; }
set {
@ -113,8 +169,14 @@ namespace Passer.Control.Core {
}
}
public event SphericalHandler OnLinearVelocityChanged;
private Spherical _linearVelocity;
/// <summary>
/// Event triggered when the linear velocity has changed
/// </summary>
public event SphericalHandler OnLinearVelocityChanged = delegate { };
private Spherical _linearVelocity = Spherical.zero;
/// <summary>
/// The linear velocity of the thing in local space in meters per second
/// </summary>
public Spherical linearVelocity {
get => _linearVelocity;
set {
@ -124,85 +186,113 @@ namespace Passer.Control.Core {
}
}
}
public Spherical angularVelocity;
/// <summary>
/// The angular velocity of the thing in local space
/// </summary>
public Spherical angularVelocity = Spherical.zero;
#if UNITY_5_3_OR_NEWER
/// <summary>
/// A reference to the representation of the thing in Unity
/// </summary>
[NonSerialized]
public Unity.Thing component;
public Unity.Thing component = null;
#endif
#endregion Properties
#region Init
// public virtual void Init(bool invokeEvent = false) {
// if (invokeEvent)
// InvokeNewThing(this);
// }
public Thing(bool invokeEvent = false) {
/// <summary>
/// Create a new thing for the given participant
/// </summary>
/// <param name="participant">The participant for which this thing is created</param>
/// <param name="invokeEvent">True when a new thing event should be triggered</param>
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;
/// <summary>
/// Create a new thing for the given participant
/// </summary>
/// <param name="participant">The participant for which this thing is created</param>
/// <param name="networkId">The network ID of the thing</param>
/// <param name="thingId">The ID of the thing</param>
/// <param name="thingType">The type of thing</param>
public Thing(RemoteParticipant participant, byte networkId, byte thingId, byte thingType = 0) {
this.participant = participant;
this.id = thingId;
this.type = thingType;
this.networkId = networkId;
//this.Init();
//OnNewThing?.Invoke(this);
//Thing.Add(this);
}
public virtual void CreateComponent() {}
/// <summary>
/// Function which can be used to create components in external engines.
/// </summary>
/// Currently this is used to create GameObjects in Unity
public virtual void CreateComponent() {
#if UNITY_5_3_OR_NEWER
this.component = Unity.Thing.Create(this);
this.component.core = this;
#endif
}
#endregion Init
#region Update
#if UNITY_5_3_OR_NEWER
/// <summary>
/// Convience function for use in Unity which removes the need for a currentTime argument
/// </summary>
public void Update() {
Update((ulong)UnityEngine.Time.time * 1000);
}
#endif
/// <summary>
/// Update this thing
/// </summary>
/// <param name="currentTime">The current time in milliseconds</param>
public virtual void Update(ulong currentTime) {
// should recurse over children...
}
/// <summary>
/// Function used to generate binary data for this thing
/// </summary>
/// <returns>a byte array with the binary data</returns>
/// @sa Passer::RoboidControl::BinaryMsg
public virtual byte[] GenerateBinary() { return new byte[0]; }
/// <summary>
/// Function used to process binary data received for this thing
/// </summary>
/// <param name="bytes">The binary data</param>
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<Thing> allThings = new();
public delegate void ThingHandler(Thing t);
public static event ThingHandler OnNewThing;
/// <summary>
/// Event triggered when a new thing has been created
/// </summary>
public static event ThingHandler OnNewThing = delegate { };
/// <summary>
/// Trigger the creation for the given thing
/// </summary>
/// <param name="thing">The created thing</param>
public static void InvokeNewThing(Thing thing) {
OnNewThing?.Invoke(thing);
OnNewThing?.Invoke(thing);
}
/// <summary>
/// Check if the thing has the given properaties
/// </summary>
/// <param name="thing">The thing to check</param>
/// <param name="networkId">The network ID to compare to</param>
/// <param name="thingId">The thing ID to compare to</param>
/// <returns>True when the thing has the given properties</returns>
public static bool IsThing(Thing thing, byte networkId, byte thingId) {
if (thing == null)
return false;

45
ThingMsg.cs
View File

@ -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;
}
}
}

2
ThingMsg.cs.meta
View File

@ -1,2 +0,0 @@
fileFormatVersion: 2
guid: 718e148be3eeb65498334ed008747482

2
Unity/DebugConsole.cs
View File

@ -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) {

36
Unity/DistanceSensor.cs
View File

@ -2,31 +2,51 @@
using System.Collections;
using UnityEngine;
namespace Passer.Control.Unity {
namespace Passer.RoboidControl.Unity {
/// <summary>
/// The Unity representation of a distance sensor
/// </summary>
public class DistanceSensor : Thing {
public new Core.DistanceSensor core {
get => (Core.DistanceSensor)base.core;
/// <summary>
/// The core distance sensor
/// </summary>
public new RoboidControl.DistanceSensor core {
get => (RoboidControl.DistanceSensor)base.core;
set => base.core = value;
}
/// <summary>
/// Start the Unity representation
/// </summary>
protected virtual void Start() {
if (core == null)
SetCoreThing(new Core.DistanceSensor());
if (core == null) {
SiteServer siteServer = FindAnyObjectByType<SiteServer>();
SetCoreThing(new RoboidControl.DistanceSensor(siteServer.site));
}
StartCoroutine(MeasureDistance());
}
public static DistanceSensor Create(Core.Thing parent) {
/// <summary>
/// Create the Unity representation of the distance sensor
/// </summary>
/// <param name="parent">The parent of the core distance sensor</param>
/// <returns>The Unity representation of the distance sensor</returns>
public static DistanceSensor Create(RoboidControl.DistanceSensor core) {
GameObject distanceObj = new("Distance sensor");
DistanceSensor component = distanceObj.AddComponent<DistanceSensor>();
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;
}
/// <summary>
/// Periodically measure the distance
/// </summary>
/// <returns></returns>
IEnumerator MeasureDistance() {
while (Application.isPlaying) {
if (Physics.Raycast(this.transform.position, this.transform.forward, out RaycastHit hitInfo, 2.0f)) {

16
Unity/SiteServer.cs
View File

@ -3,34 +3,36 @@ using System;
using System.Collections.Generic;
using UnityEngine;
namespace Passer.Control.Unity {
namespace Passer.RoboidControl.Unity {
public class SiteServer : MonoBehaviour {
public Core.SiteServer site;
public RoboidControl.SiteServer site;
public Queue<Core.Thing> thingQueue = new();
public Queue<RoboidControl.Thing> thingQueue = new();
protected virtual void Awake() {
Console.SetOut(new UnityLogWriter());
site = new(7681);
Core.Thing.OnNewThing += HandleNewThing;
RoboidControl.Thing.OnNewThing += HandleNewThing;
//site.Register<RoboidControl.TouchSensor>(RoboidControl.Thing.Type.TouchSensor);
}
void OnApplicationQuit() {
site.Close();
}
public void HandleNewThing(Core.Thing thing) {
public void HandleNewThing(RoboidControl.Thing thing) {
// Debug.Log("Handle New thing event");
site.Add(thing, false);
thingQueue.Enqueue(thing);
}
protected virtual void Update() {
site.Update((ulong)(Time.time * 1000));
if (thingQueue.TryDequeue(out Core.Thing thing)) {
while (thingQueue.TryDequeue(out RoboidControl.Thing thing))
thing.CreateComponent();
}
}
}

84
Unity/Thing.cs
View File

@ -1,14 +1,27 @@
#if UNITY_5_3_OR_NEWER
using System.Collections;
using UnityEngine;
using UnityEngine.Networking;
namespace Passer.Control.Unity {
namespace Passer.RoboidControl.Unity {
/// <summary>
/// The representation of a Thing in Unity
/// </summary>
public class Thing : MonoBehaviour {
/// <summary>
/// The core C# thing
/// </summary>
[field: SerializeField]
public Core.Thing core {get; set; }
public RoboidControl.Thing core { get; set; }
protected void SetCoreThing(Core.Thing thing) {
private string modelUrl = null;
/// <summary>
/// Set the core C# thing
/// </summary>
protected void SetCoreThing(RoboidControl.Thing thing) {
core = thing;
core.component = this;
@ -18,21 +31,82 @@ namespace Passer.Control.Unity {
return;
}
siteServer.site.Add(thing);
}
public static Thing Create(RoboidControl.Thing core) {
Debug.Log("Creating new Unity thing");
GameObject gameObj = string.IsNullOrEmpty(core.name) ?
new("Thing") :
new(core.name);
Thing component = gameObj.AddComponent<Thing>();
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;
}
/// <summary>
/// Update the Unity representation
/// </summary>
protected virtual void Update() {
if (core == null)
return;
if (core.linearVelocity != null) {
Vector3 direction = Quaternion.AngleAxis(core.linearVelocity.direction.horizontal, Vector3.up) * Vector3.forward;
this.transform.Translate(core.linearVelocity.distance * Time.deltaTime * direction);
this.transform.Translate(core.linearVelocity.distance * Time.deltaTime * direction, Space.Self);
}
if (core.angularVelocity != null) {
Vector3 angularVelocity = core.angularVelocity.ToVector3();
this.transform.rotation *= Quaternion.Euler(angularVelocity * Time.deltaTime);
this.transform.localRotation *= Quaternion.Euler(angularVelocity * Time.deltaTime);
}
if (core.hasPosition)
this.transform.localPosition = core.position.ToVector3();
//this.transform.localRotation = core.orientation.ToQuaternion();
if (!string.IsNullOrEmpty(core.modelUrl) && this.modelUrl == null) {
string extension = core.modelUrl.Substring(core.modelUrl.LastIndexOf("."));
if (extension == ".jpg" || extension == ".png") {
StartCoroutine(LoadJPG());
}
this.modelUrl = core.modelUrl;
}
}
private IEnumerator LoadJPG() {
UnityWebRequest request = UnityWebRequestTexture.GetTexture(core.modelUrl);
yield return request.SendWebRequest();
if (request.result == UnityWebRequest.Result.Success) {
Texture2D texture = ((DownloadHandlerTexture)request.downloadHandler).texture;
float aspectRatio = (float)texture.width / (float)texture.height;
GameObject modelQuad = GameObject.CreatePrimitive(PrimitiveType.Quad);
Collider c = modelQuad.GetComponent<Collider>();
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<Renderer>().material = quadMaterial;
}
else {
Debug.LogError("Failed to load image: " + request.error);
}
}
}
}

72
Unity/TouchSensor.cs Normal file
View File

@ -0,0 +1,72 @@
#if UNITY_5_3_OR_NEWER
using UnityEngine;
namespace Passer.RoboidControl.Unity {
/// <summary>
/// The Unity representation of the TouchSensor
/// </summary>
public class TouchSensor : Thing {
/// <summary>
/// The core touch sensor
/// </summary>
public RoboidControl.TouchSensor coreSensor {
get => (RoboidControl.TouchSensor)base.core;
}
/// <summary>
/// Start the Unity represention
/// </summary>
protected virtual void Start() {
if (core == null) {
SiteServer siteServer = FindAnyObjectByType<SiteServer>();
SetCoreThing(new RoboidControl.TouchSensor(siteServer.site));
}
}
/// <summary>
/// Create the Unity representation
/// </summary>
/// <param name="core">The core touch sensor</param>
/// <returns>The Unity representation of the touch sensor</returns>
public static TouchSensor Create(RoboidControl.TouchSensor core) {
GameObject gameObj = core.name != null ?
new(core.name) :
new("Touch Sensor");
TouchSensor component = gameObj.AddComponent<TouchSensor>();
Rigidbody rb = gameObj.AddComponent<Rigidbody>();
rb.isKinematic = true;
SphereCollider collider = gameObj.AddComponent<SphereCollider>();
collider.radius = 0.01F;
collider.isTrigger = true;
component.core = core;
if (core.parent != null && core.parent.component != null)
gameObj.transform.SetParent(core.parent.component.transform, false);
if (core.position != null)
gameObj.transform.localPosition = core.position.ToVector3();
return component;
}
private void OnTriggerEnter(Collider other) {
if (other.isTrigger)
return;
if (this.transform.root == other.transform.root)
return;
// Debug.Log($"touched {other.gameObject.name}");
this.coreSensor.touchedSomething = true;
}
private void OnTriggerExit(Collider other) {
if (other.isTrigger)
return;
this.coreSensor.touchedSomething = false;
}
}
}
#endif

2
float16.cs.meta
View File

@ -1,2 +0,0 @@
fileFormatVersion: 2
guid: 620fef383ba64a44995a234a71b2f189

7
test.sln.meta
View File

@ -1,7 +0,0 @@
fileFormatVersion: 2
guid: d5d87461365fd8a4da528aa84a49e62c
DefaultImporter:
externalObjects: {}
userData:
assetBundleName:
assetBundleVariant:

16
test/UnitTest1.cs
View File

@ -3,7 +3,7 @@ using System;
using System.Threading;
using NUnit.Framework;
using Passer.Control.Core;
using Passer.RoboidControl;
namespace ControlCore.test {
public class Tests {
@ -13,7 +13,7 @@ namespace ControlCore.test {
[Test]
public void Test_Participant() {
Participant participant = new("127.0.0.1", 7681);
Participant participant = new Participant("127.0.0.1", 7682);
ulong milliseconds = (ulong)DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();
ulong startTime = milliseconds;
@ -29,7 +29,7 @@ namespace ControlCore.test {
[Test]
public void Test_SiteServer() {
SiteServer siteServer = new(7681);
SiteServer siteServer = new SiteServer(7681);
ulong milliseconds = (ulong)DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();
ulong startTime = milliseconds;
@ -45,8 +45,8 @@ namespace ControlCore.test {
[Test]
public void Test_SiteParticipant() {
SiteServer siteServer = new(7681);
Participant participant = new("127.0.0.1", 7681);
SiteServer siteServer = new SiteServer(7681);
Participant participant = new Participant("127.0.0.1", 7681);
ulong milliseconds = (ulong)DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();
ulong startTime = milliseconds;
@ -63,9 +63,9 @@ namespace ControlCore.test {
[Test]
public void Test_ThingMsg() {
SiteServer siteServer = new();
Participant participant = new("127.0.0.1");
Thing thing = new() {
SiteServer siteServer = new SiteServer();
Participant participant = new Participant("127.0.0.1");
Thing thing = new Thing(participant) {
name = "First Thing",
modelUrl = "https://passer.life/extras/ant.jpg"
};

2
test/UnitTest1.cs.meta
View File

@ -1,2 +0,0 @@
fileFormatVersion: 2
guid: 743b128a79ef8414fa29d7bb3b9e2ac8

17
test/test.csproj
View File

@ -1,24 +1,15 @@
<Project Sdk="Microsoft.NET.Sdk">
<PropertyGroup>
<TargetFramework>net9.0</TargetFramework>
<LangVersion>latest</LangVersion>
<ImplicitUsings>enable</ImplicitUsings>
<Nullable>enable</Nullable>
<TargetFramework>net5.0</TargetFramework>
<IsPackable>false</IsPackable>
<IsTestProject>true</IsTestProject>
</PropertyGroup>
<ItemGroup>
<PackageReference Include="coverlet.collector" Version="6.0.2" />
<PackageReference Include="Microsoft.NET.Test.Sdk" Version="17.12.0" />
<PackageReference Include="NUnit" Version="3.13.3" />
<PackageReference Include="NUnit.Analyzers" Version="4.3.0" />
<PackageReference Include="NUnit3TestAdapter" Version="4.6.0" />
</ItemGroup>
<ItemGroup>
<Using Include="NUnit.Framework" />
<PackageReference Include="Microsoft.NET.Test.Sdk" Version="17.13.0" />
<PackageReference Include="NUnit" Version="3.13.2" />
<PackageReference Include="NUnit3TestAdapter" Version="3.17.0" />
</ItemGroup>
<ItemGroup>

7
test/test.csproj.meta
View File

@ -1,7 +0,0 @@
fileFormatVersion: 2
guid: 92729868a8379c04197dcb80d0276a63
DefaultImporter:
externalObjects: {}
userData:
assetBundleName:
assetBundleVariant: