HumanoidControl/HumanoidControl_Free
HumanoidControl/HumanoidControl_Free
2
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Squashed 'Runtime/HumanoidControl/Scripts/Networking/Roboid/ControlCore/' changes from 9919aa6..aebe4c0

Browse Source 
aebe4c0 Arduino Ant now works.
d3cb4c1 Merge commit 'fbeed8e80922152c3404fbd5d2b243ae95792ec1' into V2
fbeed8e Used Client override for processing messages
394dc22 Merge commit '355dd5c1c519cf07cfb6b9f9200f7f7311e68f20' into V2
355dd5c Fixed ThingMsg format
becb194 Merge commit 'f35d60369daf41a4fcd987ef8b31bd384b9536ba' into V2
9b53eee Merge commit 'a48ae12fc2f6d4a99119c128e78bf4b103e607c3' into V2
f35d603 Further improvements
a48ae12 ControlCore mostly works (but I don't see a model on the site server yet)
d8fc41f First step for ControlCore support

git-subtree-dir: Runtime/HumanoidControl/Scripts/Networking/Roboid/ControlCore
git-subtree-split: aebe4c0f8e805259a5aea4a4cb6b72343d73257a
This commit is contained in:
Pascal Serrarens 2024-12-09 12:12:08 +01:00
parent b3bb7d2f34
commit 7781fcf2de
8 changed files with 900 additions and 361 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

109
Client.cs Normal file
View File

@ -0,0 +1,109 @@
using System.Collections.Generic;
using System.Collections.Concurrent;
using System.Net.Sockets;
namespace Passer.Control {
public class Client {
//public ConnectionMethod connection;
public UdpClient udpClient;
public string ipAddress;
public int port;
public byte networkId = 0;
public readonly ConcurrentQueue<IMessage> messageQueue = new();
public static Client GetClient(string ipAddress, int port) {
foreach (Client c in clients) {
if (c.ipAddress == ipAddress && c.port == port)
return c;
}
return null;
}
static public List<Client> clients = new List<Client>();
//// These static functions are deprecated
//public static Client NewClient() {
// Client client = new();
// clients.Add(client);
// client.networkId = 0;
// return client;
//}
//public static Client NewUDPClient(UdpClient udpClient, string ipAddress, int port) {
// Client client = NewClient();
// client.ipAddress = null;
// client.port = port;
// client.udpClient = udpClient;
// return client;
//}
//public Client() {
//}
public Client(UdpClient udpClient, int port) {
this.udpClient = udpClient;
this.ipAddress = null;
this.port = port;
clients.Add(this);
}
public void ProcessMessage(IMessage msg) {
switch (msg) {
case ClientMsg clientMsg:
ProcessClient(clientMsg);
break;
case NetworkIdMsg networkId:
ProcessNetworkId(networkId);
break;
case InvestigateMsg investigate:
ProcessInvestigate(investigate);
break;
case ThingMsg thing:
ProcessThing(thing);
break;
case NameMsg name:
ProcessName(name);
break;
case ModelUrlMsg modelUrl:
ProcessModelUrl(modelUrl);
break;
case PoseMsg pose:
ProcessPose(pose);
break;
case CustomMsg custom:
ProcessCustom(custom);
break;
case TextMsg text:
ProcessText(text);
break;
case DestroyMsg destroy:
ProcessDestroy(destroy);
break;
}
}
protected virtual void ProcessClient(ClientMsg client) { }
protected virtual void ProcessNetworkId(NetworkIdMsg networkId) { }
protected virtual void ProcessInvestigate(InvestigateMsg investigate) { }
protected virtual void ProcessThing(ThingMsg thing) { }
protected virtual void ProcessName(NameMsg name) { }
protected virtual void ProcessModelUrl(ModelUrlMsg modelUrl) { }
protected virtual void ProcessPose(PoseMsg pose) { }
protected virtual void ProcessCustom(CustomMsg custom) { }
protected virtual void ProcessText(TextMsg text) { }
protected virtual void ProcessDestroy(DestroyMsg destroy) { }
}
}

2
Client.cs.meta Normal file
View File

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

173
EchoStream.cs Normal file
View File

@ -0,0 +1,173 @@
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();
}
}

2
EchoStream.cs.meta Normal file
View File

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

41
LowLevelMessages.cs
View File

@ -1,6 +1,13 @@
using Passer; using Passer;
public class LowLevelMessages { public class LowLevelMessages {
public static void SendSpherical(byte[] buffer, ref uint ix, Spherical v) {
SendAngle8(buffer, ref ix, v.horizontal);
SendAngle8(buffer, ref ix, v.vertical);
SendFloat16(buffer, ref ix, new float16(v.distance));
}
public static Spherical ReceiveSpherical(byte[] data, ref uint ix) { public static Spherical ReceiveSpherical(byte[] data, ref uint ix) {
float horizontal = ReceiveAngle8(data, ref ix); float horizontal = ReceiveAngle8(data, ref ix);
float vertical = ReceiveAngle8(data, ref ix); float vertical = ReceiveAngle8(data, ref ix);
@ -9,6 +16,23 @@ public class LowLevelMessages {
return v; return v;
} }
public static void SendQuat32(byte[] buffer, ref uint 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 uint ix) { public static Quat32 ReceiveQuat32(byte[] data, ref uint ix) {
Quat32 q = new( Quat32 q = new(
(data[ix++] - 128.0F) / 127.0F, (data[ix++] - 128.0F) / 127.0F,
@ -18,11 +42,26 @@ public class LowLevelMessages {
return q; return q;
} }
public static void SendAngle8(byte[] buffer, ref uint 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 uint ix) { public static float ReceiveAngle8(byte[] data, ref uint ix) {
float value = (data[ix++] * 180) / 128.0F; float value = (data[ix++] * 180) / 128.0F;
return value; return value;
} }
public static void SendFloat16(byte[] data, ref uint ix, float16 f) {
ushort binary = f.GetBinary();
data[ix++] = (byte)(binary >> 8);
data[ix++] = (byte)(binary & 255);
}
public static float ReceiveFloat16(byte[] data, ref uint ix) { public static float ReceiveFloat16(byte[] data, ref uint ix) {
ushort value = (ushort)(data[ix++] << 8 | data[ix++]); ushort value = (ushort)(data[ix++] << 8 | data[ix++]);
float16 f16 = new(); float16 f16 = new();
@ -30,6 +69,4 @@ public class LowLevelMessages {
float f = f16.toFloat(); float f = f16.toFloat();
return f; return f;
} }
} }

868
Messages.cs
View File

@ -1,365 +1,515 @@
using System.Collections.Generic;
using System.Collections.Concurrent;
using System.IO; using System.IO;
using System.Net.Sockets;
using System.Threading.Tasks; using System.Threading.Tasks;
namespace Passer.Control { namespace Passer.Control {
public class Client { public class IMessage {
//public ConnectionMethod connection; public IMessage() { }
public UdpClient udpClient; public IMessage(byte[] data) {
public string ipAddress; Deserialize(data);
public int port; }
public byte networkId; public virtual byte[] Serialize() { return null; }
public virtual void Deserialize(byte[] data) { }
public readonly ConcurrentQueue<IMessage> messageQueue = new();
public static bool SendMsg(Client client, IMessage msg) {
public static Client GetClient(string ipAddress, int port) { return SendMsg(client, msg.Serialize());
foreach (Client c in clients) { }
if (c.ipAddress == ipAddress && c.port == port) public static bool SendMsg(Client client, byte[] data) {
return c; if (client == null || client.ipAddress == null)
} return false;
return null;
} client.udpClient.Send(data, data.Length, client.ipAddress, client.port);
static public List<Client> clients = new List<Client>(); return true;
}
public static Client NewClient() {
Client client = new(); public static bool PublishMsg(Client client, IMessage msg) {
clients.Add(client); return PublishMsg(client, msg.Serialize());
client.networkId = 0; }
public static bool PublishMsg(Client client, byte[] data) {
return client; if (client == null)
} return false;
public static Client NewUDPClient(UdpClient udpClient, string ipAddress, int port) { client.udpClient.Send(data, data.Length, "127.0.0.1", client.port);
Client client = NewClient(); return true;
client.ipAddress = null; }
client.port = port;
client.udpClient = udpClient; public static async Task<byte[]> Receive(Stream dataStream, byte packetSize) {
return client; 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
public class IMessage { await Task.Delay(1);
public IMessage() { } byteCount += dataStream.Read(buffer, byteCount, packetSize - 1 - byteCount);
public IMessage(byte[] data) { }
Deserialize(data); return buffer;
} }
}
public virtual byte[] Serialize() { return null; }
public virtual void Deserialize(byte[] data) { } #region Client
public static bool SendMsg(Client client, IMessage msg) { public class ClientMsg : IMessage {
return SendMsg(client, msg.Serialize()); public const byte Id = 0xA0;
} public const byte length = 2;
public static bool SendMsg(Client client, byte[] data) { public byte networkId;
if (client == null || client.ipAddress == null)
return false; public ClientMsg(byte networkId) {
this.networkId = networkId;
client.udpClient.Send(data, data.Length, client.ipAddress, client.port); }
return true; public ClientMsg(byte[] data) : base(data) { }
}
public static async Task<byte[]> Receive(Stream dataStream, byte packetSize) { public override byte[] Serialize() {
byte[] buffer = new byte[packetSize - 1]; // without msgId byte[] buffer = new byte[ClientMsg.length];
int byteCount = dataStream.Read(buffer, 0, packetSize - 1); buffer[0] = ClientMsg.Id;
while (byteCount < packetSize - 1) { buffer[1] = networkId;
// not all bytes have been read, wait and try again return buffer;
await Task.Delay(1); }
byteCount += dataStream.Read(buffer, byteCount, packetSize - 1 - byteCount); public override void Deserialize(byte[] data) {
} base.Deserialize(data);
return buffer; uint ix = 0;
} networkId = data[ix];
} }
#region Client public static bool Send(Client client, byte networkId) {
ClientMsg msg = new(networkId);
public class ClientMsg : IMessage { return SendMsg(client, msg);
public const byte length = 2; }
public byte clientId; public static bool Publish(Client client, byte networkId) {
ClientMsg msg = new(networkId);
public ClientMsg(byte[] data) : base(data) { } return PublishMsg(client, msg);
public override void Deserialize(byte[] data) { }
base.Deserialize(data); public static async Task<bool> Receive(Stream dataStream, Client client, byte packetSize) {
uint ix = 0; if (packetSize != length)
clientId = data[ix++]; return false;
}
byte[] buffer = await Receive(dataStream, packetSize);
public static async Task<bool> Receive(Stream dataStream, Client client, byte packetSize) { ClientMsg msg = new(buffer);
if (packetSize != length)
return false; if (client.networkId == 0) {
client.networkId = (byte)(Client.clients.Count);
byte[] buffer = await Receive(dataStream, packetSize); NetworkIdMsg.Send(client, client.networkId);
ClientMsg msg = new(buffer); //if (string.IsNullOrEmpty(sceneUrl) == false)
//SendModelUrl(client, sceneUrl);
if (client.networkId == 0) { }
client.networkId = (byte)(Client.clients.Count); else if (msg.networkId == 0) {
NetworkIdMsg.Send(client); NetworkIdMsg.Send(client, client.networkId);
//if (string.IsNullOrEmpty(sceneUrl) == false) //if (string.IsNullOrEmpty(sceneUrl) == false)
//SendModelUrl(client, sceneUrl); //SendModelUrl(client, sceneUrl);
} }
else if (msg.clientId == 0) {
NetworkIdMsg.Send(client); return true;
//if (string.IsNullOrEmpty(sceneUrl) == false) }
//SendModelUrl(client, sceneUrl); }
}
#endregion Client
return true;
} #region Network Id
}
public class NetworkIdMsg : IMessage {
#endregion Client public const byte Id = 0xA1;
public const byte length = 2;
#region Network Id public byte networkId;
public class NetworkIdMsg : IMessage { NetworkIdMsg(byte networkId) {
public const byte Id = 0xA1; this.networkId = networkId;
public const byte length = 2; }
NetworkIdMsg(byte[] data) : base(data) { }
public static bool Send(Client client) {
byte[] data = new byte[NetworkIdMsg.length]; public override byte[] Serialize() {
data[0] = NetworkIdMsg.Id; byte[] data = new byte[NetworkIdMsg.length];
data[1] = client.networkId; data[0] = NetworkIdMsg.Id;
return SendMsg(client, data); data[1] = this.networkId;
} return data;
} }
public override void Deserialize(byte[] data) {
#endregion Network Id uint ix = 0;
this.networkId = data[ix];
#region Thing }
public class ThingMsg : IMessage { public static bool Send(Client client, byte networkId) {
public const byte length = 4; NetworkIdMsg msg = new(networkId);
public const byte Id = 0x80; return SendMsg(client, msg);
public byte objectId; //byte[] data = new byte[NetworkIdMsg.length];
public byte objectType; //data[0] = NetworkIdMsg.Id;
public byte parentId; //data[1] = client.networkId;
//return SendMsg(client, data);
public ThingMsg(byte[] data) : base(data) { } }
public override void Deserialize(byte[] data) { public static async Task<bool> Receive(Stream dataStream, Client client, byte packetSize) {
uint ix = 0; if (packetSize != length)
objectId = data[ix++]; return false;
objectType = data[ix++];
parentId = data[ix]; byte[] buffer = await Receive(dataStream, packetSize);
} NetworkIdMsg msg = new(buffer);
client.messageQueue.Enqueue(msg);
public static bool Send(Client client, byte networkId, byte thingId, byte thingType) { return true;
byte[] data = new byte[4]; }
data[0] = ThingMsg.Id; }
data[1] = networkId;
data[2] = thingId; #endregion Network Id
data[3] = thingType;
data[4] = 0x00; // parent not supported yet #region Investigate
return SendMsg(client, data);
} public class InvestigateMsg : IMessage {
public static async Task<bool> Receive(Stream dataStream, Client client, byte packetSize) { public const byte Id = 0x81;
if (packetSize != length) public const byte length = 3;
return false; public byte networkId;
public byte thingId;
byte[] buffer = await Receive(dataStream, packetSize);
ThingMsg msg = new(buffer); public InvestigateMsg(byte networkId, byte thingId) {
this.networkId = networkId;
client.messageQueue.Enqueue(msg); this.thingId = thingId;
return true; }
} public InvestigateMsg(byte[] data) : base(data) { }
}
public override byte[] Serialize() {
#endregion Thing byte[] buffer = new byte[InvestigateMsg.length];
buffer[0] = InvestigateMsg.Id;
#region Name buffer[1] = this.networkId;
buffer[2] = this.thingId;
public class NameMsg : IMessage { return buffer;
public byte networkId = 0; }
public byte objectId; public override void Deserialize(byte[] data) {
public byte len; uint ix = 0;
public string name; this.networkId = data[ix++];
this.thingId = data[ix++];
public NameMsg(byte[] data) : base(data) { } }
public override void Deserialize(byte[] data) {
uint ix = 0; public static bool Send(Client client, byte thingId) {
this.objectId = data[ix++]; InvestigateMsg msg = new(client.networkId, thingId);
int strlen = data[ix++]; return SendMsg(client, msg);
this.name = System.Text.Encoding.UTF8.GetString(data, (int)ix, strlen); }
} public static async Task<bool> Receive(Stream dataStream, Client client, byte packetSize) {
if (packetSize != length)
public static async Task<bool> Receive(Stream dataStream, Client client, byte packetSize) { return false;
byte[] buffer = await Receive(dataStream, packetSize);
NameMsg msg = new(buffer); byte[] buffer = await Receive(dataStream, packetSize);
InvestigateMsg msg = new(buffer);
client.messageQueue.Enqueue(msg); client.messageQueue.Enqueue(msg);
return true; return true;
}
} }
}
#endregion
#endregion Investigate
#region Model URL
#region Thing
public class ModelUrlMsg : IMessage {
public const byte Id = 0x90; // (144) Model URL public class ThingMsg : IMessage {
public byte objectId; public const byte length = 4;
public Spherical position; public const byte Id = 0x80;
public float scale; public byte thingId;
public string url; public byte thingType;
public byte parentId;
public ModelUrlMsg(string url, float scale = 1) {
this.url = url; public ThingMsg(byte thingId, byte thingType, byte parentId) {
this.scale = scale; this.thingId = thingId;
this.position = Spherical.zero; this.thingType = thingType;
} this.parentId = parentId;
public ModelUrlMsg(byte[] data) : base(data) { } }
public ThingMsg(byte[] data) : base(data) { }
public override byte[] Serialize() {
byte[] data = new byte[this.url.Length + 9]; public override byte[] Serialize() {
data[0] = ModelUrlMsg.Id; byte[] data = new byte[ThingMsg.length];
data[1] = 0x00; // Thing Id byte ix = 0;
// data[2]..[5] == position 0, 0, 0 data[ix++] = ThingMsg.Id;
data[6] = 0x3C; // Dummy float16 value 1 data[ix++] = this.thingId;
data[7] = 0x00; data[ix++] = this.thingType;
data[ix] = this.parentId;
data[8] = (byte)url.Length; return data;
for (int ix = 0; ix < this.url.Length; ix++) }
data[9 + ix] = (byte)url[ix]; public override void Deserialize(byte[] data) {
return data; uint ix = 0;
} this.thingId = data[ix++];
public override void Deserialize(byte[] data) { this.thingType = data[ix++];
uint ix = 0; this.parentId = data[ix];
this.objectId = data[ix++]; }
this.position = LowLevelMessages.ReceiveSpherical(data, ref ix);
this.scale = LowLevelMessages.ReceiveFloat16(data, ref ix); public static bool Send(Client client, byte thingId, byte thingType, byte parentId) {
int strlen = data[ix++]; ThingMsg msg = new(thingId, thingType, parentId);
url = System.Text.Encoding.UTF8.GetString(data, (int)ix, strlen); return SendMsg(client, msg);
} }
public static async Task<bool> Receive(Stream dataStream, Client client, byte packetSize) {
public static bool Send(Client client, string modelUrl) { if (packetSize != length)
ModelUrlMsg msg = new(modelUrl); return false;
return SendMsg(client, msg);
} byte[] buffer = await Receive(dataStream, packetSize);
public static async Task<bool> Receive(Stream dataStream, Client client, byte packetSize) { ThingMsg msg = new(buffer);
byte[] buffer = await Receive(dataStream, packetSize);
ModelUrlMsg msg = new(buffer); client.messageQueue.Enqueue(msg);
client.messageQueue.Enqueue(msg); return true;
return true; }
} }
}
#endregion Thing
#endregion Model URL
#region Name
#region Pose
public class NameMsg : IMessage {
public class PoseMsg : IMessage { public const byte Id = 0x91; // 145
public const byte length = 3 + 4 + 4; public const byte length = 3;
public byte thingId; public byte thingId;
public byte poseType; public byte len;
public string name;
public Spherical position;
public Quat32 orientation; public NameMsg(byte thingId, string name) {
this.thingId = thingId;
public PoseMsg(byte[] data) : base(data) { } this.name = name;
}
public override void Deserialize(byte[] data) { public NameMsg(byte[] data) : base(data) { }
uint ix = 0;
thingId = data[ix++]; public override byte[] Serialize() {
poseType = data[ix++]; byte[] buffer = new byte[length + this.name.Length];
buffer[0] = NameMsg.Id;
//if ((poseType & Pose_Position) != 0) buffer[1] = this.thingId;
position = LowLevelMessages.ReceiveSpherical(data, ref ix); buffer[2] = (byte)this.name.Length;
//if ((poseType & Pose_Orientation) != 0) { for (int ix = 0; ix < this.name.Length; ix++)
orientation = LowLevelMessages.ReceiveQuat32(data, ref ix); buffer[3 + ix] = (byte)this.name[ix];
} return buffer;
}
public static async Task<bool> Receive(Stream dataStream, Client client, byte packetSize) { public override void Deserialize(byte[] data) {
if (packetSize != length) uint ix = 0;
return false; this.thingId = data[ix++];
int strlen = data[ix++];
byte[] buffer = await Receive(dataStream, packetSize); this.name = System.Text.Encoding.UTF8.GetString(data, (int)ix, strlen);
PoseMsg msg = new(buffer); }
client.messageQueue.Enqueue(msg); public static bool Send(Client client, byte thingId, string name) {
return true; NameMsg msg = new(thingId, name);
return SendMsg(client, msg);
} }
} public static async Task<bool> Receive(Stream dataStream, Client client, byte packetSize) {
byte[] buffer = await Receive(dataStream, packetSize);
#endregion Pose NameMsg msg = new(buffer);
#region Bytes client.messageQueue.Enqueue(msg);
return true;
public class BytesMsg : IMessage { }
public const byte Id = 0xB1; }
public byte networkId;
public byte thingId; #endregion
public byte[] bytes;
#region Model URL
public BytesMsg(byte[] data) : base(data) { }
public BytesMsg(byte networkId, byte thingId, byte[] bytes) : base() { public class ModelUrlMsg : IMessage {
this.networkId = networkId; public const byte Id = 0x90; // (144) Model URL
this.thingId = thingId; public byte thingId;
this.bytes = bytes; public Spherical position;
} public float scale;
public override byte[] Serialize() { public string url;
byte[] buffer = new byte[4 + this.bytes.Length];
int ix = 0; public ModelUrlMsg(byte thingId, string url, float scale = 1) {
buffer[ix++] = BytesMsg.Id; this.thingId = thingId;
buffer[ix++] = this.networkId; this.url = url;
buffer[ix++] = this.thingId; this.scale = scale;
buffer[ix++] = (byte)bytes.Length; this.position = Spherical.zero;
foreach (byte b in bytes) }
buffer[ix++] = b; public ModelUrlMsg(byte[] data) : base(data) { }
return buffer; public override byte[] Serialize() {
} byte[] data = new byte[this.url.Length + 9];
public override void Deserialize(byte[] data) { data[0] = ModelUrlMsg.Id;
//this.bytes = data; data[1] = this.thingId; // Thing Id
uint ix = 0; // data[2]..[5] == position 0, 0, 0
this.thingId = data[ix++]; data[6] = 0x3C; // Dummy float16 value 1
this.bytes = new byte[data.Length - ix]; data[7] = 0x00;
for (uint bytesIx = 0; ix < data.Length; ix++, bytesIx++)
this.bytes[bytesIx] = data[ix]; data[8] = (byte)url.Length;
} for (int ix = 0; ix < this.url.Length; ix++)
data[9 + ix] = (byte)url[ix];
return data;
public static void Send(Client client, byte thingId, byte[] bytes) { }
BytesMsg msg = new(client.networkId, thingId, bytes); public override void Deserialize(byte[] data) {
SendMsg(client, msg); uint ix = 0;
} this.thingId = data[ix++];
this.position = LowLevelMessages.ReceiveSpherical(data, ref ix);
// received bytes this.scale = LowLevelMessages.ReceiveFloat16(data, ref ix);
public static async Task<bool> Receive(Stream dataStream, Client client, byte packetSize) { int strlen = data[ix++];
byte[] buffer = await Receive(dataStream, packetSize); url = System.Text.Encoding.UTF8.GetString(data, (int)ix, strlen);
BytesMsg msg = new(buffer); }
client.messageQueue.Enqueue(msg);
return true; public static bool Send(Client client, byte thingId, string modelUrl) {
} ModelUrlMsg msg = new(thingId, modelUrl);
} return SendMsg(client, msg);
}
#endregion Bytes public static async Task<bool> Receive(Stream dataStream, Client client, byte packetSize) {
byte[] buffer = await Receive(dataStream, packetSize);
#region Destroy ModelUrlMsg msg = new(buffer);
client.messageQueue.Enqueue(msg);
public class DestroyMsg : IMessage { return true;
public const byte length = 2; }
public byte objectId; }
public DestroyMsg(byte[] data) : base(data) { } #endregion Model URL
public override void Deserialize(byte[] data) { #region Pose
objectId = data[0];
} public class PoseMsg : IMessage {
public const byte Id = 0x10;
public static async Task<bool> Receive(Stream dataStream, Client client, byte packetSize) { public const byte length = 3 + 4 + 4;
if (packetSize != length) public byte thingId;
return false;
public byte poseType;
byte[] buffer = await Receive(dataStream, packetSize); public const byte Pose_Position = 0x01;
DestroyMsg msg = new(buffer); public const byte Pose_Orientation = 0x02;
client.messageQueue.Enqueue(msg); public Spherical position;
return true; public Quat32 orientation;
}
} public PoseMsg(byte thingId, Spherical position, Quat32 orientation) {
this.thingId = thingId;
this.position = position;
#endregion Destroy 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 = new Quat32(0, 0, 0, 1);
}
public PoseMsg(byte[] data) : base(data) { }
public override byte[] Serialize() {
byte[] buffer = new byte[PoseMsg.length];
uint ix = 0;
buffer[ix++] = PoseMsg.Id;
buffer[ix++] = this.thingId;
buffer[ix++] = this.poseType;
LowLevelMessages.SendSpherical(buffer, ref ix, position);
LowLevelMessages.SendQuat32(buffer, ref ix, orientation);
return buffer;
}
public override void Deserialize(byte[] data) {
uint ix = 0;
thingId = data[ix++];
poseType = data[ix++];
//if ((poseType & Pose_Position) != 0)
position = LowLevelMessages.ReceiveSpherical(data, ref ix);
//if ((poseType & Pose_Orientation) != 0) {
orientation = LowLevelMessages.ReceiveQuat32(data, ref ix);
}
public static bool Send(Client client, byte thingId, Spherical position, Quat32 orientation) {
PoseMsg msg = new(thingId, position, orientation);
return SendMsg(client, msg);
}
public static async Task<bool> Receive(Stream dataStream, Client client, byte packetSize) {
if (packetSize != length)
return false;
byte[] buffer = await Receive(dataStream, packetSize);
PoseMsg msg = new(buffer);
client.messageQueue.Enqueue(msg);
return true;
}
}
#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[] data) : base(data) { }
public CustomMsg(byte networkId, byte thingId, byte[] bytes) : base() {
this.networkId = networkId;
this.thingId = thingId;
this.bytes = bytes;
}
public override byte[] Serialize() {
byte[] buffer = new byte[4 + this.bytes.Length];
int 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 buffer;
}
public override void Deserialize(byte[] data) {
uint ix = 0;
this.thingId = data[ix++];
this.bytes = new byte[data.Length - ix];
for (uint bytesIx = 0; ix < data.Length; ix++, bytesIx++)
this.bytes[bytesIx] = data[ix];
}
public static void Send(Client client, byte thingId, byte[] bytes) {
CustomMsg msg = new(client.networkId, thingId, bytes);
SendMsg(client, msg);
}
public static async Task<bool> Receive(Stream dataStream, Client 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[] data) : base(data) { }
public override void Deserialize(byte[] data) {
uint ix = 0;
uint strlen = data[ix++];
this.text = System.Text.Encoding.UTF8.GetString(data, (int)ix, (int)strlen);
}
public static async Task<bool> Receive(Stream dataStream, Client 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 = 2;
public byte objectId;
public DestroyMsg(byte[] data) : base(data) { }
public override void Deserialize(byte[] data) {
objectId = data[0];
}
public static async Task<bool> Receive(Stream dataStream, Client 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
} }

64
SiteServer.cs Normal file
View File

@ -0,0 +1,64 @@
using System.IO;
using System.Threading.Tasks;
namespace Passer.Control {
public static class SiteServer {
public static async Task ReceiveData(Stream dataStream, Client client) {
while (true) {
byte packetSize = (byte)dataStream.ReadByte();
if (packetSize != 0xFF)
await ReceiveData(dataStream, client, packetSize);
// else timeout
}
}
public static async Task ReceiveData(Stream dataStream, Client client, byte packetSize) {
byte msgId = (byte)dataStream.ReadByte();
if (msgId == 0xFF) {
// Timeout
return;
}
bool result = false;
switch (msgId) {
case ClientMsg.Id: // 0xA0 / 160
result = await ClientMsg.Receive(dataStream, client, packetSize);
break;
case NetworkIdMsg.Id: // 0xA1 / 161
result = await NetworkIdMsg.Receive(dataStream, client, packetSize);
break;
case InvestigateMsg.Id: // 0x81
result = await InvestigateMsg.Receive(dataStream, client, packetSize);
break;
case ThingMsg.Id: // 0x80 / 128
result = await ThingMsg.Receive(dataStream, client, packetSize);
break;
case NameMsg.Id: // 0x91 / 145
result = await NameMsg.Receive(dataStream, client, packetSize);
break;
case ModelUrlMsg.Id: // 0x90 / 144
result = await ModelUrlMsg.Receive(dataStream, client, packetSize);
break;
case PoseMsg.Id: // 0x10 / 16
result = await PoseMsg.Receive(dataStream, client, packetSize);
break;
case CustomMsg.Id: // 0xB1 / 177
result = await CustomMsg.Receive(dataStream, client, packetSize);
break;
case TextMsg.Id: // 0xB0 / 176
result = await TextMsg.Receive(dataStream, client, packetSize);
break;
case DestroyMsg.Id: // 0x20 / 32
result = await DestroyMsg.Receive(dataStream, client, packetSize);
break;
default:
break;
}
if (result == false) {
packetSize = msgId; // skip 1 byte, msgId is possibly a packet size byte
await ReceiveData(dataStream, client, packetSize);
}
}
}
}

2
SiteServer.cs.meta Normal file
View File

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