RoboidControl-csharp/Messages.cs
2024-12-31 08:46:53 +01:00

654 lines
23 KiB
C#

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 Client
public class ClientMsg : IMessage {
public const byte Id = 0xA0;
public const byte length = 2;
public byte networkId;
public ClientMsg(byte[] buffer) : base(buffer) { }
public override void Deserialize(byte[] buffer) {
base.Deserialize(buffer);
uint ix = 0;
networkId = buffer[ix];
}
public static async Task<bool> Receive(Stream dataStream, Participant client, byte packetSize) {
if (packetSize != length)
return false;
byte[] buffer = await Receive(dataStream, packetSize);
ClientMsg msg = new(buffer);
if (client.networkId == 0) {
// client.networkId = (byte)(Participant.clients.Count);
// NetworkIdMsg.Send(client, client.networkId);
client.messageQueue.Enqueue(msg);
}
else if (msg.networkId == 0) {
// NetworkIdMsg.Send(client, client.networkId);
client.messageQueue.Enqueue(msg);
}
return true;
}
public static bool SendTo(Participant participant, byte networkId) {
if (participant == null)
return false;
byte ix = 0;
participant.buffer[ix++] = ClientMsg.Id;
participant.buffer[ix++] = networkId;
return participant.SendBuffer(ix);
}
public static bool Publish(Participant participant, byte networkId) {
if (participant == null)
return false;
byte ix = 0;
participant.buffer[ix++] = ClientMsg.Id;
participant.buffer[ix++] = networkId;
return participant.PublishBuffer(ix);
}
}
#endregion Client
#region Network Id
public class NetworkIdMsg : IMessage {
public const byte Id = 0xA1;
public const byte length = 2;
public byte networkId;
NetworkIdMsg(byte networkId) {
this.networkId = networkId;
}
NetworkIdMsg(byte[] buffer) : base(buffer) { }
public override byte Serialize(ref byte[] buffer) {
byte ix = 0;
buffer[ix++] = NetworkIdMsg.Id;
buffer[ix++] = this.networkId;
return ix;
}
public override void Deserialize(byte[] buffer) {
uint ix = 0;
this.networkId = buffer[ix];
}
public static bool Send(Participant client, byte networkId) {
NetworkIdMsg msg = new(networkId);
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);
NetworkIdMsg msg = new(buffer);
client.messageQueue.Enqueue(msg);
return true;
}
public static bool SendTo(Participant participant, byte networkId) {
if (participant == null)
return false;
byte ix = 0;
participant.buffer[ix++] = NetworkIdMsg.Id;
participant.buffer[ix++] = networkId;
return participant.SendBuffer(ix);
}
}
#endregion Network Id
#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 Thing
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, byte thingId, byte thingType, byte parentId) {
this.networkId = networkId;
this.thingId = thingId;
this.thingType = thingType;
this.parentId = parentId;
}
public ThingMsg(byte[] buffer) : base(buffer) { }
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 ix;
}
public override void Deserialize(byte[] buffer) {
uint ix = 0;
this.networkId = buffer[ix++];
this.thingId = buffer[ix++];
this.thingType = buffer[ix++];
this.parentId = buffer[ix];
}
public static bool Send(Participant client, Thing thing) {
ThingMsg msg = new(thing.networkId, thing.id, thing.type, thing.parent.id);
return SendMsg(client, msg);
}
//public static bool Send(Client client, byte networkId, byte thingId, byte thingType, byte parentId)
//{
// ThingMsg msg = new(networkId, thingId, thingType, parentId);
// //UnityEngine.Debug.Log($"Send thing [{msg.networkId}/{msg.thingId}]");
// 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);
ThingMsg msg = new(buffer);
//UnityEngine.Debug.Log($"Receive thing [{msg.networkId}/{msg.thingId}]");
// 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++] = ThingMsg.id;
participant.buffer[ix++] = participant.networkId;
participant.buffer[ix++] = thing.id;
participant.buffer[ix++] = thing.type;
if (thing.parent != null)
participant.buffer[ix++] = thing.parent.id;
else
participant.buffer[ix++] = 0;
return participant.SendBuffer(ix);
}
}
#endregion Thing
#region Name
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;
public NameMsg(byte networkId, byte thingId, string name) {
this.networkId = networkId;
this.thingId = thingId;
this.name = name;
}
public NameMsg(byte[] buffer) : base(buffer) { }
public override byte Serialize(ref byte[] buffer) {
byte ix = 0;
buffer[ix++] = NameMsg.Id;
buffer[ix++] = this.networkId;
buffer[ix++] = this.thingId;
buffer[ix++] = (byte)this.name.Length;
for (int nameIx = 0; nameIx < this.name.Length; nameIx++)
buffer[ix++] = (byte)this.name[nameIx];
return ix;
}
public override void Deserialize(byte[] buffer) {
byte ix = 0;
this.networkId = buffer[ix++];
this.thingId = buffer[ix++];
int strlen = buffer[ix++];
this.name = System.Text.Encoding.UTF8.GetString(buffer, (int)ix, strlen);
}
public static bool Send(Participant client, Thing thing) {
if (string.IsNullOrEmpty(thing.name))
return true; // nothing sent, but still a success!
NameMsg msg = new(thing.networkId, thing.id, thing.name);
return SendMsg(client, msg);
}
//public static bool Send(Client client, byte networkId, byte thingId, string name)
//{
// NameMsg msg = new(networkId, thingId, name);
// return SendMsg(client, msg);
//}
public static async Task<bool> Receive(Stream dataStream, Participant client, byte packetSize) {
byte[] buffer = await Receive(dataStream, packetSize);
NameMsg msg = new(buffer);
client.messageQueue.Enqueue(msg);
return true;
}
public static bool SendTo(Participant participant, Thing thing) {
if (participant == null || thing == null || thing.name == null)
return false;
byte nameLength = (byte)thing.name.Length;
if (participant.buffer.Length < 3 + nameLength)
return false;
byte ix = 0;
participant.buffer[ix++] = NameMsg.Id;
participant.buffer[ix++] = participant.networkId;
participant.buffer[ix++] = thing.id;
participant.buffer[ix++] = nameLength;
for (int nameIx = 0; nameIx < nameLength; nameIx++)
participant.buffer[ix++] = (byte)thing.name[nameIx];
return participant.SendBuffer(ix);
}
}
#endregion
#region Model URL
public class ModelUrlMsg : IMessage {
public const byte Id = 0x90; // (144) Model URL
public byte networkId;
public byte thingId;
public Spherical position;
public float scale;
public string url;
public ModelUrlMsg(byte networkId, byte thingId, string url, float scale = 1) {
this.networkId = networkId;
this.thingId = thingId;
this.url = url;
this.scale = scale;
this.position = Spherical.zero;
}
public ModelUrlMsg(byte[] buffer) : base(buffer) { }
public override byte Serialize(ref byte[] buffer) {
byte ix = 0;
buffer[ix++] = ModelUrlMsg.Id;
buffer[ix++] = this.networkId;
buffer[ix++] = this.thingId; // Thing Id
LowLevelMessages.SendFloat16(buffer, ref ix, new float16(1.0f));
buffer[ix++] = (byte)url.Length;
for (int urlIx = 0; urlIx < this.url.Length; urlIx++)
buffer[ix++] = (byte)url[urlIx];
return ix;
}
public override void Deserialize(byte[] buffer) {
byte ix = 0;
this.networkId = buffer[ix++];
this.thingId = buffer[ix++];
this.scale = LowLevelMessages.ReceiveFloat16(buffer, ref ix);
int strlen = buffer[ix++];
url = System.Text.Encoding.UTF8.GetString(buffer, (int)ix, strlen);
}
public static bool Send(Participant client, Thing thing) {
if (string.IsNullOrEmpty(thing.modelUrl))
return true; // nothing sent, but still a success!
ModelUrlMsg msg = new(thing.networkId, thing.id, thing.modelUrl);
return SendMsg(client, msg);
}
public static bool Send(Participant client, byte networkId, byte thingId, string modelUrl) {
if (string.IsNullOrEmpty(modelUrl))
return true; // nothing sent, but still a success!
ModelUrlMsg msg = new(networkId, thingId, modelUrl);
return SendMsg(client, msg);
}
public static async Task<bool> Receive(Stream dataStream, Participant client, byte packetSize) {
byte[] buffer = await Receive(dataStream, packetSize);
ModelUrlMsg msg = new(buffer);
client.messageQueue.Enqueue(msg);
return true;
}
public static bool SendTo(Participant participant, Thing thing) {
if (participant == null || thing == null || thing.modelUrl == null)
return false;
byte urlLength = (byte)thing.modelUrl.Length;
if (participant.buffer.Length < 3 + urlLength)
return false;
byte ix = 0;
participant.buffer[ix++] = ModelUrlMsg.Id;
participant.buffer[ix++] = participant.networkId;
participant.buffer[ix++] = thing.id; // Thing Id
LowLevelMessages.SendFloat16(participant.buffer, ref ix, new float16(1.0f));
participant.buffer[ix++] = urlLength;
for (int urlIx = 0; urlIx < urlLength; urlIx++)
participant.buffer[ix++] = (byte)thing.modelUrl[urlIx];
return participant.SendBuffer(ix);
}
}
#endregion Model URL
#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 Custom
public class CustomMsg : IMessage {
public const byte Id = 0xB1;
public byte networkId;
public byte thingId;
public byte[] bytes;
public CustomMsg(byte[] buffer) {
byte ix = 0;
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;
}
}
#endregion Custom
#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
}