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

Communication improvements, adding gyro msg

Browse Source
This commit is contained in:
Pascal Serrarens 2025-04-10 17:35:26 +02:00
parent 05d4a2acd9
commit 6699200195
8 changed files with 56 additions and 67 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
Unity/SiteServer.cs
View File

@ -18,7 +18,8 @@ namespace RoboidControl.Unity {
}
void OnApplicationQuit() {
site.Close();
if (site != null)
site.Close();
}
public void HandleNewThing(RoboidControl.Thing thing) {

4
Unity/Thing.cs
View File

@ -31,7 +31,7 @@ namespace RoboidControl.Unity {
core.component = this;
SiteServer siteServer = FindAnyObjectByType<SiteServer>();
if (siteServer == null) {
if (siteServer == null || siteServer.site == null) {
Debug.LogWarning("No site server found");
return;
}
@ -93,7 +93,7 @@ namespace RoboidControl.Unity {
}
private void PoseChanged() {
Debug.Log($"{this} pose changed");
// Debug.Log($"{this} pose changed");
if (core.positionUpdated)
this.transform.localPosition = core.position.ToVector3();
if (core.orientationUpdated)

41
src/Messages/BinaryMsg.cs
View File

@ -12,7 +12,7 @@ namespace RoboidControl {
/// The length of the message, excluding the binary data
/// </summary>
/// For the total size of the message this.bytes.Length should be added to this value.
public const byte length = 2;
public const byte length = 4;
/// <summary>
/// The network ID identifying the thing
/// </summary>
@ -23,23 +23,15 @@ namespace RoboidControl {
public byte thingId;
public Thing thing;
/// <summary>
/// The length of the data
/// </summary>
public byte dataLength;
/// <summary>
/// The binary data
/// </summary>
public byte[] bytes;
public byte[] data;
/// <summary>
/// Create a new message for sending
/// </summary>
/// <param name="networkId">The netowork ID of the thing</param>
/// <param name="thingId">The ID of the thing</param>
/// <param name="bytes">The binary data for the thing</param>
public BinaryMsg(byte networkId, byte thingId, byte[] bytes) : base() {
this.networkId = networkId;
this.thingId = thingId;
this.bytes = bytes;
}
/// <summary>
/// Create an empty message for sending
/// </summary>
@ -49,31 +41,32 @@ namespace RoboidControl {
this.networkId = networkId;
this.thingId = thing.id;
this.thing = thing;
this.bytes = this.thing.GenerateBinary();
// if (this.bytes.Length > 0)
// System.Console.Write($"Binary message for [{networkId}/{thing.id}]");
this.data = this.thing.GenerateBinary();
this.dataLength = (byte)this.data.Length;
}
/// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer)
public BinaryMsg(byte[] buffer) {
byte ix = 1;
this.networkId = buffer[ix++];
this.thingId = buffer[ix++];
byte length = (byte)(buffer.Length - ix);
this.bytes = new byte[length];
for (uint bytesIx = 0; bytesIx < length; bytesIx++)
this.bytes[bytesIx] = buffer[ix++];
this.dataLength = buffer[ix++];
this.data = new byte[this.dataLength];
for (uint dataIx = 0; dataIx < this.dataLength; dataIx++)
this.data[dataIx] = buffer[ix++];
}
/// @copydoc Passer::RoboidControl::IMessage::Serialize
public override byte Serialize(ref byte[] buffer) {
if (buffer.Length < BinaryMsg.length + this.bytes.Length || this.bytes.Length == 0)
if (buffer.Length < BinaryMsg.length + this.data.Length || this.data.Length == 0)
return 0;
System.Console.Write($"sending Binary [{this.networkId}/{this.thingId}]");
byte ix = 0;
buffer[ix++] = BinaryMsg.Id;
buffer[ix++] = this.networkId;
buffer[ix++] = this.thingId;
foreach (byte b in bytes)
buffer[ix++] = this.dataLength;
foreach (byte b in data)
buffer[ix++] = b;
return ix;

2
src/Messages/ModelUrlMsg.cs
View File

@ -58,7 +58,7 @@ namespace RoboidControl {
/// @copydoc Passer::RoboidControl::IMessage::Serialize
public override byte Serialize(ref byte[] buffer) {
if (this.url == null)
if (string.IsNullOrEmpty(this.url))
return 0;
byte ix = 0;

27
src/Messages/NameMsg.cs
View File

@ -20,10 +20,11 @@ namespace RoboidControl {
/// The ID of the thing
/// </summary>
public byte thingId;
/// <summary>
/// The length of the name, excluding null terminator
/// </summary>
public byte len;
public byte nameLength;
/// <summary>
/// The name of the thing, not terminated with a null character
/// </summary>
@ -38,6 +39,7 @@ namespace RoboidControl {
this.networkId = networkId;
this.thingId = thing.id;
this.name = thing.name;
this.nameLength = (byte)this.name.Length;
}
/// <summary>
/// Create a new message for sending
@ -45,34 +47,31 @@ namespace RoboidControl {
/// <param name="networkId">The network ID of the thing</param>
/// <param name="thingId">The ID of the thing</param>
/// <param name="name">The name of the thing</param>
public NameMsg(byte networkId, byte thingId, string name) {
this.networkId = networkId;
this.thingId = thingId;
this.name = name;
}
// public NameMsg(byte networkId, byte thingId, string name) {
// this.networkId = networkId;
// this.thingId = thingId;
// this.name = name;
// }
/// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer)
public NameMsg(byte[] buffer) {
byte ix = 1;
this.networkId = buffer[ix++];
this.thingId = buffer[ix++];
int strlen = buffer[ix++];
this.name = System.Text.Encoding.UTF8.GetString(buffer, (int)ix, strlen);
this.nameLength = buffer[ix++];
this.name = System.Text.Encoding.UTF8.GetString(buffer, (int)ix, this.nameLength);
}
/// @copydoc Passer::RoboidControl::IMessage::Serialize
public override byte Serialize(ref byte[] buffer) {
if (buffer.Length < NameMsg.length + this.name.Length || this.name.Length == 0)
if (buffer.Length < NameMsg.length + this.name.Length || string.IsNullOrEmpty(this.name))
return 0;
byte ix = 0;
buffer[ix++] = NameMsg.Id;
buffer[ix++] = this.networkId;
buffer[ix++] = this.thingId;
int nameLength = this.name.Length;
buffer[ix++] = (byte)nameLength;
for (int nameIx = 0; nameIx < nameLength; nameIx++)
buffer[ix++] = this.nameLength;
for (int nameIx = 0; nameIx < this.nameLength; nameIx++)
buffer[ix++] = (byte)this.name[nameIx];
return ix;

2
src/Messages/ThingMsg.cs
View File

@ -35,7 +35,6 @@ namespace RoboidControl {
/// <param name="networkId">The network ID of the thing</param>
/// <param name="thing">The thing</param>
public ThingMsg(byte networkId, Thing thing) {
System.Console.Write($"ThingMsg [{networkId}/{thing.id}] {thing.type}");
this.networkId = networkId;
this.thingId = thing.id;
this.thingType = thing.type;
@ -84,7 +83,6 @@ namespace RoboidControl {
buffer[ix++] = this.thingId;
buffer[ix++] = this.thingType;
buffer[ix++] = this.parentId;
System.Console.Write("ThingMsg sent");
return ThingMsg.length;
}
}

41
src/ParticipantUDP.cs
View File

@ -176,7 +176,7 @@ namespace RoboidControl {
#region Send
public void SendThingInfo(Participant owner, Thing thing) {
Console.WriteLine("Send thing info");
// Console.WriteLine("Send thing info");
this.Send(owner, new ThingMsg(this.networkId, thing));
this.Send(owner, new NameMsg(this.networkId, thing));
this.Send(owner, new ModelUrlMsg(this.networkId, thing));
@ -195,7 +195,7 @@ namespace RoboidControl {
}
public void PublishThingInfo(Thing thing) {
Console.WriteLine("Publish thing info");
// Console.WriteLine("Publish thing info");
this.Publish(new ThingMsg(this.networkId, thing));
this.Publish(new NameMsg(this.networkId, thing));
this.Publish(new ModelUrlMsg(this.networkId, thing));
@ -207,7 +207,7 @@ namespace RoboidControl {
if (bufferSize <= 0)
return true;
// Console.WriteLine($"publish to {broadcastIpAddress.ToString()} {this.port}");
Console.WriteLine($"publish to {broadcastIpAddress.ToString()} {this.port}");
this.udpClient?.Send(this.buffer, bufferSize, this.broadcastIpAddress, this.port);
return true;
}
@ -234,38 +234,39 @@ namespace RoboidControl {
#region Receive
public void ReceiveData(byte[] data, Participant remoteParticipant) {
public void ReceiveData(byte[] data, Participant sender) {
byte msgId = data[0];
if (msgId == 0xFF) {
// Timeout
return;
}
System.Console.Write($"Receive msg {msgId}");
switch (msgId) {
case ParticipantMsg.Id: // 0xA0 / 160
this.Process(remoteParticipant, new ParticipantMsg(data));
this.Process(sender, new ParticipantMsg(data));
break;
case NetworkIdMsg.Id: // 0xA1 / 161
this.Process(remoteParticipant, new NetworkIdMsg(data));
this.Process(sender, new NetworkIdMsg(data));
break;
case InvestigateMsg.Id: // 0x81
// result = await InvestigateMsg.Receive(dataStream, client, packetSize);
break;
case ThingMsg.id: // 0x80 / 128
this.Process(remoteParticipant, new ThingMsg(data));
this.Process(sender, new ThingMsg(data));
break;
case NameMsg.Id: // 0x91 / 145
this.Process(remoteParticipant, new NameMsg(data));
this.Process(sender, new NameMsg(data));
break;
case ModelUrlMsg.Id: // 0x90 / 144
this.Process(remoteParticipant, new ModelUrlMsg(data));
this.Process(sender, new ModelUrlMsg(data));
break;
case PoseMsg.Id: // 0x10 / 16
this.Process(remoteParticipant, new PoseMsg(data));
this.Process(sender, new PoseMsg(data));
// result = await PoseMsg.Receive(dataStream, client, packetSize);
break;
case BinaryMsg.Id: // 0xB1 / 177
this.Process(remoteParticipant, new BinaryMsg(data));
this.Process(sender, new BinaryMsg(data));
break;
case TextMsg.Id: // 0xB0 / 176
// result = await TextMsg.Receive(dataStream, client, packetSize);
@ -316,17 +317,13 @@ namespace RoboidControl {
}
protected virtual void Process(Participant sender, PoseMsg msg) {
Console.WriteLine($"Participant: Process pose [{msg.networkId}/{msg.thingId}] {msg.poseType}");
// Console.WriteLine($"Participant: Process pose [{msg.networkId}/{msg.thingId}] {msg.poseType}");
Thing thing = sender.Get(msg.networkId, msg.thingId);
if (thing != null) {
if ((msg.poseType & PoseMsg.Pose_Position) != 0) {
thing.position = msg.position;
Console.Write($"{thing.id} position changed");
}
if ((msg.poseType & PoseMsg.Pose_Orientation) != 0) {
thing.orientation = msg.orientation;
Console.Write($"{thing.id} orientation changed");
}
if ((msg.poseType & PoseMsg.Pose_Position) != 0)
thing.position = msg.position;
if ((msg.poseType & PoseMsg.Pose_Orientation) != 0)
thing.orientation = msg.orientation;
if ((msg.poseType & PoseMsg.Pose_LinearVelocity) != 0)
thing.linearVelocity = msg.linearVelocity;
if ((msg.poseType & PoseMsg.Pose_AngularVelocity) != 0)
@ -335,9 +332,9 @@ namespace RoboidControl {
}
protected virtual void Process(Participant sender, BinaryMsg msg) {
// Console.WriteLine($"Participant: Process binary [{msg.networkId}/{msg.thingId}]");
Console.WriteLine($"Participant: Process binary [{msg.networkId}/{msg.thingId}]");
Thing thing = sender.Get(msg.networkId, msg.thingId);
thing?.ProcessBinary(msg.bytes);
thing?.ProcessBinary(msg.data);
}
protected virtual void Process(Participant sender, TextMsg temsgxt) { }

3
src/Thing.cs
View File

@ -49,7 +49,8 @@ namespace RoboidControl {
public Thing(Participant owner, byte thingType = (byte)Type.Undetermined, bool invokeEvent = true) {
this.owner = owner;
this.type = thingType;
this.owner.Add(this);
if (this.owner != null)
this.owner.Add(this);
if (invokeEvent)
InvokeNewThing(this);
}