Arduino Ant random walk

Messages/PoseMsg.cs

@@ -69,21 +69,45 @@ namespace Passer.RoboidControl {
         /// <param name="thingId">The ID of the thing</param>
         /// <param name="position">The position of the thing in local space in meters</param>
         /// <param name="orientation">The orientation of the thing in local space</param>
-        public PoseMsg(byte networkId, byte thingId, Spherical position, SwingTwist orientation) {
+        public PoseMsg(byte networkId, byte thingId, Spherical position, SwingTwist orientation, Spherical linearVelocity = null, Spherical angularVelocity = null) {
             this.networkId = networkId;
             this.thingId = thingId;
 
-            this.position = position;
-            this.orientation = orientation;
-
             this.poseType = 0;
             if (this.position != null)
                 this.poseType |= Pose_Position;
             if (this.orientation != null)
                 this.poseType |= Pose_Orientation;
+            if (this.linearVelocity != null)
+                this.poseType |= Pose_LinearVelocity;
+            if (this.angularVelocity != null)
+                this.poseType |= Pose_AngularVelocity;
+
+            this.position = position;
+            this.orientation = orientation;
+            this.linearVelocity = linearVelocity;
+            this.angularVelocity = angularVelocity;
         }
         /// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer)
-        public PoseMsg(byte[] buffer) : base(buffer) { }
+        public PoseMsg(byte[] buffer) : base(buffer) {
+            byte ix = 1;
+            this.networkId = buffer[ix++];
+            this.thingId = buffer[ix++];
+
+            this.poseType = buffer[ix++];
+            this.position = null;
+            this.orientation = null;
+            this.linearVelocity = null;
+            this.angularVelocity = null;
+            if ((this.poseType & Pose_Position) != 0)
+                this.position = LowLevelMessages.ReceiveSpherical(buffer, ref ix);
+            if ((this.poseType & Pose_Orientation) != 0)
+                this.orientation = SwingTwist.FromQuat32(LowLevelMessages.ReceiveQuat32(buffer, ref ix));
+            if ((this.poseType & Pose_LinearVelocity) != 0)
+                this.linearVelocity = LowLevelMessages.ReceiveSpherical(buffer, ref ix);
+            if ((this.poseType & Pose_AngularVelocity) != 0)
+                this.angularVelocity = LowLevelMessages.ReceiveSpherical(buffer, ref ix);
+        }
 
         /// @copydoc Passer::RoboidControl::IMessage::Serialize
         public override byte Serialize(ref byte[] buffer) {

Participant.cs

@@ -253,6 +253,7 @@ namespace Passer.RoboidControl {
                     this.Process(remoteParticipant, new ModelUrlMsg(data));
                     break;
                 case PoseMsg.Id: // 0x10 / 16
+                    this.Process(remoteParticipant, new PoseMsg(data));
                     // result = await PoseMsg.Receive(dataStream, client, packetSize);
                     break;
                 case BinaryMsg.Id: // 0xB1 / 177
@@ -299,9 +300,31 @@ namespace Passer.RoboidControl {
 
         protected virtual void Process(RemoteParticipant sender, ModelUrlMsg msg) {
             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;
         }
 
-        protected virtual void Process(PoseMsg msg) { }
+        protected virtual void Process(RemoteParticipant sender, PoseMsg msg) {
+            //Console.WriteLine($"Participant: Process pose [{msg.networkId}/{msg.thingId}] {msg.poseType}");
+            Thing thing = sender.Get(msg.networkId, msg.thingId);
+            if (thing != null) {
+                thing.hasPosition = false;
+                if ((msg.poseType & PoseMsg.Pose_Position) != 0) {
+                    thing.position = msg.position;
+                    thing.hasPosition = true;
+                }
+                if ((msg.poseType & PoseMsg.Pose_Orientation) != 0)
+                    thing.orientation = msg.orientation;
+                else
+                    thing.orientation = null;
+                if ((msg.poseType & PoseMsg.Pose_LinearVelocity) != 0)
+                    thing.linearVelocity = msg.linearVelocity;
+                if ((msg.poseType & PoseMsg.Pose_AngularVelocity) != 0)
+                    thing.angularVelocity = msg.angularVelocity;
+                
+            }
+         }
 
         protected virtual void Process(RemoteParticipant sender, BinaryMsg msg) {
             // Console.WriteLine($"Participant: Process binary [{msg.networkId}/{msg.thingId}]");

Sensors/TouchSensor.cs

@@ -18,9 +18,14 @@ namespace Passer.RoboidControl {
             touchedSomething = false;
         }
 
+        public TouchSensor(RemoteParticipant participant, byte networkId, byte thingId) : base(participant, networkId, thingId) {
+            touchedSomething = false;
+        }
+
 #if UNITY_5_3_OR_NEWER
         /// @copydoc Passer::RoboidControl::Thing::CreateComponent 
         public override void CreateComponent() {
+            System.Console.Write("Create touch sensor component");
             this.component = Unity.TouchSensor.Create(this);
             this.component.core = this;
         }

SiteServer.cs

@@ -9,7 +9,7 @@ namespace Passer.RoboidControl {
     /// </summary>
     public class SiteServer : Participant {
 
-        public SiteServer(int port = 7681) : this("0.0.0.0", port) {}
+        public SiteServer(int port = 7681) : this("0.0.0.0", port) { }
 
         /// <summary>
         /// Create a new site server
@@ -28,6 +28,8 @@ namespace Passer.RoboidControl {
             this.udpClient.BeginReceive(
                 new AsyncCallback(result => ReceiveUDP(result)),
                 new Tuple<UdpClient, IPEndPoint>(this.udpClient, new(IPAddress.Any, port)));
+            
+            Register<TouchSensor>(Thing.Type.TouchSensor);
         }
 
         /// <summary>
@@ -55,7 +57,7 @@ namespace Passer.RoboidControl {
             if (thing == null) {
                 Thing newThing = null;
                 if (thingMsgProcessors.TryGetValue(msg.thingType, out Func<RemoteParticipant, byte, byte, Thing> value)) {
-                    Console.WriteLine("Found thing message processor");
+                    // Console.WriteLine("Found thing message processor");
                     if (value != null)
                         newThing = value(sender, msg.networkId, msg.thingId);
                 }
@@ -63,6 +65,13 @@ namespace Passer.RoboidControl {
                     newThing = new Thing(sender, msg.networkId, msg.thingId, msg.thingType);
                     Console.WriteLine("Created generic new core thing");
                 }
+                if (msg.parentId != 0) {
+                    Thing parentThing = Get(msg.networkId, msg.parentId);
+                    if (parentThing == null)
+                        Console.WriteLine("Could not find parent");
+                    else
+                        newThing.parent = parentThing;
+                }
 
                 sender.Add(newThing);
             }

Thing.cs

@@ -44,6 +44,7 @@ namespace Passer.RoboidControl {
             DistanceSensor,
             DirectionalSensor,
             TemperatureSensor,
+            TouchSensor,
             // Motor
             ControlledMotor,
             UncontrolledMotor,
@@ -148,6 +149,7 @@ namespace Passer.RoboidControl {
                 }
             }
         }
+        public bool hasPosition = false;
 
         /// <summary>
         /// Event triggered when the orientation has changed

Unity/SiteServer.cs

@@ -15,6 +15,8 @@ namespace Passer.RoboidControl.Unity {
 
             site = new(7681);
             RoboidControl.Thing.OnNewThing += HandleNewThing;
+
+            //site.Register<RoboidControl.TouchSensor>(RoboidControl.Thing.Type.TouchSensor);
         }
 
         void OnApplicationQuit() {
@@ -22,7 +24,7 @@ namespace Passer.RoboidControl.Unity {
         }
 
         public void HandleNewThing(RoboidControl.Thing thing) {
-            Debug.Log("Handle New thing event");
+            // Debug.Log("Handle New thing event");
             site.Add(thing, false);
             thingQueue.Enqueue(thing);
         }

Unity/Thing.cs

@@ -1,5 +1,7 @@
 #if UNITY_5_3_OR_NEWER
+using System.Collections;
 using UnityEngine;
+using UnityEngine.Networking;
 
 namespace Passer.RoboidControl.Unity {
 
@@ -14,6 +16,8 @@ namespace Passer.RoboidControl.Unity {
         [field: SerializeField]
         public RoboidControl.Thing core { get; set; }
 
+        private string modelUrl = null;
+
         /// <summary>
         /// Set the core C# thing
         /// </summary>
@@ -27,6 +31,7 @@ namespace Passer.RoboidControl.Unity {
                 return;
             }
             siteServer.site.Add(thing);
+
         }
 
         public static Thing Create(RoboidControl.Thing core) {
@@ -55,13 +60,53 @@ namespace Passer.RoboidControl.Unity {
 
             if (core.linearVelocity != null) {
                 Vector3 direction = Quaternion.AngleAxis(core.linearVelocity.direction.horizontal, Vector3.up) * Vector3.forward;
-                this.transform.Translate(core.linearVelocity.distance * Time.deltaTime * direction);
+                this.transform.Translate(core.linearVelocity.distance * Time.deltaTime * direction, Space.Self);
             }
             if (core.angularVelocity != null) {
                 Vector3 angularVelocity = core.angularVelocity.ToVector3();
-                this.transform.rotation *= Quaternion.Euler(angularVelocity * Time.deltaTime);
+                this.transform.localRotation *= Quaternion.Euler(angularVelocity * Time.deltaTime);
+            }
+
+            if (core.hasPosition)
+                this.transform.localPosition = core.position.ToVector3();
+            //this.transform.localRotation = core.orientation.ToQuaternion();
+
+            if (!string.IsNullOrEmpty(core.modelUrl) && this.modelUrl == null) {
+                string extension = core.modelUrl.Substring(core.modelUrl.LastIndexOf("."));
+                if (extension == ".jpg" || extension == ".png") {
+                    StartCoroutine(LoadJPG());
+                }
+
+                this.modelUrl = core.modelUrl;
             }
         }
+
+        private IEnumerator LoadJPG() {
+            UnityWebRequest request = UnityWebRequestTexture.GetTexture(core.modelUrl);
+            yield return request.SendWebRequest();
+
+            if (request.result == UnityWebRequest.Result.Success) {
+                Texture2D texture = ((DownloadHandlerTexture)request.downloadHandler).texture;
+                float aspectRatio = (float)texture.width / (float)texture.height;
+
+                GameObject modelQuad = GameObject.CreatePrimitive(PrimitiveType.Quad);
+                Collider c = modelQuad.GetComponent<Collider>();
+                c.enabled = false;
+                Destroy(c);
+                modelQuad.transform.SetParent(this.transform, false);
+                modelQuad.transform.localEulerAngles = new(90, -90, 0);
+                modelQuad.transform.localScale = new Vector3(aspectRatio, 1, 1) / 5;
+                Material quadMaterial = new(Shader.Find("Unlit/Transparent")) {
+                    mainTexture = texture
+                };
+                modelQuad.GetComponent<Renderer>().material = quadMaterial;
+            }
+            else {
+                Debug.LogError("Failed to load image: " + request.error);
+            }
+        }
+
+
     }
 
 }

Unity/TouchSensor.cs

@@ -18,7 +18,7 @@ namespace Passer.RoboidControl.Unity {
         /// <summary>
         /// Start the Unity represention
         /// </summary>
-        protected virtual void Start() {
+        protected virtual void Start() {            
             if (core == null) {
                 SiteServer siteServer = FindAnyObjectByType<SiteServer>();
                 SetCoreThing(new RoboidControl.TouchSensor(siteServer.site));