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

Alinged with Python code

Browse Source
This commit is contained in:
Pascal Serrarens 2025-01-01 09:58:15 +01:00
parent f99d4fb1b7
commit b9e28dc421
10 changed files with 624 additions and 487 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
.editorconfig Normal file
View File

@ -0,0 +1,15 @@
# EditorConfig is awesome: https://EditorConfig.org
# top-most EditorConfig file
root = true
[*]
indent_style = space
indent_size = 4
end_of_line = crlf
charset = utf-8
trim_trailing_whitespace = false
insert_final_newline = false
[*.cs]
csharp_new_line_before_open_brace = none

73
ClientMsg.cs Normal file
View File

@ -0,0 +1,73 @@
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) : base(buffer) { }
public override byte Serialize(ref byte[] buffer) {
byte ix = 0;
buffer[ix++] = ClientMsg.Id;
buffer[ix++] = networkId;
return ClientMsg.length;
}
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 byte Serialized(byte[] buffer, byte networkId) {
byte ix = 0;
buffer[ix++] = ClientMsg.Id;
buffer[ix++] = networkId;
return ix;
}
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);
}
}
}

360
Messages.cs
View File

@ -40,123 +40,6 @@ namespace Passer.Control.Core {
} }
} }
#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 byte Serialized(byte[] buffer, byte networkId) {
byte ix = 0;
buffer[ix++] = ClientMsg.Id;
buffer[ix++] = networkId;
return ix;
}
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 #region Investigate
public class InvestigateMsg : IMessage { public class InvestigateMsg : IMessage {
@ -204,249 +87,6 @@ namespace Passer.Control.Core {
#endregion Investigate #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 #region Pose
public class PoseMsg : IMessage { public class PoseMsg : IMessage {

83
ModelUrlMsg.cs Normal file
View File

@ -0,0 +1,83 @@
namespace Passer.Control.Core {
public class ModelUrlMsg : IMessage {
public const byte Id = 0x90; // (144) Model URL
public const byte length = 4;
public byte networkId;
public byte thingId;
public string? url = null;
public ModelUrlMsg(byte networkId, Thing thing) {
this.networkId = networkId;
this.thingId = thing.id;
this.url = thing.modelUrl;
}
public ModelUrlMsg(byte networkId, byte thingId, string url, float scale = 1) {
this.networkId = networkId;
this.thingId = thingId;
this.url = url;
}
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
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++];
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
participant.buffer[ix++] = urlLength;
for (int urlIx = 0; urlIx < urlLength; urlIx++)
participant.buffer[ix++] = (byte)thing.modelUrl[urlIx];
return participant.SendBuffer(ix);
}
}
}

81
NameMsg.cs Normal file
View File

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

50
NetworkIdMsg.cs Normal file
View File

@ -0,0 +1,50 @@
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;
}
NetworkIdMsg(byte[] buffer) : base(buffer) { }
public override byte Serialize(ref byte[] buffer) {
buffer[0] = NetworkIdMsg.Id;
buffer[1] = this.networkId;
return NetworkIdMsg.length;
}
public override void Deserialize(byte[] buffer) {
uint ix = 1;
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);
}
}
}

179
Participant.cs
View File

@ -5,15 +5,13 @@ using System.Net.Sockets;
using System.IO; using System.IO;
using System.Threading.Tasks; using System.Threading.Tasks;
namespace Passer.Control.Core namespace Passer.Control.Core {
{
public class Participant public class Participant {
{
//public ConnectionMethod connection; //public ConnectionMethod connection;
public IPEndPoint endPoint; public IPEndPoint? endPoint = null;
public UdpClient udpClient; public UdpClient? udpClient = null;
public string ipAddress; public string ipAddress = "0.0.0.0";
public string broadcastIpAddress = "255.255.255.255"; public string broadcastIpAddress = "255.255.255.255";
public int port; public int port;
public Stream dataStream; public Stream dataStream;
@ -24,10 +22,8 @@ namespace Passer.Control.Core
public readonly ConcurrentQueue<IMessage> messageQueue = new(); public readonly ConcurrentQueue<IMessage> messageQueue = new();
public Participant GetClient(string ipAddress, int port) public Participant? GetClient(string ipAddress, int port) {
{ foreach (Participant c in others) {
foreach (Participant c in others)
{
if (c.ipAddress == ipAddress && c.port == port) if (c.ipAddress == ipAddress && c.port == port)
return c; return c;
} }
@ -37,14 +33,26 @@ namespace Passer.Control.Core
#region Init #region Init
public Participant() public Participant() {
{
this.dataStream = new EchoStream(); this.dataStream = new EchoStream();
others.Add(this); others.Add(this);
} }
public Participant(UdpClient udpClient, int port) : this() /// <summary>
{ /// Create a new participant
/// </summary>
/// <param name="ipAddress">The ip address of the site server</param>
/// <param name="port">The port number of the site server</param>
public Participant(string ipAddress = "0.0.0.0", int port = 7681) : this() {
this.ipAddress = ipAddress;
this.port = port;
this.endPoint = new IPEndPoint(IPAddress.Parse(ipAddress), port); // for sending
this.udpClient = new UdpClient(); // for receiving
this.udpClient.Client.Bind(new IPEndPoint(IPAddress.Any, 0));
this.udpClient.BeginReceive(new AsyncCallback(result => ReceiveUDP(result)), null);
}
public Participant(UdpClient udpClient, int port) : this() {
this.udpClient = udpClient; this.udpClient = udpClient;
this.ipAddress = null; this.ipAddress = null;
this.port = port; this.port = port;
@ -56,51 +64,88 @@ namespace Passer.Control.Core
#region Update #region Update
private static readonly float publishInterval = 3.0f; protected void ReceiveUDP(IAsyncResult result) {
private float nextPublishMe = 0; if (udpClient == null || this.endPoint == null)
return;
try {
public virtual void Update(float currentTime) byte[] data = udpClient.EndReceive(result, ref this.endPoint);
{ this.dataStream.WriteByte((byte)data.Length);
if (currentTime > this.nextPublishMe) this.dataStream.Write(data, 0, data.Length);
{ Task task = Task.Run(() => ReceiveData());
this.PublishBuffer(ClientMsg.Serialized(this.buffer, this.networkId)); }
ClientMsg.Publish(this, this.networkId); catch (Exception _) {
this.nextPublishMe = currentTime + Participant.publishInterval; Console.WriteLine("connection error");
}
udpClient.BeginReceive(new AsyncCallback(result => ReceiveUDP(result)), null);
} }
for (int ix = 0; ix < this.others.Count; ix++) private static readonly long publishInterval = 3000;
{ private long nextPublishMe = 0;
public virtual void Update(long currentTimeMs) {
if (currentTimeMs > this.nextPublishMe) {
this.Publish(new ClientMsg(this.networkId));
Console.WriteLine($"Sent ClientMsg {this.networkId}");
this.nextPublishMe = currentTimeMs + Participant.publishInterval;
}
for (int ix = 0; ix < this.others.Count; ix++) {
Participant client = this.others[ix]; Participant client = this.others[ix];
if (client == null) if (client == null)
continue; continue;
client.ProcessMessages(); client.ProcessMessages();
} }
Thing.UpdateAll(currentTime); Thing.UpdateAll(currentTimeMs);
} }
#endregion Update #endregion Update
#region Send #region Send
public bool SendBuffer(int bufferSize) public void SendThingInfo(Thing thing) {
{ this.Send(new ThingMsg(this.networkId, thing));
this.Send(new NameMsg(this.networkId, thing));
this.Send(new ModelUrlMsg(this.networkId, thing));
}
public bool Send(IMessage msg) {
int bufferSize = msg.Serialize(ref this.buffer);
if (bufferSize <= 0)
return true;
Console.WriteLine($"msg to {endPoint.Address.ToString()} {endPoint.Port}");
this.udpClient?.Send(this.buffer, bufferSize, this.endPoint);
return true;
}
public bool Publish(IMessage msg) {
int bufferSize = msg.Serialize(ref this.buffer);
if (bufferSize <= 0)
return true;
//Console.WriteLine($"publish to {broadcastIpAddress.ToString()} {this.port}");
this.udpClient?.Send(this.buffer, bufferSize, this.broadcastIpAddress, this.port);
return true;
}
public bool SendBuffer(int bufferSize) {
//if (this.ipAddress == null) //if (this.ipAddress == null)
// return false; // return false;
// UnityEngine.Debug.Log($"Send msg {buffer[0]} to {ipAddress}"); // UnityEngine.Debug.Log($"Send msg {buffer[0]} to {ipAddress}");
//this.udpClient.Send(this.buffer, bufferSize, this.ipAddress, this.port); //this.udpClient.Send(this.buffer, bufferSize, this.ipAddress, this.port);
this.udpClient.Send(this.buffer, bufferSize, this.endPoint); this.udpClient?.Send(this.buffer, bufferSize, this.endPoint);
return true; return true;
} }
public bool PublishBuffer(int bufferSize) public bool PublishBuffer(int bufferSize) {
{
if (this.broadcastIpAddress == null) if (this.broadcastIpAddress == null)
return false; return false;
this.udpClient.Send(this.buffer, bufferSize, this.broadcastIpAddress, this.port); this.udpClient?.Send(this.buffer, bufferSize, this.broadcastIpAddress, this.port);
return true; return true;
} }
@ -108,10 +153,8 @@ namespace Passer.Control.Core
#region Receive #region Receive
public async Task ReceiveData() public async Task ReceiveData() {
{ while (true) {
while (true)
{
byte packetSize = (byte)this.dataStream.ReadByte(); byte packetSize = (byte)this.dataStream.ReadByte();
if (packetSize != 0xFF) if (packetSize != 0xFF)
await ReceiveData(this.dataStream, this, packetSize); await ReceiveData(this.dataStream, this, packetSize);
@ -119,19 +162,16 @@ namespace Passer.Control.Core
} }
} }
public static async Task ReceiveData(Stream dataStream, Participant client, byte packetSize) public static async Task ReceiveData(Stream dataStream, Participant client, byte packetSize) {
{
byte msgId = (byte)dataStream.ReadByte(); byte msgId = (byte)dataStream.ReadByte();
if (msgId == 0xFF) if (msgId == 0xFF) {
{
// Timeout // Timeout
return; return;
} }
//UnityEngine.Debug.Log($"R {msgId} from {client.ipAddress}"); //UnityEngine.Debug.Log($"R {msgId} from {client.ipAddress}");
bool result = false; bool result = false;
switch (msgId) switch (msgId) {
{
case ClientMsg.Id: // 0xA0 / 160 case ClientMsg.Id: // 0xA0 / 160
result = await ClientMsg.Receive(dataStream, client, packetSize); result = await ClientMsg.Receive(dataStream, client, packetSize);
break; break;
@ -165,8 +205,7 @@ namespace Passer.Control.Core
default: default:
break; break;
} }
if (result == false) if (result == false) {
{
packetSize = msgId; // skip 1 byte, msgId is possibly a packet size byte packetSize = msgId; // skip 1 byte, msgId is possibly a packet size byte
await ReceiveData(dataStream, client, packetSize); await ReceiveData(dataStream, client, packetSize);
} }
@ -176,16 +215,13 @@ namespace Passer.Control.Core
#region Process #region Process
public virtual void ProcessMessages() public virtual void ProcessMessages() {
{
while (this.messageQueue.TryDequeue(out IMessage msg)) while (this.messageQueue.TryDequeue(out IMessage msg))
ProcessMessage(msg); ProcessMessage(msg);
} }
public void ProcessMessage(IMessage msg) public void ProcessMessage(IMessage msg) {
{ switch (msg) {
switch (msg)
{
case ClientMsg clientMsg: case ClientMsg clientMsg:
ProcessClient(clientMsg); ProcessClient(clientMsg);
break; break;
@ -223,30 +259,41 @@ namespace Passer.Control.Core
ForwardMessage(msg); ForwardMessage(msg);
} }
protected virtual void ProcessClient(ClientMsg client) { } protected virtual void ProcessClient(ClientMsg msg) { }
protected virtual void ProcessNetworkId(NetworkIdMsg networkId) { } protected virtual void ProcessNetworkId(NetworkIdMsg msg) {
if (this.networkId != msg.networkId) {
this.networkId = msg.networkId;
Console.WriteLine($"receive network id {msg.networkId}");
foreach (Thing thing in Thing.GetAllThings())
this.SendThingInfo(thing);
}
}
protected virtual void ProcessInvestigate(InvestigateMsg investigate) { } protected virtual void ProcessInvestigate(InvestigateMsg msg) { }
protected virtual void ProcessThing(ThingMsg thing) { } protected virtual void ProcessThing(ThingMsg msg) {
Console.WriteLine($"received thing {msg.thingId}");
}
protected virtual void ProcessName(NameMsg name) { } protected virtual void ProcessName(NameMsg msg) {
Console.WriteLine($"received name {msg.name}");
}
protected virtual void ProcessModelUrl(ModelUrlMsg modelUrl) { } protected virtual void ProcessModelUrl(ModelUrlMsg msg) {
Console.WriteLine($"received name {msg.url}");
}
protected virtual void ProcessPose(PoseMsg pose) { } protected virtual void ProcessPose(PoseMsg msg) { }
protected virtual void ProcessCustom(CustomMsg custom) { } protected virtual void ProcessCustom(CustomMsg msg) { }
protected virtual void ProcessText(TextMsg text) { } protected virtual void ProcessText(TextMsg temsgxt) { }
protected virtual void ProcessDestroy(DestroyMsg destroy) { } protected virtual void ProcessDestroy(DestroyMsg msg) { }
private void ForwardMessage(IMessage msg) private void ForwardMessage(IMessage msg) {
{ foreach (Participant client in others) {
foreach (Participant client in others)
{
if (client == this) if (client == this)
continue; continue;
//UnityEngine.Debug.Log($"---> {client.ipAddress}"); //UnityEngine.Debug.Log($"---> {client.ipAddress}");

135
SiteServer.cs
View File

@ -1,66 +1,87 @@
using System.IO; using System.Net;
using System.Threading.Tasks; using System.Net.Sockets;
namespace Passer.Control.Core { namespace Passer.Control.Core {
public static class SiteServer { public class SiteServer : Participant {
public SiteServer(int port) {
this.ipAddress = "0.0.0.0";
this.port = port;
this.endPoint = new IPEndPoint(IPAddress.Parse(ipAddress), 0); // for sending, but should not be used really...
this.udpClient = new UdpClient(); // for receiving
this.udpClient.Client.Bind(new IPEndPoint(IPAddress.Any, port));
this.udpClient.BeginReceive(new AsyncCallback(result => ReceiveUDP(result)), null);
public static async Task ReceiveData(Stream dataStream, Participant 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, Participant client, byte packetSize) { protected override void ProcessClient(ClientMsg msg) {
byte msgId = (byte)dataStream.ReadByte(); if (msg.networkId == 0) {
if (msgId == 0xFF) { this.Send(new NetworkIdMsg(this.networkId));
// Timeout Console.WriteLine($"############################ New Client -> {this.networkId}");
return;
} }
else
Console.WriteLine($"############################ New Client");
}
}
// public static class SiteServer {
//UnityEngine.Debug.Log($"R {msgId} from {client.ipAddress}"); // public static async Task ReceiveData(Stream dataStream, Participant client) {
bool result = false; // while (true) {
switch (msgId) { // byte packetSize = (byte)dataStream.ReadByte();
case ClientMsg.Id: // 0xA0 / 160 // if (packetSize != 0xFF)
result = await ClientMsg.Receive(dataStream, client, packetSize); // await ReceiveData(dataStream, client, packetSize);
break; // // else timeout
case NetworkIdMsg.Id: // 0xA1 / 161 // }
result = await NetworkIdMsg.Receive(dataStream, client, packetSize); // }
break;
case InvestigateMsg.Id: // 0x81 // public static async Task ReceiveData(Stream dataStream, Participant client, byte packetSize) {
result = await InvestigateMsg.Receive(dataStream, client, packetSize); // byte msgId = (byte)dataStream.ReadByte();
break; // if (msgId == 0xFF) {
case ThingMsg.id: // 0x80 / 128 // // Timeout
result = await ThingMsg.Receive(dataStream, client, packetSize); // return;
break; // }
case NameMsg.Id: // 0x91 / 145
result = await NameMsg.Receive(dataStream, client, packetSize); // //UnityEngine.Debug.Log($"R {msgId} from {client.ipAddress}");
break; // bool result = false;
case ModelUrlMsg.Id: // 0x90 / 144 // switch (msgId) {
result = await ModelUrlMsg.Receive(dataStream, client, packetSize); // case ClientMsg.Id: // 0xA0 / 160
break; // result = await ClientMsg.Receive(dataStream, client, packetSize);
case PoseMsg.Id: // 0x10 / 16 // break;
result = await PoseMsg.Receive(dataStream, client, packetSize); // case NetworkIdMsg.Id: // 0xA1 / 161
break; // result = await NetworkIdMsg.Receive(dataStream, client, packetSize);
case CustomMsg.Id: // 0xB1 / 177 // break;
result = await CustomMsg.Receive(dataStream, client, packetSize); // case InvestigateMsg.Id: // 0x81
break; // result = await InvestigateMsg.Receive(dataStream, client, packetSize);
case TextMsg.Id: // 0xB0 / 176 // break;
result = await TextMsg.Receive(dataStream, client, packetSize); // case ThingMsg.id: // 0x80 / 128
break; // result = await ThingMsg.Receive(dataStream, client, packetSize);
case DestroyMsg.Id: // 0x20 / 32 // break;
result = await DestroyMsg.Receive(dataStream, client, packetSize); // case NameMsg.Id: // 0x91 / 145
break; // result = await NameMsg.Receive(dataStream, client, packetSize);
default: // break;
break; // case ModelUrlMsg.Id: // 0x90 / 144
} // result = await ModelUrlMsg.Receive(dataStream, client, packetSize);
if (result == false) { // break;
packetSize = msgId; // skip 1 byte, msgId is possibly a packet size byte // case PoseMsg.Id: // 0x10 / 16
await ReceiveData(dataStream, client, packetSize); // 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);
// }
// }
// }
} }

86
ThingMsg.cs Normal file
View File

@ -0,0 +1,86 @@
namespace Passer.Control.Core {
public class ThingMsg : IMessage {
public const byte length = 5;
public const byte id = 0x80;
public byte networkId;
public byte thingId;
public byte thingType;
public byte parentId;
public ThingMsg(byte networkId, Thing thing) {
this.networkId = networkId;
this.thingId = thing.id;
this.thingType = thing.type;
this.parentId = thing.parent.id;
}
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 ThingMsg.length;
}
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);
}
}
}

53
test/UnitTest1.cs
View File

@ -2,16 +2,57 @@
using Passer.Control.Core; using Passer.Control.Core;
public class Tests public class Tests {
{
[SetUp] [SetUp]
public void Setup() public void Setup() {
{
} }
[Test] [Test]
public void Test1() public void Test_Client() {
{ Participant participant = new("127.0.0.1", 7681);
long milliseconds = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();
long startTime = milliseconds;
while (milliseconds < startTime + 7000) {
participant.Update(milliseconds);
milliseconds = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();
Thread.Sleep(100);
}
Assert.Pass();
}
[Test]
public void Test_SiteServer() {
SiteServer siteServer = new(7681);
long milliseconds = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();
long startTime = milliseconds;
while (milliseconds < startTime + 7000) {
siteServer.Update(milliseconds);
milliseconds = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();
Thread.Sleep(100);
}
Assert.Pass();
}
[Test]
public void Test_ThingMsg() {
Participant participant = new("127.0.0.1");
Thing thing = new() {
name = "First Thing",
modelUrl = "https://passer.life/extras/ant.jpg"
};
long milliseconds = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();
long startTime = milliseconds;
while (milliseconds < startTime + 7000) {
participant.Update(milliseconds);
milliseconds = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();
Thread.Sleep(100);
}
Assert.Pass(); Assert.Pass();
} }
} }