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

Completed migration to controlcore

Browse Source
This commit is contained in:
Pascal Serrarens 2024-12-18 17:02:12 +01:00
parent ae6f9fa395
commit e532f31236
2 changed files with 68 additions and 130 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

196
Messages.cs
View File

@ -2,14 +2,11 @@ using System.IO;
using System.Threading.Tasks; using System.Threading.Tasks;
using Passer.LinearAlgebra; using Passer.LinearAlgebra;
namespace Passer.Control namespace Passer.Control {
{
public class IMessage public class IMessage {
{
public IMessage() { } public IMessage() { }
public IMessage(byte[] buffer) public IMessage(byte[] buffer) {
{
Deserialize(buffer); Deserialize(buffer);
} }
@ -21,24 +18,20 @@ namespace Passer.Control
return client.SendBuffer(); return client.SendBuffer();
} }
public static bool SendMsg(Participant client, IMessage msg) public static bool SendMsg(Participant client, IMessage msg) {
{
msg.Serialize(ref client.buffer); msg.Serialize(ref client.buffer);
return client.SendBuffer(); return client.SendBuffer();
} }
public static bool PublishMsg(Participant client, IMessage msg) public static bool PublishMsg(Participant client, IMessage msg) {
{
msg.Serialize(ref client.buffer); msg.Serialize(ref client.buffer);
return client.PublishBuffer(); return client.PublishBuffer();
} }
public static async Task<byte[]> Receive(Stream dataStream, byte packetSize) public static async Task<byte[]> Receive(Stream dataStream, byte packetSize) {
{
byte[] buffer = new byte[packetSize - 1]; // without msgId byte[] buffer = new byte[packetSize - 1]; // without msgId
int byteCount = dataStream.Read(buffer, 0, packetSize - 1); int byteCount = dataStream.Read(buffer, 0, packetSize - 1);
while (byteCount < packetSize - 1) while (byteCount < packetSize - 1) {
{
// not all bytes have been read, wait and try again // not all bytes have been read, wait and try again
await Task.Delay(1); await Task.Delay(1);
byteCount += dataStream.Read(buffer, byteCount, packetSize - 1 - byteCount); byteCount += dataStream.Read(buffer, byteCount, packetSize - 1 - byteCount);
@ -49,58 +42,49 @@ namespace Passer.Control
#region Client #region Client
public class ClientMsg : IMessage public class ClientMsg : IMessage {
{
public const byte Id = 0xA0; public const byte Id = 0xA0;
public const byte length = 2; public const byte length = 2;
public byte networkId; public byte networkId;
public ClientMsg(byte networkId) public ClientMsg(byte networkId) {
{
this.networkId = networkId; this.networkId = networkId;
} }
public ClientMsg(byte[] buffer) : base(buffer) { } public ClientMsg(byte[] buffer) : base(buffer) { }
public override byte Serialize(ref byte[] buffer) public override byte Serialize(ref byte[] buffer) {
{
byte ix = 0; byte ix = 0;
buffer[ix++] = ClientMsg.Id; buffer[ix++] = ClientMsg.Id;
buffer[ix++] = networkId; buffer[ix++] = networkId;
return ix; return ix;
} }
public override void Deserialize(byte[] buffer) public override void Deserialize(byte[] buffer) {
{
base.Deserialize(buffer); base.Deserialize(buffer);
uint ix = 0; uint ix = 0;
networkId = buffer[ix]; networkId = buffer[ix];
} }
public static bool Send(Participant client, byte networkId) public static bool Send(Participant client, byte networkId) {
{
ClientMsg msg = new(networkId); ClientMsg msg = new(networkId);
return SendMsg(client, msg); return SendMsg(client, msg);
} }
public static bool Publish(Participant client, byte networkId) public static bool Publish(Participant client, byte networkId) {
{
ClientMsg msg = new(networkId); ClientMsg msg = new(networkId);
return PublishMsg(client, msg); return PublishMsg(client, msg);
} }
public static async Task<bool> Receive(Stream dataStream, Participant client, byte packetSize) public static async Task<bool> Receive(Stream dataStream, Participant client, byte packetSize) {
{
if (packetSize != length) if (packetSize != length)
return false; return false;
byte[] buffer = await Receive(dataStream, packetSize); byte[] buffer = await Receive(dataStream, packetSize);
ClientMsg msg = new(buffer); ClientMsg msg = new(buffer);
if (client.networkId == 0) if (client.networkId == 0) {
{
client.networkId = (byte)(Participant.clients.Count); client.networkId = (byte)(Participant.clients.Count);
NetworkIdMsg.Send(client, client.networkId); NetworkIdMsg.Send(client, client.networkId);
client.messageQueue.Enqueue(msg); client.messageQueue.Enqueue(msg);
} }
else if (msg.networkId == 0) else if (msg.networkId == 0) {
{
NetworkIdMsg.Send(client, client.networkId); NetworkIdMsg.Send(client, client.networkId);
client.messageQueue.Enqueue(msg); client.messageQueue.Enqueue(msg);
} }
@ -113,38 +97,32 @@ namespace Passer.Control
#region Network Id #region Network Id
public class NetworkIdMsg : IMessage public class NetworkIdMsg : IMessage {
{
public const byte Id = 0xA1; public const byte Id = 0xA1;
public const byte length = 2; public const byte length = 2;
public byte networkId; public byte networkId;
NetworkIdMsg(byte networkId) NetworkIdMsg(byte networkId) {
{
this.networkId = networkId; this.networkId = networkId;
} }
NetworkIdMsg(byte[] buffer) : base(buffer) { } NetworkIdMsg(byte[] buffer) : base(buffer) { }
public override byte Serialize(ref byte[] buffer) public override byte Serialize(ref byte[] buffer) {
{
byte ix = 0; byte ix = 0;
buffer[ix++] = NetworkIdMsg.Id; buffer[ix++] = NetworkIdMsg.Id;
buffer[ix++] = this.networkId; buffer[ix++] = this.networkId;
return ix; return ix;
} }
public override void Deserialize(byte[] buffer) public override void Deserialize(byte[] buffer) {
{
uint ix = 0; uint ix = 0;
this.networkId = buffer[ix]; this.networkId = buffer[ix];
} }
public static bool Send(Participant client, byte networkId) public static bool Send(Participant client, byte networkId) {
{
NetworkIdMsg msg = new(networkId); NetworkIdMsg msg = new(networkId);
return SendMsg(client, msg); return SendMsg(client, msg);
} }
public static async Task<bool> Receive(Stream dataStream, Participant client, byte packetSize) public static async Task<bool> Receive(Stream dataStream, Participant client, byte packetSize) {
{
if (packetSize != length) if (packetSize != length)
return false; return false;
@ -159,30 +137,26 @@ namespace Passer.Control
#region Investigate #region Investigate
public class InvestigateMsg : IMessage public class InvestigateMsg : IMessage {
{
public const byte Id = 0x81; public const byte Id = 0x81;
public const byte length = 3; public const byte length = 3;
public byte networkId; public byte networkId;
public byte thingId; public byte thingId;
public InvestigateMsg(byte networkId, byte thingId) public InvestigateMsg(byte networkId, byte thingId) {
{
this.networkId = networkId; this.networkId = networkId;
this.thingId = thingId; this.thingId = thingId;
} }
public InvestigateMsg(byte[] buffer) : base(buffer) { } public InvestigateMsg(byte[] buffer) : base(buffer) { }
public override byte Serialize(ref byte[] buffer) public override byte Serialize(ref byte[] buffer) {
{
byte ix = 0; byte ix = 0;
buffer[ix++] = InvestigateMsg.Id; buffer[ix++] = InvestigateMsg.Id;
buffer[ix++] = this.networkId; buffer[ix++] = this.networkId;
buffer[ix++] = this.thingId; buffer[ix++] = this.thingId;
return ix; return ix;
} }
public override void Deserialize(byte[] buffer) public override void Deserialize(byte[] buffer) {
{
uint ix = 0; uint ix = 0;
this.networkId = buffer[ix++]; this.networkId = buffer[ix++];
this.thingId = buffer[ix++]; this.thingId = buffer[ix++];
@ -192,8 +166,7 @@ namespace Passer.Control
// InvestigateMsg msg = new(thing.networkId, thing.id); // InvestigateMsg msg = new(thing.networkId, thing.id);
// return SendMsg(client, msg); // return SendMsg(client, msg);
//} //}
public static async Task<bool> Receive(Stream dataStream, Participant client, byte packetSize) public static async Task<bool> Receive(Stream dataStream, Participant client, byte packetSize) {
{
if (packetSize != length) if (packetSize != length)
return false; return false;
@ -211,8 +184,7 @@ namespace Passer.Control
#region Thing #region Thing
public class ThingMsg : IMessage public class ThingMsg : IMessage {
{
public const byte length = 5; public const byte length = 5;
public const byte Id = 0x80; public const byte Id = 0x80;
public byte networkId; public byte networkId;
@ -220,8 +192,7 @@ namespace Passer.Control
public byte thingType; public byte thingType;
public byte parentId; public byte parentId;
public ThingMsg(byte networkId, byte thingId, byte thingType, byte parentId) public ThingMsg(byte networkId, byte thingId, byte thingType, byte parentId) {
{
this.networkId = networkId; this.networkId = networkId;
this.thingId = thingId; this.thingId = thingId;
this.thingType = thingType; this.thingType = thingType;
@ -229,8 +200,7 @@ namespace Passer.Control
} }
public ThingMsg(byte[] buffer) : base(buffer) { } public ThingMsg(byte[] buffer) : base(buffer) { }
public override byte Serialize(ref byte[] buffer) public override byte Serialize(ref byte[] buffer) {
{
byte ix = 0; byte ix = 0;
buffer[ix++] = ThingMsg.Id; buffer[ix++] = ThingMsg.Id;
buffer[ix++] = this.networkId; buffer[ix++] = this.networkId;
@ -239,8 +209,7 @@ namespace Passer.Control
buffer[ix++] = this.parentId; buffer[ix++] = this.parentId;
return ix; return ix;
} }
public override void Deserialize(byte[] buffer) public override void Deserialize(byte[] buffer) {
{
uint ix = 0; uint ix = 0;
this.networkId = buffer[ix++]; this.networkId = buffer[ix++];
this.thingId = buffer[ix++]; this.thingId = buffer[ix++];
@ -258,8 +227,7 @@ namespace Passer.Control
// //UnityEngine.Debug.Log($"Send thing [{msg.networkId}/{msg.thingId}]"); // //UnityEngine.Debug.Log($"Send thing [{msg.networkId}/{msg.thingId}]");
// return SendMsg(client, msg); // return SendMsg(client, msg);
//} //}
public static async Task<bool> Receive(Stream dataStream, Participant client, byte packetSize) public static async Task<bool> Receive(Stream dataStream, Participant client, byte packetSize) {
{
if (packetSize != length) if (packetSize != length)
return false; return false;
@ -280,8 +248,7 @@ namespace Passer.Control
#region Name #region Name
public class NameMsg : IMessage public class NameMsg : IMessage {
{
public const byte Id = 0x91; // 145 public const byte Id = 0x91; // 145
public const byte length = 4; public const byte length = 4;
public byte networkId; public byte networkId;
@ -289,16 +256,14 @@ namespace Passer.Control
public byte len; public byte len;
public string name; public string name;
public NameMsg(byte networkId, byte thingId, string name) public NameMsg(byte networkId, byte thingId, string name) {
{
this.networkId = networkId; this.networkId = networkId;
this.thingId = thingId; this.thingId = thingId;
this.name = name; this.name = name;
} }
public NameMsg(byte[] buffer) : base(buffer) { } public NameMsg(byte[] buffer) : base(buffer) { }
public override byte Serialize(ref byte[] buffer) public override byte Serialize(ref byte[] buffer) {
{
byte ix = 0; byte ix = 0;
buffer[ix++] = NameMsg.Id; buffer[ix++] = NameMsg.Id;
buffer[ix++] = this.networkId; buffer[ix++] = this.networkId;
@ -308,8 +273,7 @@ namespace Passer.Control
buffer[ix++] = (byte)this.name[nameIx]; buffer[ix++] = (byte)this.name[nameIx];
return ix; return ix;
} }
public override void Deserialize(byte[] buffer) public override void Deserialize(byte[] buffer) {
{
byte ix = 0; byte ix = 0;
this.networkId = buffer[ix++]; this.networkId = buffer[ix++];
this.thingId = buffer[ix++]; this.thingId = buffer[ix++];
@ -329,8 +293,7 @@ namespace Passer.Control
// NameMsg msg = new(networkId, thingId, name); // NameMsg msg = new(networkId, thingId, name);
// return SendMsg(client, msg); // return SendMsg(client, msg);
//} //}
public static async Task<bool> Receive(Stream dataStream, Participant client, byte packetSize) public static async Task<bool> Receive(Stream dataStream, Participant client, byte packetSize) {
{
byte[] buffer = await Receive(dataStream, packetSize); byte[] buffer = await Receive(dataStream, packetSize);
NameMsg msg = new(buffer); NameMsg msg = new(buffer);
@ -343,8 +306,7 @@ namespace Passer.Control
#region Model URL #region Model URL
public class ModelUrlMsg : IMessage public class ModelUrlMsg : IMessage {
{
public const byte Id = 0x90; // (144) Model URL public const byte Id = 0x90; // (144) Model URL
public byte networkId; public byte networkId;
public byte thingId; public byte thingId;
@ -352,8 +314,7 @@ namespace Passer.Control
public float scale; public float scale;
public string url; public string url;
public ModelUrlMsg(byte networkId, byte thingId, string url, float scale = 1) public ModelUrlMsg(byte networkId, byte thingId, string url, float scale = 1) {
{
this.networkId = networkId; this.networkId = networkId;
this.thingId = thingId; this.thingId = thingId;
this.url = url; this.url = url;
@ -362,8 +323,7 @@ namespace Passer.Control
} }
public ModelUrlMsg(byte[] buffer) : base(buffer) { } public ModelUrlMsg(byte[] buffer) : base(buffer) { }
public override byte Serialize(ref byte[] buffer) public override byte Serialize(ref byte[] buffer) {
{
byte ix = 0; byte ix = 0;
buffer[ix++] = ModelUrlMsg.Id; buffer[ix++] = ModelUrlMsg.Id;
buffer[ix++] = this.networkId; buffer[ix++] = this.networkId;
@ -375,8 +335,7 @@ namespace Passer.Control
buffer[ix++] = (byte)url[urlIx]; buffer[ix++] = (byte)url[urlIx];
return ix; return ix;
} }
public override void Deserialize(byte[] buffer) public override void Deserialize(byte[] buffer) {
{
byte ix = 0; byte ix = 0;
this.networkId = buffer[ix++]; this.networkId = buffer[ix++];
this.thingId = buffer[ix++]; this.thingId = buffer[ix++];
@ -392,16 +351,14 @@ namespace Passer.Control
ModelUrlMsg msg = new(thing.networkId, thing.id, thing.modelUrl); ModelUrlMsg msg = new(thing.networkId, thing.id, thing.modelUrl);
return SendMsg(client, msg); return SendMsg(client, msg);
} }
public static bool Send(Participant client, byte networkId, byte thingId, string modelUrl) public static bool Send(Participant client, byte networkId, byte thingId, string modelUrl) {
{
if (string.IsNullOrEmpty(modelUrl)) if (string.IsNullOrEmpty(modelUrl))
return true; // nothing sent, but still a success! return true; // nothing sent, but still a success!
ModelUrlMsg msg = new(networkId, thingId, modelUrl); ModelUrlMsg msg = new(networkId, thingId, modelUrl);
return SendMsg(client, msg); return SendMsg(client, msg);
} }
public static async Task<bool> Receive(Stream dataStream, Participant client, byte packetSize) public static async Task<bool> Receive(Stream dataStream, Participant client, byte packetSize) {
{
byte[] buffer = await Receive(dataStream, packetSize); byte[] buffer = await Receive(dataStream, packetSize);
ModelUrlMsg msg = new(buffer); ModelUrlMsg msg = new(buffer);
client.messageQueue.Enqueue(msg); client.messageQueue.Enqueue(msg);
@ -413,8 +370,7 @@ namespace Passer.Control
#region Pose #region Pose
public class PoseMsg : IMessage public class PoseMsg : IMessage {
{
public const byte Id = 0x10; public const byte Id = 0x10;
public const byte length = 4 + 4 + 4; public const byte length = 4 + 4 + 4;
public byte networkId; public byte networkId;
@ -431,8 +387,7 @@ namespace Passer.Control
public Spherical linearVelocity; public Spherical linearVelocity;
public Spherical angularVelocity; public Spherical angularVelocity;
public PoseMsg(byte networkId, byte thingId, Spherical position, Quat32 orientation) public PoseMsg(byte networkId, byte thingId, Spherical position, Quat32 orientation) {
{
this.networkId = networkId; this.networkId = networkId;
this.thingId = thingId; this.thingId = thingId;
this.position = position; this.position = position;
@ -449,8 +404,7 @@ namespace Passer.Control
} }
public PoseMsg(byte[] buffer) : base(buffer) { } public PoseMsg(byte[] buffer) : base(buffer) { }
public override byte Serialize(ref byte[] buffer) public override byte Serialize(ref byte[] buffer) {
{
byte ix = 0; byte ix = 0;
buffer[ix++] = PoseMsg.Id; buffer[ix++] = PoseMsg.Id;
buffer[ix++] = this.networkId; buffer[ix++] = this.networkId;
@ -461,33 +415,27 @@ namespace Passer.Control
LowLevelMessages.SendQuat32(buffer, ref ix, this.orientation); LowLevelMessages.SendQuat32(buffer, ref ix, this.orientation);
return ix; return ix;
} }
public override void Deserialize(byte[] buffer) public override void Deserialize(byte[] buffer) {
{
byte ix = 0; byte ix = 0;
this.networkId = buffer[ix++]; this.networkId = buffer[ix++];
this.thingId = buffer[ix++]; this.thingId = buffer[ix++];
this.poseType = buffer[ix++]; this.poseType = buffer[ix++];
//if ((poseType & Pose_Position) != 0) if ((poseType & Pose_Position) != 0)
this.position = LowLevelMessages.ReceiveSpherical(buffer, ref ix); this.position = LowLevelMessages.ReceiveSpherical(buffer, ref ix);
//if ((poseType & Pose_Orientation) != 0) if ((poseType & Pose_Orientation) != 0)
this.orientation = LowLevelMessages.ReceiveQuat32(buffer, ref ix); this.orientation = LowLevelMessages.ReceiveQuat32(buffer, ref ix);
if ((poseType & Pose_LinearVelocity) != 0) { if ((poseType & Pose_LinearVelocity) != 0)
this.linearVelocity = LowLevelMessages.ReceiveSpherical(buffer, ref ix); this.linearVelocity = LowLevelMessages.ReceiveSpherical(buffer, ref ix);
UnityEngine.Debug.Log($"Received linear velocity: {this.linearVelocity}"); if ((poseType & Pose_AngularVelocity) != 0)
} this.angularVelocity = LowLevelMessages.ReceiveSpherical(buffer, ref ix);
} }
public static bool Send(Participant client, byte thingId, Spherical position, Quat32 orientation) public static bool Send(Participant client, byte thingId, Spherical position, Quat32 orientation) {
{
PoseMsg msg = new(client.networkId, thingId, position, orientation); PoseMsg msg = new(client.networkId, thingId, position, orientation);
return SendMsg(client, msg); return SendMsg(client, msg);
} }
public static async Task<bool> Receive(Stream dataStream, Participant client, byte packetSize) public static async Task<bool> Receive(Stream dataStream, Participant client, byte packetSize) {
{
if (packetSize != length)
return false;
byte[] buffer = await Receive(dataStream, packetSize); byte[] buffer = await Receive(dataStream, packetSize);
PoseMsg msg = new(buffer); PoseMsg msg = new(buffer);
@ -504,14 +452,13 @@ namespace Passer.Control
#region Custom #region Custom
public class CustomMsg : IMessage public class CustomMsg : IMessage {
{
public const byte Id = 0xB1; public const byte Id = 0xB1;
public byte networkId; public byte networkId;
public byte thingId; public byte thingId;
public byte[] bytes; public byte[] bytes;
public CustomMsg(byte[] buffer) { public CustomMsg(byte[] buffer) {
byte ix = 0; byte ix = 0;
this.networkId = buffer[ix++]; this.networkId = buffer[ix++];
this.thingId = buffer[ix++]; this.thingId = buffer[ix++];
@ -520,15 +467,13 @@ namespace Passer.Control
for (uint bytesIx = 0; bytesIx < length; bytesIx++) for (uint bytesIx = 0; bytesIx < length; bytesIx++)
this.bytes[bytesIx] = buffer[ix++]; this.bytes[bytesIx] = buffer[ix++];
} }
public CustomMsg(byte networkId, byte thingId, byte[] bytes) : base() public CustomMsg(byte networkId, byte thingId, byte[] bytes) : base() {
{
this.networkId = networkId; this.networkId = networkId;
this.thingId = thingId; this.thingId = thingId;
this.bytes = bytes; this.bytes = bytes;
} }
public override byte Serialize(ref byte[] buffer) public override byte Serialize(ref byte[] buffer) {
{
byte ix = 0; byte ix = 0;
buffer[ix++] = CustomMsg.Id; buffer[ix++] = CustomMsg.Id;
buffer[ix++] = this.networkId; buffer[ix++] = this.networkId;
@ -540,8 +485,7 @@ namespace Passer.Control
return ix; return ix;
} }
public static async Task<bool> Receive(Stream dataStream, Participant client, byte packetSize) public static async Task<bool> Receive(Stream dataStream, Participant client, byte packetSize) {
{
byte[] buffer = await Receive(dataStream, packetSize); byte[] buffer = await Receive(dataStream, packetSize);
CustomMsg msg = new(buffer); CustomMsg msg = new(buffer);
@ -554,21 +498,18 @@ namespace Passer.Control
#region Text #region Text
public class TextMsg : IMessage public class TextMsg : IMessage {
{
public const byte Id = 0xB0; public const byte Id = 0xB0;
public string text; public string text;
public TextMsg(byte[] buffer) : base(buffer) { } public TextMsg(byte[] buffer) : base(buffer) { }
public override void Deserialize(byte[] buffer) public override void Deserialize(byte[] buffer) {
{
uint ix = 0; uint ix = 0;
uint strlen = buffer[ix++]; uint strlen = buffer[ix++];
this.text = System.Text.Encoding.UTF8.GetString(buffer, (int)ix, (int)strlen); this.text = System.Text.Encoding.UTF8.GetString(buffer, (int)ix, (int)strlen);
} }
public static async Task<bool> Receive(Stream dataStream, Participant client, byte packetSize) public static async Task<bool> Receive(Stream dataStream, Participant client, byte packetSize) {
{
byte[] buffer = await Receive(dataStream, packetSize); byte[] buffer = await Receive(dataStream, packetSize);
TextMsg msg = new(buffer); TextMsg msg = new(buffer);
@ -581,8 +522,7 @@ namespace Passer.Control
#region Destroy #region Destroy
public class DestroyMsg : IMessage public class DestroyMsg : IMessage {
{
public const byte Id = 0x20; public const byte Id = 0x20;
public const byte length = 3; public const byte length = 3;
public byte networkId; public byte networkId;
@ -590,14 +530,12 @@ namespace Passer.Control
public DestroyMsg(byte[] buffer) : base(buffer) { } public DestroyMsg(byte[] buffer) : base(buffer) { }
public override void Deserialize(byte[] buffer) public override void Deserialize(byte[] buffer) {
{
this.networkId = buffer[0]; this.networkId = buffer[0];
this.thingId = buffer[1]; this.thingId = buffer[1];
} }
public static async Task<bool> Receive(Stream dataStream, Participant client, byte packetSize) public static async Task<bool> Receive(Stream dataStream, Participant client, byte packetSize) {
{
if (packetSize != length) if (packetSize != length)
return false; return false;

2
Thing.cs
View File

@ -2,13 +2,13 @@ using System.Collections.Generic;
namespace Passer.Control namespace Passer.Control
{ {
public class CoreThing public class CoreThing
{ {
public Participant participant; public Participant participant;
public byte networkId; public byte networkId;
public byte id; public byte id;
public CoreThing parent; public CoreThing parent;
public List<CoreThing> children;
public byte type; public byte type;
public string name; public string name;
public string modelUrl; public string modelUrl;