From 28d3a98bea156585c804417f43fe49a3b09d88fb Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Mon, 24 Feb 2025 17:42:09 +0100 Subject: [PATCH] Arduino Ant random walk --- Messages/PoseMsg.cs | 34 ++++++++++++++++++++++++----- Participant.cs | 25 ++++++++++++++++++++- Sensors/TouchSensor.cs | 5 +++++ SiteServer.cs | 13 +++++++++-- Thing.cs | 2 ++ Unity/SiteServer.cs | 4 +++- Unity/Thing.cs | 49 ++++++++++++++++++++++++++++++++++++++++-- Unity/TouchSensor.cs | 2 +- 8 files changed, 122 insertions(+), 12 deletions(-) diff --git a/Messages/PoseMsg.cs b/Messages/PoseMsg.cs index a43b40d..4f1320d 100644 --- a/Messages/PoseMsg.cs +++ b/Messages/PoseMsg.cs @@ -69,21 +69,45 @@ namespace Passer.RoboidControl { /// The ID of the thing /// The position of the thing in local space in meters /// The orientation of the thing in local space - 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) { diff --git a/Participant.cs b/Participant.cs index cacb27a..349f50d 100644 --- a/Participant.cs +++ b/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}]"); diff --git a/Sensors/TouchSensor.cs b/Sensors/TouchSensor.cs index 72d0350..f6bc873 100644 --- a/Sensors/TouchSensor.cs +++ b/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; } diff --git a/SiteServer.cs b/SiteServer.cs index 23679c9..9bd1ae9 100644 --- a/SiteServer.cs +++ b/SiteServer.cs @@ -9,7 +9,7 @@ namespace Passer.RoboidControl { /// 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) { } /// /// Create a new site server @@ -28,6 +28,8 @@ namespace Passer.RoboidControl { this.udpClient.BeginReceive( new AsyncCallback(result => ReceiveUDP(result)), new Tuple(this.udpClient, new(IPAddress.Any, port))); + + Register(Thing.Type.TouchSensor); } /// @@ -55,7 +57,7 @@ namespace Passer.RoboidControl { if (thing == null) { Thing newThing = null; if (thingMsgProcessors.TryGetValue(msg.thingType, out Func 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); } diff --git a/Thing.cs b/Thing.cs index 95dc1b1..92d929d 100644 --- a/Thing.cs +++ b/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; /// /// Event triggered when the orientation has changed diff --git a/Unity/SiteServer.cs b/Unity/SiteServer.cs index ae16757..ce665de 100644 --- a/Unity/SiteServer.cs +++ b/Unity/SiteServer.cs @@ -15,6 +15,8 @@ namespace Passer.RoboidControl.Unity { site = new(7681); RoboidControl.Thing.OnNewThing += HandleNewThing; + + //site.Register(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); } diff --git a/Unity/Thing.cs b/Unity/Thing.cs index 2d81238..8515edb 100644 --- a/Unity/Thing.cs +++ b/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; + /// /// Set the core C# thing /// @@ -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(); + 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().material = quadMaterial; + } + else { + Debug.LogError("Failed to load image: " + request.error); + } + } + + } } diff --git a/Unity/TouchSensor.cs b/Unity/TouchSensor.cs index 31dd50a..62f6b51 100644 --- a/Unity/TouchSensor.cs +++ b/Unity/TouchSensor.cs @@ -18,7 +18,7 @@ namespace Passer.RoboidControl.Unity { /// /// Start the Unity represention /// - protected virtual void Start() { + protected virtual void Start() { if (core == null) { SiteServer siteServer = FindAnyObjectByType(); SetCoreThing(new RoboidControl.TouchSensor(siteServer.site));