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
guid: 1989654e8505b074d9a0280de8649b7d
guid: 4b081f36574e30442818c4087c3324c1
folderAsset: yes
DefaultImporter:
externalObjects: {}

2
DoxyGen/Doxyfile.meta
View File

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

2
DoxyGen/DoxygenLayout.xml.meta
View File

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

2
DoxyGen/custom_doxygen.css.meta
View File

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

2
Unity/SiteServer.cs
View File

@ -22,7 +22,7 @@ namespace RoboidControl.Unity {
}
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);
thingQueue.Enqueue(thing);
}

30
Unity/Thing.cs
View File

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

143
Unity/TouchSensor.cs
View File

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

18
src/LocalParticipant.cs
View File

@ -150,8 +150,8 @@ namespace RoboidControl {
int n = this.things.Count;
for (int ix = 0; ix < n; ix++) {
Thing thing = this.things[ix];
if (thing != null) {
thing.Update(currentTimeMS);
if (thing != null && thing.parent == null) {
thing.Update(currentTimeMS, true);
// if (thing.owner != this) {
// BinaryMsg binaryMsg = new(thing.owner.networkId, thing);
// this.Send(thing.owner, binaryMsg);
@ -293,14 +293,14 @@ namespace RoboidControl {
}
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);
if (thing != null)
thing.name = msg.name;
}
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);
if (thing != null)
thing.modelUrl = msg.url;
@ -310,18 +310,12 @@ namespace RoboidControl {
//Console.WriteLine($"Participant: Process pose [{msg.networkId}/{msg.thingId}] {msg.poseType}");
Thing thing = sender.Get(msg.networkId, msg.thingId);
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.positionUpdated = true;
}
thing.orientationUpdated = false;
if ((msg.poseType & PoseMsg.Pose_Orientation) != 0) {
thing.orientation = msg.orientation;
thing.orientationUpdated = true;
Console.Write($"{thing.id} orientation changed");
}
else
thing.orientation = null;
if ((msg.poseType & PoseMsg.Pose_LinearVelocity) != 0)
thing.linearVelocity = msg.linearVelocity;
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>
/// <returns>The thing when it is found, null in other cases.</returns>
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)
// Console.WriteLine($"Could not find thing {ipAddress}:{port}[{networkId}/{thingId}]");
return thing;

10
src/SiteServer.cs
View File

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

38
src/Thing.cs
View File

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