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