From 390f7807cd0f18b42547fd083b13598e8d5c6d07 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 19 Mar 2025 17:35:21 +0100 Subject: [PATCH] Fix roboidcontrol repo issue --- DoxyGen.meta | 2 +- DoxyGen/Doxyfile.meta | 2 +- DoxyGen/DoxygenLayout.xml.meta | 2 +- DoxyGen/custom_doxygen.css.meta | 2 +- Unity/SiteServer.cs | 2 +- Unity/Thing.cs | 30 +++++-- Unity/TouchSensor.cs | 143 +++++++++++++++++--------------- src/LocalParticipant.cs | 18 ++-- src/Participant.cs | 3 +- src/SiteServer.cs | 10 +-- src/Thing.cs | 38 ++++----- 11 files changed, 132 insertions(+), 120 deletions(-) diff --git a/DoxyGen.meta b/DoxyGen.meta index e2b31f0..64d7172 100644 --- a/DoxyGen.meta +++ b/DoxyGen.meta @@ -1,5 +1,5 @@ fileFormatVersion: 2 -guid: 1989654e8505b074d9a0280de8649b7d +guid: 4b081f36574e30442818c4087c3324c1 folderAsset: yes DefaultImporter: externalObjects: {} diff --git a/DoxyGen/Doxyfile.meta b/DoxyGen/Doxyfile.meta index 70f2192..a632fe7 100644 --- a/DoxyGen/Doxyfile.meta +++ b/DoxyGen/Doxyfile.meta @@ -1,5 +1,5 @@ fileFormatVersion: 2 -guid: db5f4144ac032d649994939f1d833737 +guid: f7566f52e6a505b439792756759e12e5 DefaultImporter: externalObjects: {} userData: diff --git a/DoxyGen/DoxygenLayout.xml.meta b/DoxyGen/DoxygenLayout.xml.meta index efc816b..5a9ae73 100644 --- a/DoxyGen/DoxygenLayout.xml.meta +++ b/DoxyGen/DoxygenLayout.xml.meta @@ -1,5 +1,5 @@ fileFormatVersion: 2 -guid: dd31613f75db97f4ca4d408bfce69746 +guid: 083f532f0c28cc648816ade951aa9d1e TextScriptImporter: externalObjects: {} userData: diff --git a/DoxyGen/custom_doxygen.css.meta b/DoxyGen/custom_doxygen.css.meta index 40ede28..5e2c242 100644 --- a/DoxyGen/custom_doxygen.css.meta +++ b/DoxyGen/custom_doxygen.css.meta @@ -1,5 +1,5 @@ fileFormatVersion: 2 -guid: 186de70a0b3d098409ce1a5ec887b1ae +guid: d8fca5c59f506a347804140dd15492fb DefaultImporter: externalObjects: {} userData: diff --git a/Unity/SiteServer.cs b/Unity/SiteServer.cs index deb237b..d1f4236 100644 --- a/Unity/SiteServer.cs +++ b/Unity/SiteServer.cs @@ -22,7 +22,7 @@ namespace RoboidControl.Unity { } public void HandleNewThing(RoboidControl.Thing thing) { - Debug.Log($"Handle New thing event for {thing}"); + //Debug.Log($"Handle New thing event for {thing}"); site.Add(thing, false); thingQueue.Enqueue(thing); } diff --git a/Unity/Thing.cs b/Unity/Thing.cs index aa4921e..98551fe 100644 --- a/Unity/Thing.cs +++ b/Unity/Thing.cs @@ -1,5 +1,6 @@ #if UNITY_5_3_OR_NEWER using System.Collections; +using Mono.Cecil.Cil; using UnityEngine; using UnityEngine.Networking; @@ -18,6 +19,10 @@ namespace RoboidControl.Unity { private string modelUrl = null; + // protected virtual void Awake() { + // core.OnPoseChanged += PoseChanged; + // } + /// /// Set the core C# thing /// @@ -31,7 +36,7 @@ namespace RoboidControl.Unity { return; } siteServer.site.Add(thing); - + core.OnPoseChanged += PoseChanged; } public static Thing Create(RoboidControl.Thing core) { @@ -47,7 +52,10 @@ namespace RoboidControl.Unity { if (core.position != null) gameObj.transform.localPosition = core.position.ToVector3(); + if (core.orientation != null) + gameObj.transform.localRotation = core.orientation.ToQuaternion(); + core.OnPoseChanged += component.PoseChanged; return component; } @@ -61,16 +69,18 @@ namespace RoboidControl.Unity { if (core.linearVelocity != null && core.linearVelocity.distance != 0) { Vector3 direction = Quaternion.AngleAxis(core.linearVelocity.direction.horizontal, Vector3.up) * Vector3.forward; this.transform.Translate(core.linearVelocity.distance * Time.deltaTime * direction, Space.Self); - } else if (core.positionUpdated) - this.transform.localPosition = core.position.ToVector3(); - + } + // else if (core.positionUpdated) + // this.transform.localPosition = core.position.ToVector3(); + if (core.angularVelocity != null && core.angularVelocity.distance != 0) { Vector3 angularVelocity = core.angularVelocity.ToVector3(); Vector3 axis = core.angularVelocity.direction.ToVector3(); this.transform.localRotation *= Quaternion.AngleAxis(core.angularVelocity.distance * Time.deltaTime, axis); //this.transform.localRotation *= Quaternion.Euler(angularVelocity * Time.deltaTime); - } else if (core.orientationUpdated) - this.transform.localRotation = core.orientation.ToQuaternion(); + } + // else if (core.orientationUpdated) + // this.transform.localRotation = core.orientation.ToQuaternion(); if (!string.IsNullOrEmpty(core.modelUrl) && this.modelUrl == null) { string extension = core.modelUrl.Substring(core.modelUrl.LastIndexOf(".")); @@ -82,6 +92,14 @@ namespace RoboidControl.Unity { } } + private void PoseChanged() { + Debug.Log($"{this} pose changed"); + if (core.positionUpdated) + this.transform.localPosition = core.position.ToVector3(); + if (core.orientationUpdated) + this.transform.localRotation = core.orientation.ToQuaternion(); + } + private IEnumerator LoadJPG() { UnityWebRequest request = UnityWebRequestTexture.GetTexture(core.modelUrl); yield return request.SendWebRequest(); diff --git a/Unity/TouchSensor.cs b/Unity/TouchSensor.cs index 5f8d8ec..2859fc4 100644 --- a/Unity/TouchSensor.cs +++ b/Unity/TouchSensor.cs @@ -1,87 +1,94 @@ #if UNITY_5_3_OR_NEWER +using Unity.VisualScripting; using UnityEngine; namespace RoboidControl.Unity { - /// - /// The Unity representation of the TouchSensor - /// - public class TouchSensor : Thing { + /// + /// The Unity representation of the TouchSensor + /// + public class TouchSensor : Thing { - public SiteServer participant; - /// - /// The core touch sensor - /// - public RoboidControl.TouchSensor coreSensor { - get => (RoboidControl.TouchSensor)base.core; - } + public SiteServer participant; + /// + /// The core touch sensor + /// + public RoboidControl.TouchSensor coreSensor { + get => (RoboidControl.TouchSensor)base.core; + } - /// - /// Start the Unity represention - /// - protected virtual void Start() { - if (core == null) { - participant = FindAnyObjectByType(); - SetCoreThing(new RoboidControl.TouchSensor(participant.site)); - } - // Somehow this does not work. + /// + /// Start the Unity represention + /// + protected virtual void Start() { + if (core == null) { + participant = FindAnyObjectByType(); + SetCoreThing(new RoboidControl.TouchSensor(participant.site)); + } + // Somehow this does not work. - // Rigidbody rb = GetComponentInParent(); - // if (rb == null) { - // RoboidControl.Thing thing = core; - // while (thing.parent != null) - // thing = thing.parent; - - // Thing unityThing = thing.component; - // rb = unityThing.gameObject.AddComponent(); - // rb.isKinematic = true; - // } - } + // Rigidbody rb = GetComponentInParent(); + // if (rb == null) { + // RoboidControl.Thing thing = core; + // while (thing.parent != null) + // thing = thing.parent; - /// - /// Create the Unity representation - /// - /// The core touch sensor - /// The Unity representation of the touch sensor - public static TouchSensor Create(RoboidControl.TouchSensor core) { - GameObject gameObj = core.name != null ? - new(core.name) : - new("Touch Sensor"); - TouchSensor component = gameObj.AddComponent(); - Rigidbody rb = gameObj.AddComponent(); - rb.isKinematic = true; + // Thing unityThing = thing.component; + // rb = unityThing.gameObject.AddComponent(); + // rb.isKinematic = true; + // } + } - SphereCollider collider = gameObj.AddComponent(); - collider.radius = 0.01F; - collider.isTrigger = true; + /// + /// Create the Unity representation + /// + /// The core touch sensor + /// The Unity representation of the touch sensor + public static TouchSensor Create(RoboidControl.TouchSensor core) { + GameObject gameObj = core.name != null ? + new(core.name) : + new("Touch Sensor"); + TouchSensor component = gameObj.AddComponent(); + Rigidbody rb = gameObj.AddComponent(); + rb.isKinematic = true; - component.core = core; - component.participant = FindAnyObjectByType(); - core.thisParticipant = component.participant.site; + SphereCollider collider = gameObj.AddComponent(); + collider.radius = 0.01f; + collider.isTrigger = true; - if (core.parent != null && core.parent.component != null) - gameObj.transform.SetParent(core.parent.component.transform, false); + component.core = core; + component.participant = FindAnyObjectByType(); + core.thisParticipant = component.participant.site; - if (core.position != null) - gameObj.transform.localPosition = core.position.ToVector3(); + if (core.parent != null && core.parent.component != null) { + gameObj.transform.SetParent(core.parent.component.transform, false); + } - return component; - } + if (core.position != null) + gameObj.transform.localPosition = core.position.ToVector3(); - private void OnTriggerEnter(Collider other) { - if (other.isTrigger) - return; - if (this.transform.root == other.transform.root) - return; + if (gameObj.transform.parent != null && gameObj.transform.localPosition.magnitude > 0) { + collider.radius = Vector3.Distance(gameObj.transform.position, gameObj.transform.parent.position) / 2; + gameObj.transform.position = (gameObj.transform.position + gameObj.transform.parent.position) / 2; + } - this.coreSensor.touchedSomething = true; - } - private void OnTriggerExit(Collider other) { - if (other.isTrigger) - return; + return component; + } - this.coreSensor.touchedSomething = false; - } - } + private void OnTriggerEnter(Collider other) { + if (other.isTrigger) + return; + if (this.transform.root == other.transform.root) + return; + + this.coreSensor.touchedSomething = true; + } + private void OnTriggerExit(Collider other) { + if (other.isTrigger) + return; + + this.coreSensor.touchedSomething = false; + } + } } #endif \ No newline at end of file diff --git a/src/LocalParticipant.cs b/src/LocalParticipant.cs index 131b6b2..7dcdde7 100644 --- a/src/LocalParticipant.cs +++ b/src/LocalParticipant.cs @@ -150,8 +150,8 @@ namespace RoboidControl { int n = this.things.Count; for (int ix = 0; ix < n; ix++) { Thing thing = this.things[ix]; - if (thing != null) { - thing.Update(currentTimeMS); + if (thing != null && thing.parent == null) { + thing.Update(currentTimeMS, true); // if (thing.owner != this) { // BinaryMsg binaryMsg = new(thing.owner.networkId, thing); // this.Send(thing.owner, binaryMsg); @@ -293,14 +293,14 @@ namespace RoboidControl { } protected virtual void Process(Participant sender, NameMsg msg) { - //Console.WriteLine($"Participant: Process name [{msg.networkId}/{msg.thingId}] {msg.name}"); + Console.WriteLine($"Participant: Process name [{msg.networkId}/{msg.thingId}] {msg.name}"); Thing thing = sender.Get(msg.networkId, msg.thingId); if (thing != null) thing.name = msg.name; } protected virtual void Process(Participant sender, ModelUrlMsg msg) { - //Console.WriteLine($"Participant: Process model [{msg.networkId}/{msg.thingId}] {msg.url}"); + Console.WriteLine($"Participant: Process model [{msg.networkId}/{msg.thingId}] {msg.url}"); Thing thing = sender.Get(msg.networkId, msg.thingId); if (thing != null) thing.modelUrl = msg.url; @@ -310,18 +310,12 @@ namespace RoboidControl { //Console.WriteLine($"Participant: Process pose [{msg.networkId}/{msg.thingId}] {msg.poseType}"); Thing thing = sender.Get(msg.networkId, msg.thingId); if (thing != null) { - thing.positionUpdated = false; - if ((msg.poseType & PoseMsg.Pose_Position) != 0) { + if ((msg.poseType & PoseMsg.Pose_Position) != 0) thing.position = msg.position; - thing.positionUpdated = true; - } - thing.orientationUpdated = false; if ((msg.poseType & PoseMsg.Pose_Orientation) != 0) { thing.orientation = msg.orientation; - thing.orientationUpdated = true; + Console.Write($"{thing.id} orientation changed"); } - else - thing.orientation = null; if ((msg.poseType & PoseMsg.Pose_LinearVelocity) != 0) thing.linearVelocity = msg.linearVelocity; if ((msg.poseType & PoseMsg.Pose_AngularVelocity) != 0) diff --git a/src/Participant.cs b/src/Participant.cs index b3d7d44..172aa35 100644 --- a/src/Participant.cs +++ b/src/Participant.cs @@ -51,7 +51,8 @@ namespace RoboidControl { /// The ID of the thing /// The thing when it is found, null in other cases. public Thing Get(byte networkId, byte thingId) { - Thing thing = things.Find(aThing => Thing.IsThing(aThing, networkId, thingId)); + Thing thing = things.Find(aThing => aThing.id == thingId); + //Thing thing = things.Find(aThing => Thing.IsThing(aThing, networkId, thingId)); // if (thing == null) // Console.WriteLine($"Could not find thing {ipAddress}:{port}[{networkId}/{thingId}]"); return thing; diff --git a/src/SiteServer.cs b/src/SiteServer.cs index 4d70718..616cada 100644 --- a/src/SiteServer.cs +++ b/src/SiteServer.cs @@ -44,7 +44,7 @@ namespace RoboidControl { protected override void Process(Participant remoteParticipant, ParticipantMsg msg) { if (msg.networkId == 0) { - Console.WriteLine($"{this.name} received New Client -> {remoteParticipant.networkId}"); + Console.WriteLine($"{this.name} received New Participant -> {remoteParticipant.networkId}"); this.Send(remoteParticipant, new NetworkIdMsg(remoteParticipant.networkId)); } } @@ -52,7 +52,7 @@ namespace RoboidControl { protected override void Process(Participant sender, NetworkIdMsg msg) { } protected override void Process(Participant sender, ThingMsg msg) { - //Console.WriteLine($"SiteServer: Process thing [{msg.networkId}/{msg.thingId}]"); + Console.WriteLine($"SiteServer: Process thing [{msg.networkId}/{msg.thingId}]"); Thing thing = sender.Get(msg.networkId, msg.thingId); if (thing == null) { Thing newThing = null; @@ -68,12 +68,12 @@ namespace RoboidControl { if (msg.parentId != 0) { Thing parentThing = Get(msg.networkId, msg.parentId); if (parentThing == null) - Console.WriteLine("Could not find parent"); + Console.WriteLine($"Could not find parent [{msg.networkId}/{msg.parentId}]"); else newThing.parent = parentThing; } - Console.WriteLine("Adding to remote sender"); - sender.Add(newThing, false, true); + //Console.WriteLine("Adding to remote sender"); + //sender.Add(newThing, false, true); } } diff --git a/src/Thing.cs b/src/Thing.cs index 9df720d..607beec 100644 --- a/src/Thing.cs +++ b/src/Thing.cs @@ -77,31 +77,20 @@ namespace RoboidControl { this.parent = parent; } - /* - /// - /// Create a new thing for the given participant - /// - /// The participant for which this thing is created - /// True when a new thing event should be triggered - public Thing(Participant owner, bool invokeEvent = false) { - this.owner = owner; - if (invokeEvent) - InvokeNewThing(this); - } - */ - /// /// Create a new thing for the given participant /// - /// The participant owning the thing + /// The participant owning the thing /// The network ID of the thing /// The ID of the thing /// The type of thing - public Thing(Participant participant, byte networkId, byte thingId, byte thingType = 0) { - this.owner = participant; + public Thing(Participant owner, byte networkId, byte thingId, byte thingType = 0) { + this.owner = owner; this.id = thingId; this.type = thingType; this.networkId = networkId; + this.owner.Add(this); + InvokeNewThing(this); } /// @@ -269,14 +258,14 @@ namespace RoboidControl { if (_position != value) { _position = value; positionUpdated = true; - OnPositionChanged?.Invoke(); + //OnPositionChanged?.Invoke(); } } } /// - /// Event triggered when the position has changed + /// Event triggered when the pose has changed /// - public event ChangeHandler OnPositionChanged = delegate { }; + public event ChangeHandler OnPoseChanged = null; //delegate { }; /// /// Boolean indicating that the thing has an updated position /// @@ -292,14 +281,14 @@ namespace RoboidControl { if (_orientation != value) { _orientation = value; orientationUpdated = true; - OnOrientationChanged?.Invoke(); + //OnOrientationChanged?.Invoke(); } } } /// /// Event triggered when the orientation has changed /// - public event ChangeHandler OnOrientationChanged = delegate { }; + //public event ChangeHandler OnOrientationChanged = delegate { }; /// /// Boolean indicating the thing has an updated orientation /// @@ -376,6 +365,11 @@ namespace RoboidControl { /// /// The current time in milliseconds public virtual void Update(ulong currentTimeMs, bool recursively = false) { + if (this.positionUpdated || this.orientationUpdated) + OnPoseChanged?.Invoke(); + this.positionUpdated = false; + this.orientationUpdated = false; + // should recurse over children... if (recursively) { for (byte childIx = 0; childIx < this.children.Count; childIx++) { @@ -385,8 +379,6 @@ namespace RoboidControl { child.Update(currentTimeMs, recursively); } } - positionUpdated = false; - orientationUpdated = false; } ///