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

Fix roboidcontrol repo issue

Browse Source
This commit is contained in:
Pascal Serrarens 2025-03-19 17:35:21 +01:00
parent 33ae03bd83
commit 390f7807cd
11 changed files with 132 additions and 120 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
DoxyGen.meta
View File

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

2
DoxyGen/Doxyfile.meta
View File

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

2
DoxyGen/DoxygenLayout.xml.meta
View File

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

2
DoxyGen/custom_doxygen.css.meta
View File

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

2
Unity/SiteServer.cs
View File

@ -22,7 +22,7 @@ namespace RoboidControl.Unity {
} }
public void HandleNewThing(RoboidControl.Thing thing) { 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); site.Add(thing, false);
thingQueue.Enqueue(thing); thingQueue.Enqueue(thing);
} }

28
Unity/Thing.cs
View File

@ -1,5 +1,6 @@
#if UNITY_5_3_OR_NEWER #if UNITY_5_3_OR_NEWER
using System.Collections; using System.Collections;
using Mono.Cecil.Cil;
using UnityEngine; using UnityEngine;
using UnityEngine.Networking; using UnityEngine.Networking;
@ -18,6 +19,10 @@ namespace RoboidControl.Unity {
private string modelUrl = null; private string modelUrl = null;
// protected virtual void Awake() {
// core.OnPoseChanged += PoseChanged;
// }
/// <summary> /// <summary>
/// Set the core C# thing /// Set the core C# thing
/// </summary> /// </summary>
@ -31,7 +36,7 @@ namespace RoboidControl.Unity {
return; return;
} }
siteServer.site.Add(thing); siteServer.site.Add(thing);
core.OnPoseChanged += PoseChanged;
} }
public static Thing Create(RoboidControl.Thing core) { public static Thing Create(RoboidControl.Thing core) {
@ -47,7 +52,10 @@ namespace RoboidControl.Unity {
if (core.position != null) if (core.position != null)
gameObj.transform.localPosition = core.position.ToVector3(); gameObj.transform.localPosition = core.position.ToVector3();
if (core.orientation != null)
gameObj.transform.localRotation = core.orientation.ToQuaternion();
core.OnPoseChanged += component.PoseChanged;
return component; return component;
} }
@ -61,16 +69,18 @@ namespace RoboidControl.Unity {
if (core.linearVelocity != null && core.linearVelocity.distance != 0) { if (core.linearVelocity != null && core.linearVelocity.distance != 0) {
Vector3 direction = Quaternion.AngleAxis(core.linearVelocity.direction.horizontal, Vector3.up) * Vector3.forward; Vector3 direction = Quaternion.AngleAxis(core.linearVelocity.direction.horizontal, Vector3.up) * Vector3.forward;
this.transform.Translate(core.linearVelocity.distance * Time.deltaTime * direction, Space.Self); 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) { if (core.angularVelocity != null && core.angularVelocity.distance != 0) {
Vector3 angularVelocity = core.angularVelocity.ToVector3(); Vector3 angularVelocity = core.angularVelocity.ToVector3();
Vector3 axis = core.angularVelocity.direction.ToVector3(); Vector3 axis = core.angularVelocity.direction.ToVector3();
this.transform.localRotation *= Quaternion.AngleAxis(core.angularVelocity.distance * Time.deltaTime, axis); this.transform.localRotation *= Quaternion.AngleAxis(core.angularVelocity.distance * Time.deltaTime, axis);
//this.transform.localRotation *= Quaternion.Euler(angularVelocity * Time.deltaTime); //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) { if (!string.IsNullOrEmpty(core.modelUrl) && this.modelUrl == null) {
string extension = core.modelUrl.Substring(core.modelUrl.LastIndexOf(".")); 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() { private IEnumerator LoadJPG() {
UnityWebRequest request = UnityWebRequestTexture.GetTexture(core.modelUrl); UnityWebRequest request = UnityWebRequestTexture.GetTexture(core.modelUrl);
yield return request.SendWebRequest(); yield return request.SendWebRequest();

141
Unity/TouchSensor.cs
View File

@ -1,87 +1,94 @@
#if UNITY_5_3_OR_NEWER #if UNITY_5_3_OR_NEWER
using Unity.VisualScripting;
using UnityEngine; using UnityEngine;
namespace RoboidControl.Unity { namespace RoboidControl.Unity {
/// <summary> /// <summary>
/// The Unity representation of the TouchSensor /// The Unity representation of the TouchSensor
/// </summary> /// </summary>
public class TouchSensor : Thing { public class TouchSensor : Thing {
public SiteServer participant; public SiteServer participant;
/// <summary> /// <summary>
/// The core touch sensor /// The core touch sensor
/// </summary> /// </summary>
public RoboidControl.TouchSensor coreSensor { public RoboidControl.TouchSensor coreSensor {
get => (RoboidControl.TouchSensor)base.core; get => (RoboidControl.TouchSensor)base.core;
} }
/// <summary> /// <summary>
/// Start the Unity represention /// Start the Unity represention
/// </summary> /// </summary>
protected virtual void Start() { protected virtual void Start() {
if (core == null) { if (core == null) {
participant = FindAnyObjectByType<SiteServer>(); participant = FindAnyObjectByType<SiteServer>();
SetCoreThing(new RoboidControl.TouchSensor(participant.site)); SetCoreThing(new RoboidControl.TouchSensor(participant.site));
} }
// Somehow this does not work. // Somehow this does not work.
// Rigidbody rb = GetComponentInParent<Rigidbody>(); // Rigidbody rb = GetComponentInParent<Rigidbody>();
// if (rb == null) { // if (rb == null) {
// RoboidControl.Thing thing = core; // RoboidControl.Thing thing = core;
// while (thing.parent != null) // while (thing.parent != null)
// thing = thing.parent; // thing = thing.parent;
// Thing unityThing = thing.component; // Thing unityThing = thing.component;
// rb = unityThing.gameObject.AddComponent<Rigidbody>(); // rb = unityThing.gameObject.AddComponent<Rigidbody>();
// rb.isKinematic = true; // rb.isKinematic = true;
// } // }
} }
/// <summary> /// <summary>
/// Create the Unity representation /// Create the Unity representation
/// </summary> /// </summary>
/// <param name="core">The core touch sensor</param> /// <param name="core">The core touch sensor</param>
/// <returns>The Unity representation of the touch sensor</returns> /// <returns>The Unity representation of the touch sensor</returns>
public static TouchSensor Create(RoboidControl.TouchSensor core) { public static TouchSensor Create(RoboidControl.TouchSensor core) {
GameObject gameObj = core.name != null ? GameObject gameObj = core.name != null ?
new(core.name) : new(core.name) :
new("Touch Sensor"); new("Touch Sensor");
TouchSensor component = gameObj.AddComponent<TouchSensor>(); TouchSensor component = gameObj.AddComponent<TouchSensor>();
Rigidbody rb = gameObj.AddComponent<Rigidbody>(); Rigidbody rb = gameObj.AddComponent<Rigidbody>();
rb.isKinematic = true; rb.isKinematic = true;
SphereCollider collider = gameObj.AddComponent<SphereCollider>(); SphereCollider collider = gameObj.AddComponent<SphereCollider>();
collider.radius = 0.01F; collider.radius = 0.01f;
collider.isTrigger = true; collider.isTrigger = true;
component.core = core; component.core = core;
component.participant = FindAnyObjectByType<SiteServer>(); component.participant = FindAnyObjectByType<SiteServer>();
core.thisParticipant = component.participant.site; 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); gameObj.transform.SetParent(core.parent.component.transform, false);
}
if (core.position != null) if (core.position != null)
gameObj.transform.localPosition = core.position.ToVector3(); gameObj.transform.localPosition = core.position.ToVector3();
return component; 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;
}
private void OnTriggerEnter(Collider other) { return component;
if (other.isTrigger) }
return;
if (this.transform.root == other.transform.root)
return;
this.coreSensor.touchedSomething = true; private void OnTriggerEnter(Collider other) {
} if (other.isTrigger)
private void OnTriggerExit(Collider other) { return;
if (other.isTrigger) if (this.transform.root == other.transform.root)
return; return;
this.coreSensor.touchedSomething = false; this.coreSensor.touchedSomething = true;
} }
} private void OnTriggerExit(Collider other) {
if (other.isTrigger)
return;
this.coreSensor.touchedSomething = false;
}
}
} }
#endif #endif

18
src/LocalParticipant.cs
View File

@ -150,8 +150,8 @@ namespace RoboidControl {
int n = this.things.Count; int n = this.things.Count;
for (int ix = 0; ix < n; ix++) { for (int ix = 0; ix < n; ix++) {
Thing thing = this.things[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) { // if (thing.owner != this) {
// BinaryMsg binaryMsg = new(thing.owner.networkId, thing); // BinaryMsg binaryMsg = new(thing.owner.networkId, thing);
// this.Send(thing.owner, binaryMsg); // this.Send(thing.owner, binaryMsg);
@ -293,14 +293,14 @@ namespace RoboidControl {
} }
protected virtual void Process(Participant sender, NameMsg msg) { 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); Thing thing = sender.Get(msg.networkId, msg.thingId);
if (thing != null) if (thing != null)
thing.name = msg.name; thing.name = msg.name;
} }
protected virtual void Process(Participant sender, ModelUrlMsg msg) { 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); Thing thing = sender.Get(msg.networkId, msg.thingId);
if (thing != null) if (thing != null)
thing.modelUrl = msg.url; thing.modelUrl = msg.url;
@ -310,18 +310,12 @@ namespace RoboidControl {
//Console.WriteLine($"Participant: Process pose [{msg.networkId}/{msg.thingId}] {msg.poseType}"); //Console.WriteLine($"Participant: Process pose [{msg.networkId}/{msg.thingId}] {msg.poseType}");
Thing thing = sender.Get(msg.networkId, msg.thingId); Thing thing = sender.Get(msg.networkId, msg.thingId);
if (thing != null) { 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.position = msg.position;
thing.positionUpdated = true;
}
thing.orientationUpdated = false;
if ((msg.poseType & PoseMsg.Pose_Orientation) != 0) { if ((msg.poseType & PoseMsg.Pose_Orientation) != 0) {
thing.orientation = msg.orientation; thing.orientation = msg.orientation;
thing.orientationUpdated = true; Console.Write($"{thing.id} orientation changed");
} }
else
thing.orientation = null;
if ((msg.poseType & PoseMsg.Pose_LinearVelocity) != 0) if ((msg.poseType & PoseMsg.Pose_LinearVelocity) != 0)
thing.linearVelocity = msg.linearVelocity; thing.linearVelocity = msg.linearVelocity;
if ((msg.poseType & PoseMsg.Pose_AngularVelocity) != 0) if ((msg.poseType & PoseMsg.Pose_AngularVelocity) != 0)

3
src/Participant.cs
View File

@ -51,7 +51,8 @@ namespace RoboidControl {
/// <param name="thingId">The ID of the thing</param> /// <param name="thingId">The ID of the thing</param>
/// <returns>The thing when it is found, null in other cases.</returns> /// <returns>The thing when it is found, null in other cases.</returns>
public Thing Get(byte networkId, byte thingId) { 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) // if (thing == null)
// Console.WriteLine($"Could not find thing {ipAddress}:{port}[{networkId}/{thingId}]"); // Console.WriteLine($"Could not find thing {ipAddress}:{port}[{networkId}/{thingId}]");
return thing; return thing;

10
src/SiteServer.cs
View File

@ -44,7 +44,7 @@ namespace RoboidControl {
protected override void Process(Participant remoteParticipant, ParticipantMsg msg) { protected override void Process(Participant remoteParticipant, ParticipantMsg msg) {
if (msg.networkId == 0) { 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)); 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, NetworkIdMsg msg) { }
protected override void Process(Participant sender, ThingMsg 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); Thing thing = sender.Get(msg.networkId, msg.thingId);
if (thing == null) { if (thing == null) {
Thing newThing = null; Thing newThing = null;
@ -68,12 +68,12 @@ namespace RoboidControl {
if (msg.parentId != 0) { if (msg.parentId != 0) {
Thing parentThing = Get(msg.networkId, msg.parentId); Thing parentThing = Get(msg.networkId, msg.parentId);
if (parentThing == null) if (parentThing == null)
Console.WriteLine("Could not find parent"); Console.WriteLine($"Could not find parent [{msg.networkId}/{msg.parentId}]");
else else
newThing.parent = parentThing; 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);
} }
} }

38
src/Thing.cs
View File

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