RoboidControl/RoboidControl-csharp
RoboidControl/RoboidControl-csharp
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Arduino Ant random walk

Browse Source
This commit is contained in:
Pascal Serrarens 2025-02-24 17:42:09 +01:00
parent c42c253362
commit 28d3a98bea
8 changed files with 122 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
Messages/PoseMsg.cs
View File

@ -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) {

25
Participant.cs
View File

@ -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}]");

5
Sensors/TouchSensor.cs
View File

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

13
SiteServer.cs
View File

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

2
Thing.cs
View File

@ -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

4
Unity/SiteServer.cs
View File

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

49
Unity/Thing.cs
View File

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

2
Unity/TouchSensor.cs
View File

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