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

Cleanup

Browse Source
This commit is contained in:
Pascal Serrarens 2025-01-14 09:10:27 +01:00
parent dc165edf79
commit 97afb3b730
28 changed files with 25 additions and 627 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

2
ClientMsg.cs.meta
View File

@ -1,2 +0,0 @@
fileFormatVersion: 2
guid: f73ac1131d4d7bd4a8f5b62500069867

7
ControlCore.csproj.meta
View File

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

45
CustomMsg.cs
View File

@ -1,45 +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 override byte Serialize(ref byte[] buffer) {
byte ix = 0;
buffer[ix++] = CustomMsg.Id;
buffer[ix++] = this.networkId;
buffer[ix++] = this.thingId;
//buffer[ix++] = (byte)bytes.Length;
foreach (byte b in bytes)
buffer[ix++] = b;
return ix;
}
//public static async Task<bool> Receive(Stream dataStream, Participant client, byte packetSize) {
// byte[] buffer = await Receive(dataStream, packetSize);
// CustomMsg msg = new(buffer);
// client.messageQueue.Enqueue(msg);
// return true;
//}
}
}

2
CustomMsg.cs.meta
View File

@ -1,2 +0,0 @@
fileFormatVersion: 2
guid: b9dcd611539afab429456b08500ae152

8
DoxyGen.meta
View File

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

4
EchoStream.cs
View File

@ -1,3 +1,4 @@
/*
using System;
using System.IO;
using System.Threading.Tasks;
@ -170,4 +171,5 @@ public class EchoStream : Stream {
public override void SetLength(long value) {
throw new NotImplementedException();
}
}
}
*/

2
EchoStream.cs.meta
View File

@ -1,2 +0,0 @@
fileFormatVersion: 2
guid: 422516a56cbf14d46aaa0b1bc09115bf

8
LinearAlgebra.meta
View File

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

109
LowLevelMessages.cs
View File

@ -1,109 +0,0 @@
using Passer.LinearAlgebra;
namespace Passer.Control.Core
{
public class LowLevelMessages
{
public static void SendSpherical(byte[] buffer, ref byte ix, Spherical v)
{
SendFloat16(buffer, ref ix, new float16(v.distance));
SendAngle8(buffer, ref ix, v.direction.horizontal);
SendAngle8(buffer, ref ix, v.direction.horizontal);
}
public static Spherical ReceiveSpherical(byte[] data, ref byte ix)
{
float distance = ReceiveFloat16(data, ref ix);
float horizontal = ReceiveAngle8(data, ref ix);
float vertical = ReceiveAngle8(data, ref ix);
Spherical v = new(distance, horizontal, vertical);
return v;
}
public static void SendQuat32(byte[] buffer, ref byte ix, SwingTwist s)
{
Quat32 q32 = Quat32.FromSwingTwist(s);
SendQuat32(buffer, ref ix, q32);
}
public static void SendQuat32(byte[] buffer, ref byte ix, Quat32 q)
{
int qx = (int)(q.x * 127 + 128);
int qy = (int)(q.y * 127 + 128);
int qz = (int)(q.z * 127 + 128);
int qw = (int)(q.w * 255);
if (q.w < 0)
{
qx = -qx;
qy = -qy;
qz = -qz;
qw = -qw;
}
buffer[ix++] = (byte)qx;
buffer[ix++] = (byte)qy;
buffer[ix++] = (byte)qz;
buffer[ix++] = (byte)qw;
}
public static Quat32 ReceiveQuat32(byte[] data, ref byte ix)
{
Quat32 q = new(
(data[ix++] - 128.0F) / 127.0F,
(data[ix++] - 128.0F) / 127.0F,
(data[ix++] - 128.0F) / 127.0F,
data[ix++] / 255.0F);
return q;
}
public static SwingTwist ReceiveSwingTwist(byte[] data, ref byte ix)
{
Quat32 q32 = ReceiveQuat32(data, ref ix);
// UnityEngine.Quaternion q = new(q32.x, q32.y, q32.z, q32.w);
// SwingTwist r = new(q.eulerAngles.y, q.eulerAngles.x, q.eulerAngles.z);
SwingTwist r = SwingTwist.FromQuat32(q32);
return r;
}
public static void SendAngle8(byte[] buffer, ref byte ix, float angle)
{
// Normalize angle
while (angle >= 180)
angle -= 360;
while (angle < -180)
angle += 360;
buffer[ix++] = (byte)((angle / 360.0f) * 256.0f);
}
public static float ReceiveAngle8(byte[] data, ref byte ix)
{
float value = (data[ix++] * 180) / 128.0F;
return value;
}
public static void SendFloat16(byte[] data, ref byte ix, float f)
{
float16 f16 = new(f);
ushort binary = f16.GetBinary();
data[ix++] = (byte)(binary >> 8);
data[ix++] = (byte)(binary & 255);
}
public static void SendFloat16(byte[] data, ref byte ix, float16 f)
{
ushort binary = f.GetBinary();
data[ix++] = (byte)(binary >> 8);
data[ix++] = (byte)(binary & 255);
}
public static float ReceiveFloat16(byte[] data, ref byte ix)
{
ushort value = (ushort)(data[ix++] << 8 | data[ix++]);
float16 f16 = new();
f16.SetBinary(value);
float f = f16.toFloat();
return f;
}
}
}

11
LowLevelMessages.cs.meta
View File

@ -1,11 +0,0 @@
fileFormatVersion: 2
guid: 157cf85b523c1f648adc007539f8b736
MonoImporter:
externalObjects: {}
serializedVersion: 2
defaultReferences: []
executionOrder: 0
icon: {instanceID: 0}
userData:
assetBundleName:
assetBundleVariant:

253
Messages.cs
View File

@ -1,253 +0,0 @@
using System.IO;
using System.Threading.Tasks;
using Passer.LinearAlgebra;
namespace Passer.Control.Core {
public class IMessage {
public IMessage() { }
public IMessage(byte[] buffer) {
Deserialize(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<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
}

11
Messages.cs.meta
View File

@ -1,11 +0,0 @@
fileFormatVersion: 2
guid: d3f26f22a5422a44997b39a1844ab2ad
MonoImporter:
externalObjects: {}
serializedVersion: 2
defaultReferences: []
executionOrder: 0
icon: {instanceID: 0}
userData:
assetBundleName:
assetBundleVariant:

2
ModelUrlMsg.cs.meta
View File

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

49
NameMsg.cs
View File

@ -1,49 +0,0 @@
#nullable enable
namespace Passer.Control.Core {
public class NameMsg : IMessage {
public const byte Id = 0x91; // 145
public const byte length = 4;
public byte networkId;
public byte thingId;
public byte len;
public string? name = null;
public NameMsg(byte networkId, Thing thing) {
this.networkId = networkId;
this.thingId = thing.id;
this.name = thing.name;
}
public NameMsg(byte networkId, byte thingId, string name) {
this.networkId = networkId;
this.thingId = thingId;
this.name = name;
}
public NameMsg(byte[] buffer) {
byte ix = 1;
this.networkId = buffer[ix++];
this.thingId = buffer[ix++];
int strlen = buffer[ix++];
this.name = System.Text.Encoding.UTF8.GetString(buffer, (int)ix, strlen);
}
public override byte Serialize(ref byte[] buffer) {
if (this.name == null)
return 0;
byte ix = 0;
buffer[ix++] = NameMsg.Id;
buffer[ix++] = this.networkId;
buffer[ix++] = this.thingId;
int nameLength = this.name.Length;
buffer[ix++] = (byte)nameLength;
for (int nameIx = 0; nameIx < nameLength; nameIx++)
buffer[ix++] = (byte)this.name[nameIx];
return ix;
}
}
}

2
NameMsg.cs.meta
View File

@ -1,2 +0,0 @@
fileFormatVersion: 2
guid: 462c645008baabe46b81f5d0ab83e26e

22
NetworkIdMsg.cs
View File

@ -1,22 +0,0 @@
namespace Passer.Control.Core {
public class NetworkIdMsg : IMessage {
public const byte Id = 0xA1;
public const byte length = 2;
public byte networkId;
public NetworkIdMsg(byte networkId) {
this.networkId = networkId;
}
public NetworkIdMsg(byte[] buffer) {
this.networkId = buffer[1];
}
public override byte Serialize(ref byte[] buffer) {
buffer[0] = NetworkIdMsg.Id;
buffer[1] = this.networkId;
return NetworkIdMsg.length;
}
}
}

2
NetworkIdMsg.cs.meta
View File

@ -1,2 +0,0 @@
fileFormatVersion: 2
guid: c51dae79b8c6f5a428349e1f4d34982c

11
Participant.cs.meta
View File

@ -1,11 +0,0 @@
fileFormatVersion: 2
guid: 302ed9b89a108a4429ea274044424a03
MonoImporter:
externalObjects: {}
serializedVersion: 2
defaultReferences: []
executionOrder: 0
icon: {instanceID: 0}
userData:
assetBundleName:
assetBundleVariant:

7
README.md.meta
View File

@ -1,7 +0,0 @@
fileFormatVersion: 2
guid: 3b79b5b373e9ced4abe72eb4d2f83c6a
TextScriptImporter:
externalObjects: {}
userData:
assetBundleName:
assetBundleVariant:

8
Sensors.meta
View File

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

32
SiteServer.cs
View File

@ -1,4 +1,3 @@
using System;
using System.Net;
using System.Net.Sockets;
@ -14,9 +13,11 @@ namespace Passer.Control.Core {
this.udpClient.Client.Bind(new IPEndPoint(IPAddress.Any, port));
this.udpClient.BeginReceive(new AsyncCallback(result => ReceiveUDP(result)), null);
this.name = "Site Server";
Register<TemperatureSensor>((byte)Thing.Type.TemperatureSensor);
}
public override void Update(long currentTimeMs) {
public override void Update(long currentTimeMs) {
Thing.UpdateAll(currentTimeMs);
}
@ -30,17 +31,28 @@ namespace Passer.Control.Core {
protected override void Process(Participant sender, NetworkIdMsg msg) {
}
delegate Thing ThingConstructor(byte networkId, byte thingId);
readonly Dictionary<byte, ThingConstructor> thingMsgProcessors = [];
public void Register<ThingClass>(byte thingType) where ThingClass : Thing {
thingMsgProcessors[thingType] = (byte networkId, byte thingId) => {
if (Activator.CreateInstance(typeof(Thing), networkId, thingId) is not ThingClass instance)
throw new InvalidOperationException($"Could not created an instance of {(typeof(ThingClass))}.");
return instance;
};
Console.WriteLine($"Registering {typeof(ThingClass)} for thing type {thingType}");
}
protected override void Process(ThingMsg msg) {
Thing thing = Thing.Get(msg.networkId, msg.thingId);
if (thing == null) {
switch ((Thing.Type) msg.thingType) {
case Thing.Type.TemperatureSensor:
new TemperatureSensor(msg.networkId, msg.thingId);
break;
default:
new Thing(this, msg.networkId, msg.thingId, msg.thingType);
break;
}
if (thingMsgProcessors.TryGetValue(msg.thingType, out ThingConstructor? thingConstructor)) {
thingConstructor(msg.networkId, msg.thingId);
}
else {
new Thing(this, msg.networkId, msg.thingId, msg.thingType);
}
}
}

2
SiteServer.cs.meta
View File

@ -1,2 +0,0 @@
fileFormatVersion: 2
guid: 53345abb9310d344baa67c19a8d8249f

11
Thing.cs.meta
View File

@ -1,11 +0,0 @@
fileFormatVersion: 2
guid: 7429282ee0e367445bd1e2111631b27d
MonoImporter:
externalObjects: {}
serializedVersion: 2
defaultReferences: []
executionOrder: 0
icon: {instanceID: 0}
userData:
assetBundleName:
assetBundleVariant:

2
ThingMsg.cs.meta
View File

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

2
float16.cs.meta
View File

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

8
test.meta
View File

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

7
test.sln.meta
View File

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