diff --git a/Unity/SiteServer.cs b/Unity/SiteServer.cs index d1f4236..4dde64b 100644 --- a/Unity/SiteServer.cs +++ b/Unity/SiteServer.cs @@ -18,7 +18,8 @@ namespace RoboidControl.Unity { } void OnApplicationQuit() { - site.Close(); + if (site != null) + site.Close(); } public void HandleNewThing(RoboidControl.Thing thing) { diff --git a/Unity/Thing.cs b/Unity/Thing.cs index 98551fe..129f34b 100644 --- a/Unity/Thing.cs +++ b/Unity/Thing.cs @@ -31,7 +31,7 @@ namespace RoboidControl.Unity { core.component = this; SiteServer siteServer = FindAnyObjectByType(); - 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) diff --git a/src/Messages/BinaryMsg.cs b/src/Messages/BinaryMsg.cs index 6953188..408ba94 100644 --- a/src/Messages/BinaryMsg.cs +++ b/src/Messages/BinaryMsg.cs @@ -12,7 +12,7 @@ namespace RoboidControl { /// The length of the message, excluding the binary data /// /// 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; /// /// The network ID identifying the thing /// @@ -23,23 +23,15 @@ namespace RoboidControl { public byte thingId; public Thing thing; - + /// + /// The length of the data + /// + public byte dataLength; /// /// The binary data /// - public byte[] bytes; + public byte[] data; - /// - /// Create a new message for sending - /// - /// The netowork ID of the thing - /// The ID of the thing - /// The binary data for the thing - public BinaryMsg(byte networkId, byte thingId, byte[] bytes) : base() { - this.networkId = networkId; - this.thingId = thingId; - this.bytes = bytes; - } /// /// Create an empty message for sending /// @@ -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; diff --git a/src/Messages/ModelUrlMsg.cs b/src/Messages/ModelUrlMsg.cs index be15087..3a2d3fc 100644 --- a/src/Messages/ModelUrlMsg.cs +++ b/src/Messages/ModelUrlMsg.cs @@ -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; diff --git a/src/Messages/NameMsg.cs b/src/Messages/NameMsg.cs index 632540f..c1b5faa 100644 --- a/src/Messages/NameMsg.cs +++ b/src/Messages/NameMsg.cs @@ -20,10 +20,11 @@ namespace RoboidControl { /// The ID of the thing /// public byte thingId; + /// /// The length of the name, excluding null terminator /// - public byte len; + public byte nameLength; /// /// The name of the thing, not terminated with a null character /// @@ -38,6 +39,7 @@ namespace RoboidControl { this.networkId = networkId; this.thingId = thing.id; this.name = thing.name; + this.nameLength = (byte)this.name.Length; } /// /// Create a new message for sending @@ -45,34 +47,31 @@ namespace RoboidControl { /// The network ID of the thing /// The ID of the thing /// The name of the thing - 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; diff --git a/src/Messages/ThingMsg.cs b/src/Messages/ThingMsg.cs index 2eeae36..9725e23 100644 --- a/src/Messages/ThingMsg.cs +++ b/src/Messages/ThingMsg.cs @@ -35,7 +35,6 @@ namespace RoboidControl { /// The network ID of the thing /// The thing 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; } } diff --git a/src/ParticipantUDP.cs b/src/ParticipantUDP.cs index 3065e62..9021123 100644 --- a/src/ParticipantUDP.cs +++ b/src/ParticipantUDP.cs @@ -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) { } diff --git a/src/Thing.cs b/src/Thing.cs index c1e79ec..00a2020 100644 --- a/src/Thing.cs +++ b/src/Thing.cs @@ -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); }