Fix roboidcontrol repo issue

DoxyGen.meta

@@ -1,5 +1,5 @@
 fileFormatVersion: 2
-guid: 1989654e8505b074d9a0280de8649b7d
+guid: 4b081f36574e30442818c4087c3324c1
 folderAsset: yes
 DefaultImporter:
   externalObjects: {}

DoxyGen/Doxyfile.meta

@@ -1,5 +1,5 @@
 fileFormatVersion: 2
-guid: db5f4144ac032d649994939f1d833737
+guid: f7566f52e6a505b439792756759e12e5
 DefaultImporter:
   externalObjects: {}
   userData: 

DoxyGen/DoxygenLayout.xml.meta

@@ -1,5 +1,5 @@
 fileFormatVersion: 2
-guid: dd31613f75db97f4ca4d408bfce69746
+guid: 083f532f0c28cc648816ade951aa9d1e
 TextScriptImporter:
   externalObjects: {}
   userData: 

DoxyGen/custom_doxygen.css.meta

@@ -1,5 +1,5 @@
 fileFormatVersion: 2
-guid: 186de70a0b3d098409ce1a5ec887b1ae
+guid: d8fca5c59f506a347804140dd15492fb
 DefaultImporter:
   externalObjects: {}
   userData: 

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);
         }

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;
+        // }
+
         /// <summary>
         /// Set the core C# thing
         /// </summary>
@@ -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();

Unity/TouchSensor.cs

@@ -1,4 +1,5 @@
 #if UNITY_5_3_OR_NEWER
+using Unity.VisualScripting;
 using UnityEngine;
 
 namespace RoboidControl.Unity {
@@ -52,19 +53,25 @@ namespace RoboidControl.Unity {
 			rb.isKinematic = true;
 
 			SphereCollider collider = gameObj.AddComponent<SphereCollider>();
-            collider.radius = 0.01F;
+			collider.radius = 0.01f;
 			collider.isTrigger = true;
 
 			component.core = core;
 			component.participant = FindAnyObjectByType<SiteServer>();
 			core.thisParticipant = component.participant.site;
 
-            if (core.parent != null && core.parent.component != null)
+			if (core.parent != null && core.parent.component != null) {
 				gameObj.transform.SetParent(core.parent.component.transform, false);
+			}
 
 			if (core.position != null)
 				gameObj.transform.localPosition = core.position.ToVector3();
 
+			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;
+			}
+
 			return component;
 		}
 

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) {
+                if (thing != null && thing.parent == null) {
-                    thing.Update(currentTimeMS);
+                    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)

src/Participant.cs

@@ -51,7 +51,8 @@ namespace RoboidControl {
         /// <param name="thingId">The ID of the thing</param>
         /// <returns>The thing when it is found, null in other cases.</returns>
         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;

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");
+                //Console.WriteLine("Adding to remote sender");
-                sender.Add(newThing, false, true);
+                //sender.Add(newThing, false, true);
             }
         }
 

src/Thing.cs

@@ -77,31 +77,20 @@ namespace RoboidControl {
             this.parent = parent;
         }
 
-        /*
         /// <summary>
         /// Create a new thing for the given participant
         /// </summary>
-                /// <param name="owner">The participant for which this thing is created</param>
+        /// <param name="owner">The participant owning the thing</param>
-                /// <param name="invokeEvent">True when a new thing event should be triggered</param>
-                public Thing(Participant owner, bool invokeEvent = false) {
-                    this.owner = owner;
-                    if (invokeEvent)
-                        InvokeNewThing(this);
-                }
-        */
-
-        /// <summary>
-        /// Create a new thing for the given participant
-        /// </summary>
-        /// <param name="participant">The participant owning the thing</param>
         /// <param name="networkId">The network ID of the thing</param>
         /// <param name="thingId">The ID of the thing</param>
         /// <param name="thingType">The type of thing</param>
-        public Thing(Participant participant, byte networkId, byte thingId, byte thingType = 0) {
+        public Thing(Participant owner, byte networkId, byte thingId, byte thingType = 0) {
-            this.owner = participant;
+            this.owner = owner;
             this.id = thingId;
             this.type = thingType;
             this.networkId = networkId;
+            this.owner.Add(this);
+            InvokeNewThing(this);
         }
 
         /// <summary>
@@ -269,14 +258,14 @@ namespace RoboidControl {
                 if (_position != value) {
                     _position = value;
                     positionUpdated = true;
-                    OnPositionChanged?.Invoke();
+                    //OnPositionChanged?.Invoke();
                 }
             }
         }
         /// <summary>
-        /// Event triggered when the position has changed
+        /// Event triggered when the pose has changed
         /// </summary>
-        public event ChangeHandler OnPositionChanged = delegate { };
+        public event ChangeHandler OnPoseChanged = null; //delegate { };
         /// <summary>
         /// Boolean indicating that the thing has an updated position
         /// </summary>
@@ -292,14 +281,14 @@ namespace RoboidControl {
                 if (_orientation != value) {
                     _orientation = value;
                     orientationUpdated = true;
-                    OnOrientationChanged?.Invoke();
+                    //OnOrientationChanged?.Invoke();
                 }
             }
         }
         /// <summary>
         /// Event triggered when the orientation has changed
         /// </summary>
-        public event ChangeHandler OnOrientationChanged = delegate { };
+        //public event ChangeHandler OnOrientationChanged = delegate { };
         /// <summary>
         /// Boolean indicating the thing has an updated orientation
         /// </summary>
@@ -376,6 +365,11 @@ namespace RoboidControl {
         /// </summary>
         /// <param name="currentTime">The current time in milliseconds</param>
         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;
         }
 
         /// <summary>