Compare commits

...

7 Commits

9 changed files with 86 additions and 45 deletions

View File

@ -1,4 +1,4 @@
#if UNITY_5_3_OR_NEWER #if UNITY_5_3_OR_NEWER && RC_DEBUG
using System.IO; using System.IO;
using System.Text; using System.Text;
using UnityEngine; using UnityEngine;

View File

@ -125,7 +125,6 @@ namespace RoboidControl.Unity {
// Use smoothing to emulate motor inertia // Use smoothing to emulate motor inertia
rb.velocity = 0.9f * rb.velocity + 0.1f * forwardSpeed * transform.forward; rb.velocity = 0.9f * rb.velocity + 0.1f * forwardSpeed * transform.forward;
Debug.Log(rb.velocity);
rb.angularVelocity = 0.9f * rb.angularVelocity + 0.1f * turningSpeed * Vector3.up; rb.angularVelocity = 0.9f * rb.angularVelocity + 0.1f * turningSpeed * Vector3.up;
} }
} }

View File

@ -48,7 +48,11 @@ namespace RoboidControl.Unity {
if (coreThing.component == null) { if (coreThing.component == null) {
Thing[] things = FindObjectsByType<Thing>(FindObjectsSortMode.None); Thing[] things = FindObjectsByType<Thing>(FindObjectsSortMode.None);
// Debug.Log(things.Length); // Debug.Log(things.Length);
Thing thing = things.FirstOrDefault(t => t.core != null && t.core.id == coreThing.id); Thing thing = things.FirstOrDefault(t =>
t.core != null &&
t.core.owner.networkId == coreThing.owner.networkId &&
t.core.id == coreThing.id
);
if (thing == null) if (thing == null)
thing = Thing.Create(coreThing); thing = Thing.Create(coreThing);
coreThing.component = thing; coreThing.component = thing;

View File

@ -1,21 +1,30 @@
#if UNITY_5_3_OR_NEWER #if UNITY_5_3_OR_NEWER
using System; using System;
using System.Collections.Generic;
using UnityEngine; using UnityEngine;
namespace RoboidControl.Unity { namespace RoboidControl.Unity {
public class SiteServer : Participant { public class SiteServer : Participant {
//public RoboidControl.SiteServer site;
public RoboidControl.SiteServer site => this.coreParticipant as RoboidControl.SiteServer; public RoboidControl.SiteServer site => this.coreParticipant as RoboidControl.SiteServer;
public string modelURL;
//public Queue<RoboidControl.Thing> thingQueue = new(); private RoboidControl.Thing model;
protected virtual void Awake() { protected virtual void Awake() {
#if RC_DEBUG
Console.SetOut(new UnityLogWriter()); Console.SetOut(new UnityLogWriter());
#endif
this.coreParticipant = new RoboidControl.SiteServer(port); this.coreParticipant = new RoboidControl.SiteServer(port);
#if GLTF
if (!string.IsNullOrEmpty(modelURL)) {
model = new() {
name = "Model",
modelUrl = this.modelURL
};
}
#endif
} }
void OnApplicationQuit() { void OnApplicationQuit() {
@ -31,8 +40,6 @@ namespace RoboidControl.Unity {
HandleUpdateEvent(e); HandleUpdateEvent(e);
site.Update(); site.Update();
// while (thingQueue.TryDequeue(out RoboidControl.Thing thing))
// thing.CreateComponent();
} }
private void HandleUpdateEvent(RoboidControl.Participant.UpdateEvent e) { private void HandleUpdateEvent(RoboidControl.Participant.UpdateEvent e) {
@ -44,6 +51,10 @@ namespace RoboidControl.Unity {
participant.coreParticipant.component = participant; participant.coreParticipant.component = participant;
participant.ipAddress = e.participant.ipAddress; participant.ipAddress = e.participant.ipAddress;
participant.port = e.participant.port; participant.port = e.participant.port;
foreach (RoboidControl.Thing thing in this.site.things)
participant.coreParticipant.SendThingInfo(thing);
break; break;
case ThingMsg.id: case ThingMsg.id:
HandleThingEvent(e); HandleThingEvent(e);

View File

@ -14,7 +14,7 @@ namespace RoboidControl {
/// <summary> /// <summary>
/// The length of the message /// The length of the message
/// </summary> /// </summary>
public const byte length = 4 + 4 + 4; public byte length = 4 + 4 + 4;
/// <summary> /// <summary>
/// The network ID of the thing /// The network ID of the thing
/// </summary> /// </summary>
@ -108,12 +108,12 @@ namespace RoboidControl {
if ((this.poseType & Pose_Position) != 0) if ((this.poseType & Pose_Position) != 0)
this.position = LowLevelMessages.ReceiveSpherical(buffer, ref ix); this.position = LowLevelMessages.ReceiveSpherical(buffer, ref ix);
if ((this.poseType & Pose_Orientation) != 0) if ((this.poseType & Pose_Orientation) != 0)
//this.orientation = SwingTwist.FromQuat32(LowLevelMessages.ReceiveQuat32(buffer, ref ix));
this.orientation = LowLevelMessages.ReceiveSwingTwist(buffer, ref ix); this.orientation = LowLevelMessages.ReceiveSwingTwist(buffer, ref ix);
if ((this.poseType & Pose_LinearVelocity) != 0) if ((this.poseType & Pose_LinearVelocity) != 0)
this.linearVelocity = LowLevelMessages.ReceiveSpherical(buffer, ref ix); this.linearVelocity = LowLevelMessages.ReceiveSpherical(buffer, ref ix);
if ((this.poseType & Pose_AngularVelocity) != 0) if ((this.poseType & Pose_AngularVelocity) != 0)
this.angularVelocity = LowLevelMessages.ReceiveSpherical(buffer, ref ix); this.angularVelocity = LowLevelMessages.ReceiveSpherical(buffer, ref ix);
this.length = ix;
} }
/// @copydoc Passer::RoboidControl::IMessage::Serialize /// @copydoc Passer::RoboidControl::IMessage::Serialize
@ -133,7 +133,6 @@ namespace RoboidControl {
if ((poseType & Pose_Position) != 0) if ((poseType & Pose_Position) != 0)
LowLevelMessages.SendSpherical(buffer, ref ix, this.position); LowLevelMessages.SendSpherical(buffer, ref ix, this.position);
if ((poseType & Pose_Orientation) != 0) if ((poseType & Pose_Orientation) != 0)
//LowLevelMessages.SendQuat32(buffer, ref ix, this.orientation);
LowLevelMessages.SendSwingTwist(buffer, ref ix, this.orientation); LowLevelMessages.SendSwingTwist(buffer, ref ix, this.orientation);
if ((poseType & Pose_LinearVelocity) != 0) if ((poseType & Pose_LinearVelocity) != 0)
LowLevelMessages.SendSpherical(buffer, ref ix, this.linearVelocity); LowLevelMessages.SendSpherical(buffer, ref ix, this.linearVelocity);

View File

@ -191,6 +191,14 @@ namespace RoboidControl {
return true; return true;
} }
public void SendThingInfo(Thing thing) {
this.Send(new ThingMsg(thing.owner.networkId, thing));
this.Send(new NameMsg(thing.owner.networkId, thing));
this.Send(new ModelUrlMsg(thing.owner.networkId, thing));
this.Send(new PoseMsg(thing.owner.networkId, thing));
this.Send(new BinaryMsg(thing.owner.networkId, thing));
}
#endregion Send #endregion Send
#region Participant Registry #region Participant Registry

View File

@ -107,7 +107,7 @@ namespace RoboidControl {
/// The interval in milliseconds for publishing (broadcasting) data on the local network /// The interval in milliseconds for publishing (broadcasting) data on the local network
/// </summary> /// </summary>
public ulong publishIntervalMS = 3000; // = 3 seconds public ulong publishIntervalMS = 3000; // = 3 seconds
public ulong sendUpdateIntervalMS = 100; // for object updates public ulong sendUpdateIntervalMS = 100; // = 0.1 seconds for object updates
//public byte[] buffer = new byte[1024]; //public byte[] buffer = new byte[1024];
@ -219,8 +219,8 @@ namespace RoboidControl {
// this.Send(participant, poseMsg); // this.Send(participant, poseMsg);
// BinaryMsg binaryMsg = new(thing.owner.networkId, thing); // BinaryMsg binaryMsg = new(thing.owner.networkId, thing);
// this.Send(participant, binaryMsg); // this.Send(participant, binaryMsg);
participant.Send(new PoseMsg(thing.owner.networkId, thing)); // participant.Send(new PoseMsg(thing.owner.networkId, thing));
participant.Send(new BinaryMsg(thing.owner.networkId, thing)); // participant.Send(new BinaryMsg(thing.owner.networkId, thing));
} }
this.nextSendUpdate = currentTimeMS + this.sendUpdateIntervalMS; this.nextSendUpdate = currentTimeMS + this.sendUpdateIntervalMS;
} }
@ -232,14 +232,6 @@ namespace RoboidControl {
#region Send #region Send
public void SendThingInfo(Participant owner, Thing thing) {
owner.Send(new ThingMsg(this.networkId, thing));
owner.Send(new NameMsg(this.networkId, thing));
owner.Send(new ModelUrlMsg(this.networkId, thing));
owner.Send(new PoseMsg(this.networkId, thing));
owner.Send(new BinaryMsg(this.networkId, thing));
}
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));
@ -305,37 +297,58 @@ namespace RoboidControl {
} }
public void ReceiveData(byte[] data, Participant sender) { public void ReceiveData(byte[] data, Participant sender) {
byte msgId = data[0]; int dataIx = 0;
byte msgId = data[dataIx];
if (msgId == 0xFF) { if (msgId == 0xFF) {
// Timeout // Timeout
return; return;
} }
switch (msgId) { switch (msgId) {
case ParticipantMsg.Id: // 0xA0 / 160 case ParticipantMsg.Id: { // 0xA0 / 160
this.Process(sender, new ParticipantMsg(data)); ParticipantMsg msg = new(data);
this.Process(sender, msg);
dataIx += ParticipantMsg.length;
}
break; break;
case NetworkIdMsg.Id: // 0xA1 / 161 case NetworkIdMsg.Id: {// 0xA1 / 161
this.Process(sender, new NetworkIdMsg(data)); NetworkIdMsg msg = new(data);
this.Process(sender, msg);
dataIx += NetworkIdMsg.length;
}
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(sender, new ThingMsg(data)); ThingMsg msg = new(data);
this.Process(sender, msg);
dataIx += ThingMsg.length;
}
break; break;
case NameMsg.Id: // 0x91 / 145 case NameMsg.Id: { // 0x91 / 145
this.Process(sender, new NameMsg(data)); NameMsg msg = new(data);
this.Process(sender, msg);
dataIx += NameMsg.length + msg.nameLength;
}
break; break;
case ModelUrlMsg.Id: // 0x90 / 144 case ModelUrlMsg.Id: { // 0x90 / 144
this.Process(sender, new ModelUrlMsg(data)); ModelUrlMsg msg = new(data);
this.Process(sender, msg);
dataIx += ModelUrlMsg.length + msg.urlLength;
}
break; break;
case PoseMsg.Id: // 0x10 / 16 case PoseMsg.Id: { // 0x10 / 16
this.Process(sender, new PoseMsg(data)); PoseMsg msg = new(data);
// result = await PoseMsg.Receive(dataStream, client, packetSize); this.Process(sender, msg);
dataIx += msg.length;
}
break; break;
case BinaryMsg.Id: // 0xB1 / 177 case BinaryMsg.Id: { // 0xB1 / 177
this.Process(sender, new BinaryMsg(data)); BinaryMsg msg = new(data);
this.Process(sender, msg);
dataIx += BinaryMsg.length + msg.dataLength;
}
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);
@ -347,6 +360,8 @@ namespace RoboidControl {
default: default:
break; break;
} }
if (dataIx < data.Length)
Console.WriteLine($"####### Buffer not fully read, remaining {data.Length - dataIx}");
} }
protected virtual void Process(Participant sender, ParticipantMsg msg) { protected virtual void Process(Participant sender, ParticipantMsg msg) {
@ -363,7 +378,7 @@ namespace RoboidControl {
if (this.networkId != msg.networkId) { if (this.networkId != msg.networkId) {
this.networkId = msg.networkId; this.networkId = msg.networkId;
foreach (Thing thing in this.things) //Thing.GetAllThings()) foreach (Thing thing in this.things) //Thing.GetAllThings())
this.SendThingInfo(sender, thing); sender.SendThingInfo(thing);
} }
} }
@ -401,7 +416,7 @@ namespace RoboidControl {
protected virtual void Process(Participant sender, PoseMsg msg) { protected virtual void Process(Participant sender, PoseMsg msg) {
#if DEBUG #if DEBUG
// Console.WriteLine($"{this.name}: Process PoseMsg [{msg.networkId}/{msg.thingId}] {msg.poseType}"); Console.WriteLine($"{this.name}: Process PoseMsg [{msg.networkId}/{msg.thingId}] {msg.poseType}");
#endif #endif
Participant owner = Participant.GetParticipant(msg.networkId); Participant owner = Participant.GetParticipant(msg.networkId);
if (owner == null) if (owner == null)

View File

@ -49,7 +49,7 @@ namespace RoboidControl {
// this.leftWheel.position = new Spherical(distance, Direction.left); // this.leftWheel.position = new Spherical(distance, Direction.left);
this.rightWheel = rightWheel; this.rightWheel = rightWheel;
this.rightWheel.parent= this; this.rightWheel.parent = this;
// if (this.rightWheel != null) // if (this.rightWheel != null)
// this.rightWheel.position = new Spherical(distance, Direction.right); // this.rightWheel.position = new Spherical(distance, Direction.right);
@ -139,8 +139,12 @@ namespace RoboidControl {
byte ix = 0; byte ix = 0;
byte leftWheelId = data[ix++]; byte leftWheelId = data[ix++];
this.leftWheel = this.owner.Get(leftWheelId) as Motor; this.leftWheel = this.owner.Get(leftWheelId) as Motor;
this.leftWheel ??= new Motor(this) { id = leftWheelId };
byte rightWheelId = data[ix++]; byte rightWheelId = data[ix++];
this.rightWheel = this.owner.Get(rightWheelId) as Motor; this.rightWheel = this.owner.Get(rightWheelId) as Motor;
this.rightWheel ??= new Motor(this) { id = rightWheelId };
this._wheelRadius = LowLevelMessages.ReceiveFloat16(data, ref ix); this._wheelRadius = LowLevelMessages.ReceiveFloat16(data, ref ix);
//this._wheelSeparation = LowLevelMessages.ReceiveFloat16(data, ref ix); //this._wheelSeparation = LowLevelMessages.ReceiveFloat16(data, ref ix);
this.updateQueue.Enqueue(new CoreEvent(BinaryMsg.Id)); this.updateQueue.Enqueue(new CoreEvent(BinaryMsg.Id));

View File

@ -40,7 +40,8 @@ namespace RoboidControl {
return data; return data;
} }
public override void ProcessBinary(byte[] data) { public override void ProcessBinary(byte[] data) {
this.targetSpeed = (float)data[0] / 127; this._targetSpeed = (float)(sbyte)data[0] / 127;
updateQueue.Enqueue(new CoreEvent(BinaryMsg.Id));
} }
} }