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() { void OnApplicationQuit() {
site.Close(); if (site != null)
site.Close();
} }
public void HandleNewThing(RoboidControl.Thing thing) { public void HandleNewThing(RoboidControl.Thing thing) {

4
Unity/Thing.cs
View File

@ -31,7 +31,7 @@ namespace RoboidControl.Unity {
core.component = this; core.component = this;
SiteServer siteServer = FindAnyObjectByType<SiteServer>(); SiteServer siteServer = FindAnyObjectByType<SiteServer>();
if (siteServer == null) { if (siteServer == null || siteServer.site == null) {
Debug.LogWarning("No site server found"); Debug.LogWarning("No site server found");
return; return;
} }
@ -93,7 +93,7 @@ namespace RoboidControl.Unity {
} }
private void PoseChanged() { private void PoseChanged() {
Debug.Log($"{this} pose changed"); // Debug.Log($"{this} pose changed");
if (core.positionUpdated) if (core.positionUpdated)
this.transform.localPosition = core.position.ToVector3(); this.transform.localPosition = core.position.ToVector3();
if (core.orientationUpdated) 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 /// The length of the message, excluding the binary data
/// </summary> /// </summary>
/// For the total size of the message this.bytes.Length should be added to this value. /// 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> /// <summary>
/// The network ID identifying the thing /// The network ID identifying the thing
/// </summary> /// </summary>
@ -23,23 +23,15 @@ namespace RoboidControl {
public byte thingId; public byte thingId;
public Thing thing; public Thing thing;
/// <summary>
/// The length of the data
/// </summary>
public byte dataLength;
/// <summary> /// <summary>
/// The binary data /// The binary data
/// </summary> /// </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> /// <summary>
/// Create an empty message for sending /// Create an empty message for sending
/// </summary> /// </summary>
@ -49,31 +41,32 @@ namespace RoboidControl {
this.networkId = networkId; this.networkId = networkId;
this.thingId = thing.id; this.thingId = thing.id;
this.thing = thing; this.thing = thing;
this.bytes = this.thing.GenerateBinary(); this.data = this.thing.GenerateBinary();
// if (this.bytes.Length > 0) this.dataLength = (byte)this.data.Length;
// System.Console.Write($"Binary message for [{networkId}/{thing.id}]");
} }
/// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer) /// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer)
public BinaryMsg(byte[] buffer) { public BinaryMsg(byte[] buffer) {
byte ix = 1; byte ix = 1;
this.networkId = buffer[ix++]; this.networkId = buffer[ix++];
this.thingId = buffer[ix++]; this.thingId = buffer[ix++];
byte length = (byte)(buffer.Length - ix); this.dataLength = buffer[ix++];
this.bytes = new byte[length]; this.data = new byte[this.dataLength];
for (uint bytesIx = 0; bytesIx < length; bytesIx++) for (uint dataIx = 0; dataIx < this.dataLength; dataIx++)
this.bytes[bytesIx] = buffer[ix++]; this.data[dataIx] = buffer[ix++];
} }
/// @copydoc Passer::RoboidControl::IMessage::Serialize /// @copydoc Passer::RoboidControl::IMessage::Serialize
public override byte Serialize(ref byte[] buffer) { 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; return 0;
System.Console.Write($"sending Binary [{this.networkId}/{this.thingId}]");
byte ix = 0; byte ix = 0;
buffer[ix++] = BinaryMsg.Id; buffer[ix++] = BinaryMsg.Id;
buffer[ix++] = this.networkId; buffer[ix++] = this.networkId;
buffer[ix++] = this.thingId; buffer[ix++] = this.thingId;
foreach (byte b in bytes) buffer[ix++] = this.dataLength;
foreach (byte b in data)
buffer[ix++] = b; buffer[ix++] = b;
return ix; return ix;

2
src/Messages/ModelUrlMsg.cs
View File

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

27
src/Messages/NameMsg.cs
View File

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

41
src/ParticipantUDP.cs
View File

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