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

AR mode works, but reaction time is slow

Browse Source
This commit is contained in:
Pascal Serrarens 2025-05-21 15:59:29 +02:00
parent c4c01bedae
commit 66c58d25f5
14 changed files with 172 additions and 127 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

89
Unity/DifferentialDrive.cs
View File

@ -42,6 +42,8 @@ namespace RoboidControl.Unity {
return differentialDrive;
}
public Motor leftMotor;
public Motor rightMotor;
/// <summary>
/// The left wheel of the differential drive
/// </summary>
@ -71,41 +73,8 @@ namespace RoboidControl.Unity {
/// Handle the binary event indicating a configuration change
/// </summary>
protected override void HandleBinary() {
// Ignore it when the wheel radius is not set
if (coreDrive.wheelRadius <= 0)
return;
// Destroy any (generic) thing with the same id
if (leftWheel == null) {
Thing[] things = FindObjectsOfType<Thing>();
foreach (Thing thing in things) {
if (thing.core.id == coreDrive.leftWheel.id)
Destroy(thing.gameObject);
}
}
if (leftWheel == null && coreDrive.leftWheel != null)
leftWheel = Wheel.Create(this.transform, coreDrive.leftWheel.id);
if (leftWheel != null) {
leftWheel.core ??= coreDrive.leftWheel;
SphereCollider leftWheelCollider = leftWheel.GetComponent<SphereCollider>();
leftWheelCollider.radius = coreDrive.wheelRadius;
}
// Destroy any (generic) thing with the same id
if (rightWheel == null) {
Thing[] things = FindObjectsOfType<Thing>();
foreach (Thing thing in things) {
if (thing.core.id == coreDrive.rightWheel.id)
Destroy(thing.gameObject);
}
}
if (rightWheel == null && coreDrive.rightWheel != null)
rightWheel = Wheel.Create(this.transform, coreDrive.rightWheel.id);
if (rightWheel != null) {
rightWheel.core ??= coreDrive.rightWheel;
SphereCollider rightWheelCollider = rightWheel.GetComponent<SphereCollider>();
rightWheelCollider.radius = coreDrive.wheelRadius;
}
HandleWheelBinary(coreDrive.leftWheel, ref leftMotor, ref leftWheel);
HandleWheelBinary(coreDrive.rightWheel, ref rightMotor, ref rightWheel);
if (casterWheel == null) {
GameObject gameObj = new("Caster wheel");
@ -113,11 +82,30 @@ namespace RoboidControl.Unity {
casterWheel = gameObj.AddComponent<SphereCollider>();
casterWheel.material = Wheel.slidingWheel;
}
casterWheel.radius = coreDrive.wheelRadius;
// Put it in the middle of the back
// This code assumes that the left wheel position has Direction.left and the right wheel Direction.right...
float wheelSeparation = coreDrive.leftWheel.position.distance + coreDrive.rightWheel.position.distance;
casterWheel.center = new Vector3(0, 0, -wheelSeparation);
if (coreDrive.wheelRadius > 0 && coreDrive.leftWheel != null && coreDrive.rightWheel != null) {
casterWheel.radius = coreDrive.wheelRadius;
// Put it in the middle of the back
// This code assumes that the left wheel position has Direction.left and the right wheel Direction.right...
float wheelSeparation = coreDrive.leftWheel.position.distance + coreDrive.rightWheel.position.distance;
casterWheel.center = new Vector3(0, 0, -wheelSeparation);
}
}
private void HandleWheelBinary(RoboidControl.Motor coreMotor, ref Motor motor, ref Wheel wheel) {
if (coreMotor == null)
return;
if (motor == null) {
motor = coreMotor.component as Motor;
if (motor == null)
motor = Motor.Create(coreMotor);
wheel.transform.SetParent(motor.transform);
}
else if (motor.core.id != coreMotor.id) {
motor = coreMotor.component as Motor;
if (motor != null)
wheel.transform.SetParent(motor.transform);
}
}
/// <summary>
@ -126,18 +114,19 @@ namespace RoboidControl.Unity {
protected override void FixedUpdate() {
base.FixedUpdate();
if (rb != null && leftWheel != null && rightWheel != null) {
float leftWheelVelocity = leftWheel.rotationSpeed * (2 * Mathf.PI) * coreDrive.wheelRadius;
float rightWheelVelocity = rightWheel.rotationSpeed * (2 * Mathf.PI) * coreDrive.wheelRadius;
if (rb != null && leftMotor != null && rightMotor != null) {
float leftWheelVelocity = leftMotor.rotationSpeed * (2 * Mathf.PI) * leftWheel.wheelRadius;
float rightWheelVelocity = rightMotor.rotationSpeed * (2 * Mathf.PI) * rightWheel.wheelRadius;
// This code assumes that the left wheel position has Direction.left and the right wheel Direction.right...
float wheelSeparation = coreDrive.leftWheel.position.distance + coreDrive.rightWheel.position.distance;
float forwardSpeed = (leftWheelVelocity + rightWheelVelocity) / 2f;
float turningSpeed = (leftWheelVelocity - rightWheelVelocity) / wheelSeparation;
if (leftWheel != null && rightWheel != null) {
float wheelSeparation = Vector3.Distance(leftWheel.transform.position, rightWheel.transform.position);
float forwardSpeed = (leftWheelVelocity + rightWheelVelocity) / 2f;
float turningSpeed = (leftWheelVelocity - rightWheelVelocity) / wheelSeparation;
// Use smoothing to emulate motor inertia
rb.velocity = 0.9f * rb.velocity + 0.1f * forwardSpeed * transform.forward;
rb.angularVelocity = 0.9f * rb.angularVelocity + 0.1f * turningSpeed * Vector3.up;
// Use smoothing to emulate motor inertia
rb.velocity = 0.9f * rb.velocity + 0.1f * forwardSpeed * transform.forward;
rb.angularVelocity = 0.9f * rb.angularVelocity + 0.1f * turningSpeed * Vector3.up;
}
}
}
}

6
Unity/Motor.cs
View File

@ -34,8 +34,8 @@ namespace RoboidControl.Unity {
motor = gameObj.AddComponent<Motor>();
motor.Init(coreMotor);
Rigidbody rb = gameObj.AddComponent<Rigidbody>();
rb.isKinematic = true;
// Rigidbody rb = gameObj.AddComponent<Rigidbody>();
// rb.isKinematic = true;
}
return motor;
}
@ -43,7 +43,7 @@ namespace RoboidControl.Unity {
/// <summary>
/// The maximum rotation speed of the motor in rotations per second
/// </summary>
public float maxSpeed = 5;
public float maxSpeed = 2;
/// <summary>
/// The actual rotation speed in rotations per second

22
Unity/Participant.cs
View File

@ -8,6 +8,16 @@ namespace RoboidControl.Unity {
public RoboidControl.Participant coreParticipant;
// public Thing GetThing(RoboidControl.Thing coreThing) {
// // Not efficient or full correct but will do for now
// Thing[] things = FindObjectsOfType<Thing>();
// foreach (Thing thing in things) {
// if (thing.core.id == coreThing.id)
// return thing;
// }
// return null;
// }
protected virtual void Update() {
if (coreParticipant == null)
return;
@ -36,12 +46,12 @@ namespace RoboidControl.Unity {
DifferentialDrive differentialDrive = DifferentialDrive.Create(coreDrive);
coreDrive.component = differentialDrive;
break;
// case RoboidControl.Motor coreMotor:
// //Wheel wheel = Wheel.Create(coreMotor);
// //coreMotor.component = wheel;
// // We need to know the details (though a binary msg)
// // before we can create the wheel reliably
// break;
case RoboidControl.Motor coreMotor:
Motor wheel = Motor.Create(coreMotor);
coreMotor.component = wheel;
// We need to know the details (though a binary msg)
// before we can create the wheel reliably
break;
case RoboidControl.Thing coreThing:
Debug.Log("Handle Thing");
if (coreThing.component == null) {

24
Unity/Resources/DifferentialDrive_disabled.prefab → Unity/Resources/DifferentialDrive.prefab
View File

@ -101,7 +101,7 @@ BoxCollider:
m_LayerOverridePriority: 0
m_IsTrigger: 0
m_ProvidesContacts: 0
m_Enabled: 0
m_Enabled: 1
serializedVersion: 3
m_Size: {x: 1, y: 1, z: 1}
m_Center: {x: -3.4930155e-36, y: 0, z: 0}
@ -156,7 +156,7 @@ SphereCollider:
m_ProvidesContacts: 0
m_Enabled: 1
serializedVersion: 3
m_Radius: 0.00001
m_Radius: 0.025
m_Center: {x: 0, y: 0, z: 0}
--- !u!1 &3377575892836316963
GameObject:
@ -204,9 +204,8 @@ MonoBehaviour:
m_Script: {fileID: 11500000, guid: ddd5065d5e866894cbcb569c3a898ccb, type: 3}
m_Name:
m_EditorClassIdentifier:
participant: {fileID: 0}
maxSpeed: 5
rotationSpeed: 0
owner: {fileID: 0}
wheelRadius: 0.025
--- !u!135 &3720747953092717687
SphereCollider:
m_ObjectHideFlags: 0
@ -226,7 +225,7 @@ SphereCollider:
m_ProvidesContacts: 0
m_Enabled: 1
serializedVersion: 3
m_Radius: 0.00001
m_Radius: 0.025
m_Center: {x: 0, y: 0, z: 0}
--- !u!1 &4326386140118987503
GameObject:
@ -240,7 +239,7 @@ GameObject:
- component: {fileID: 2975045340952151157}
- component: {fileID: 4118868690347991998}
m_Layer: 0
m_Name: DifferentialDrive_disabled
m_Name: DifferentialDrive
m_TagString: Untagged
m_Icon: {fileID: 0}
m_NavMeshLayer: 0
@ -277,7 +276,9 @@ MonoBehaviour:
m_Script: {fileID: 11500000, guid: 5c80bc2fd1d8aa14facb1ad4b6ccf7b3, type: 3}
m_Name:
m_EditorClassIdentifier:
participant: {fileID: 0}
owner: {fileID: 0}
leftMotor: {fileID: 0}
rightMotor: {fileID: 0}
leftWheel: {fileID: 6773972788910618332}
rightWheel: {fileID: 7425139233737877139}
casterWheel: {fileID: 894333789048616995}
@ -437,9 +438,8 @@ MonoBehaviour:
m_Script: {fileID: 11500000, guid: ddd5065d5e866894cbcb569c3a898ccb, type: 3}
m_Name:
m_EditorClassIdentifier:
participant: {fileID: 0}
maxSpeed: 5
rotationSpeed: 0
owner: {fileID: 0}
wheelRadius: 0.025
--- !u!135 &8150853676422519226
SphereCollider:
m_ObjectHideFlags: 0
@ -459,7 +459,7 @@ SphereCollider:
m_ProvidesContacts: 0
m_Enabled: 1
serializedVersion: 3
m_Radius: 0.00001
m_Radius: 0.025
m_Center: {x: 0, y: 0, z: 0}
--- !u!1 &5663058822615269368
GameObject:

15
Unity/Thing.cs
View File

@ -46,8 +46,13 @@ namespace RoboidControl.Unity {
protected void Init(RoboidControl.Thing core) {
this.core = core;
this.core.component = this;
this.owner = FindAnyObjectByType<SiteServer>();
core.owner = this.owner.coreParticipant;
Participant[] participants = FindObjectsByType<Participant>(FindObjectsSortMode.None);
foreach (Participant participant in participants) {
if (participant.ipAddress == core.owner.ipAddress && participant.port == core.owner.port)
this.owner = participant;
}
//this.owner = FindAnyObjectByType<SiteServer>();
//core.owner = this.owner.coreParticipant;
if (core.parent != null && core.parent.component != null) {
this.transform.SetParent(core.parent.component.transform, false);
@ -90,8 +95,8 @@ namespace RoboidControl.Unity {
/// </summary>
public void UpdateThing() {
if (core == null) {
Debug.Log($"{this} core thing is gone, self destruct in 0 seconds...");
Destroy(this);
//Debug.Log($"{this} core thing is gone, self destruct in 0 seconds...");
//Destroy(this);
return;
}
@ -129,7 +134,7 @@ namespace RoboidControl.Unity {
this.HandlePose();
break;
case BinaryMsg.Id:
Debug.Log($"{this.core.id} Handle Binary");
// Debug.Log($"{this.core.id} Handle Binary");
this.HandleBinary();
break;
}

32
Unity/TouchSensor.cs
View File

@ -45,6 +45,8 @@ namespace RoboidControl.Unity {
return touchSensor;
}
public bool touchedSomething = false;
/// <summary>
/// Handle touch trigger collider enter event
/// </summary>
@ -57,17 +59,35 @@ namespace RoboidControl.Unity {
if (this.transform.root == other.transform.root)
return;
Debug.Log("TOUCH!");
this.touchedSomething = true;
this.coreSensor.touchedSomething = true;
this.core.updateQueue.Enqueue(new RoboidControl.Thing.CoreEvent(BinaryMsg.Id));
//this.core.updateQueue.Enqueue(new RoboidControl.Thing.CoreEvent(BinaryMsg.Id));
}
/// <summary>
/// Handl touch trigger collider exit event
/// </summary>
/// <param name="other">The collider which exited our trigger collider </param>
private void OnTriggerExit(Collider other) {
// private void OnTriggerStay(Collider other) {
// // Don't detect trigger colliders
// if (other.isTrigger)
// return;
// // Don't touch yourself
// if (this.transform.root == other.transform.root)
// return;
// Debug.Log("Touching...");
// this.touchedSomething = true;
// this.coreSensor.touchedSomething = true;
// }
/// <summary>
/// Handl touch trigger collider exit event
/// </summary>
/// <param name="other">The collider which exited our trigger collider </param>
private void OnTriggerExit(Collider other) {
if (other.isTrigger)
return;
Debug.Log("TOUCH END!");
this.touchedSomething = false;
this.coreSensor.touchedSomething = false;
}
}

6
Unity/Wheel.cs
View File

@ -6,13 +6,13 @@ namespace RoboidControl.Unity {
/// <summary>
/// The Unity representation of a Roboid Control wheel
/// </summary>
public class Wheel : Motor {
public class Wheel : Thing {
/// <summary>
/// Create the Unity representation
/// </summary>
/// <param name="core">The core motor</param>
/// <returns>The Unity representation of a motorised wheel</returns>
public static Wheel Create(RoboidControl.Motor core, float wheelRadius) {
public static new Wheel Create(RoboidControl.Thing core) {
GameObject prefab = (GameObject)Resources.Load("Wheel");
if (prefab != null) {
// Use resource prefab when available
@ -62,6 +62,8 @@ namespace RoboidControl.Unity {
}
}
public float wheelRadius = 0;
private static PhysicMaterial _slidingWheel;
public static PhysicMaterial slidingWheel {
get {

2
src/Messages/BinaryMsg.cs
View File

@ -72,7 +72,7 @@ namespace RoboidControl {
return 0;
#if DEBUG
System.Console.WriteLine($"Send BinaryMsg [{this.networkId}/{this.thingId}] {this.dataLength}");
// System.Console.WriteLine($"Send BinaryMsg [{this.networkId}/{this.thingId}] {this.dataLength}");
#endif
byte ix = 0;
buffer[ix++] = BinaryMsg.Id;

4
src/Participant.cs
View File

@ -120,9 +120,9 @@ namespace RoboidControl {
return true;
IPEndPoint participantEndpoint = new IPEndPoint(IPAddress.Parse(this.ipAddress), this.port);
Console.WriteLine($"msg to remote participant {participantEndpoint.Address.ToString()} {participantEndpoint.Port}");
// Console.WriteLine($"msg to remote participant {participantEndpoint.Address.ToString()} {participantEndpoint.Port}");
if (udpClient != null) {
Console.WriteLine("sending...");
//Console.WriteLine("sending...");
this.udpClient?.Send(this.buffer, bufferSize, participantEndpoint);
}
return true;

73
src/Participants/ParticipantUDP.cs
View File

@ -113,7 +113,8 @@ namespace RoboidControl {
/// <summary>
/// The interval in milliseconds for publishing (broadcasting) data on the local network
/// </summary>
public ulong publishInterval = 3000; // = 3 seconds
public ulong publishIntervalMS = 3000; // = 3 seconds
public ulong sendUpdateIntervalMS = 100; // for object updates
//public byte[] buffer = new byte[1024];
@ -145,21 +146,22 @@ namespace RoboidControl {
#region Update
protected ulong nextPublishMe = 0;
protected ulong nextSendUpdate = 0;
public override void Update(ulong currentTimeMS = 0) {
if (currentTimeMS == 0)
currentTimeMS = Thing.GetTimeMs();
if (this.isIsolated == false) {
if (this.publishInterval > 0 && currentTimeMS > this.nextPublishMe) {
ParticipantMsg msg = new ParticipantMsg(this.networkId);
if (this.publishIntervalMS > 0 && currentTimeMS > this.nextPublishMe) {
ParticipantMsg msg = new(this.networkId);
if (this.remoteSite == null)
this.Publish(msg);
else
//this.Send(this.remoteSite, msg);
this.remoteSite.Send(msg);
this.nextPublishMe = currentTimeMS + this.publishInterval;
this.nextPublishMe = currentTimeMS + this.publishIntervalMS;
}
}
@ -214,17 +216,20 @@ namespace RoboidControl {
if (this.isIsolated)
continue;
for (int thingIx = 0; thingIx < participant.things.Count; thingIx++) {
Thing thing = participant.things[thingIx];
if (thing == null)
continue;
if (currentTimeMS > this.nextSendUpdate) {
for (int thingIx = 0; thingIx < participant.things.Count; thingIx++) {
Thing thing = participant.things[thingIx];
if (thing == null)
continue;
// PoseMsg poseMsg = new(thing.owner.networkId, thing);
// this.Send(participant, poseMsg);
// BinaryMsg binaryMsg = new(thing.owner.networkId, thing);
// this.Send(participant, binaryMsg);
participant.Send(new PoseMsg(thing.owner.networkId, thing));
participant.Send(new BinaryMsg(thing.owner.networkId, thing));
// PoseMsg poseMsg = new(thing.owner.networkId, thing);
// this.Send(participant, poseMsg);
// BinaryMsg binaryMsg = new(thing.owner.networkId, thing);
// this.Send(participant, binaryMsg);
participant.Send(new PoseMsg(thing.owner.networkId, thing));
participant.Send(new BinaryMsg(thing.owner.networkId, thing));
}
this.nextSendUpdate = currentTimeMS + this.sendUpdateIntervalMS;
}
}
@ -283,26 +288,32 @@ namespace RoboidControl {
#region Receive
protected void ReceiveUDP(IAsyncResult result) {
// UnityEngine.Debug.Log("received");
if (this.udpClient == null) // || this.endPoint == null)
return;
try {
// UnityEngine.Debug.Log("received");
if (this.udpClient == null) // || this.endPoint == null)
return;
byte[] data = this.udpClient.EndReceive(result, ref endPoint);
// This does not yet take multi-packet messages into account!
if (endPoint == null)
return;
byte[] data = this.udpClient.EndReceive(result, ref endPoint);
// This does not yet take multi-packet messages into account!
if (endPoint == null)
return;
// We can receive our own publish (broadcast) packages. How do we recognize them????
// It is hard to determine our source port
string ipAddress = endPoint.Address.ToString();
if (ipAddress != this.ipAddress) {
Participant remoteParticipant = GetParticipant(ipAddress, endPoint.Port);
remoteParticipant ??= AddParticipant(ipAddress, endPoint.Port, this);
// We can receive our own publish (broadcast) packages. How do we recognize them????
// It is hard to determine our source port
string ipAddress = endPoint.Address.ToString();
if (ipAddress != this.ipAddress) {
Participant remoteParticipant = GetParticipant(ipAddress, endPoint.Port);
remoteParticipant ??= AddParticipant(ipAddress, endPoint.Port, this);
ReceiveData(data, remoteParticipant);
ReceiveData(data, remoteParticipant);
}
udpClient.BeginReceive(new AsyncCallback(callbackResult => ReceiveUDP(callbackResult)), null);
}
catch (Exception e) {
Console.WriteLine($"Exception in communication thread: {e.Message}");
Console.WriteLine($"Stack trace: {e.StackTrace}");
}
udpClient.BeginReceive(new AsyncCallback(callbackResult => ReceiveUDP(callbackResult)), null);
}
public void ReceiveData(byte[] data, Participant sender) {
@ -424,7 +435,7 @@ namespace RoboidControl {
protected virtual void Process(Participant sender, BinaryMsg msg) {
#if DEBUG
Console.WriteLine($"{this.name}: Process BinaryMsg [{msg.networkId}/{msg.thingId}] {msg.dataLength}");
// Console.WriteLine($"{this.name}: Process BinaryMsg [{msg.networkId}/{msg.thingId}] {msg.dataLength}");
#endif
Thing thing = sender.Get(msg.thingId);
thing?.ProcessBinary(msg.data);

1
src/Participants/SiteServer.cs
View File

@ -134,6 +134,7 @@ namespace RoboidControl {
return msg.thingType switch {
Thing.Type.TouchSensor => new TouchSensor(sender, msg.thingId),
Thing.Type.DifferentialDrive => new DifferentialDrive(sender, msg.thingId),
Thing.Type.UncontrolledMotor => new Motor(sender, msg.thingId),
_ => new Thing(sender, msg.thingType, msg.thingId),
};

3
src/Things/DifferentialDrive.cs
View File

@ -160,7 +160,8 @@ namespace RoboidControl {
public override void ProcessBinary(byte[] data) {
byte ix = 0;
byte leftWheelId = data[ix++];
this.leftWheel = this.owner.Get(leftWheelId) as Motor;
Thing leftMotor = this.owner.Get(leftWheelId);
this.leftWheel = leftMotor as Motor;
byte rightWheelId = data[ix++];
this.rightWheel = this.owner.Get(rightWheelId) as Motor;
this._wheelRadius = LowLevelMessages.ReceiveFloat16(data, ref ix);

4
src/Things/Motor.cs
View File

@ -5,6 +5,8 @@ namespace RoboidControl {
public class Motor : Thing {
public Motor(bool invokeEvent = true) : base(Type.UncontrolledMotor, invokeEvent) { }
public Motor(Thing parent, byte thingId = 0, bool invokeEvent = true) : base(parent, Type.UncontrolledMotor, thingId, invokeEvent) { }
public Motor(Participant owner, byte thingId = 0, bool invokeEvent = true) : base(owner, Type.UncontrolledMotor, thingId, invokeEvent) { }
/// @brief Motor turning direction
public enum Direction {
@ -38,7 +40,7 @@ namespace RoboidControl {
return data;
}
public override void ProcessBinary(byte[] data) {
this.targetSpeed = (float)data[0] / 127;
this.targetSpeed = (float)(sbyte)data[0] / 127;
}
}

14
src/Things/TouchSensor.cs
View File

@ -35,9 +35,10 @@ namespace RoboidControl {
public bool touchedSomething {
get { return _touchedSomething; }
set {
if (_touchedSomething != value) {
//if (_touchedSomething != value) {
_touchedSomething = value;
}
owner.Send(new BinaryMsg(this));
//}
touchUpdated = true;
}
}
@ -57,21 +58,24 @@ namespace RoboidControl {
/// <returns>A byte array with the binary data</returns>
/// <remark>The byte array will be empty when the touch status has not changed</remark>
public override byte[] GenerateBinary() {
if (!touchUpdated)
if (this.component == null || !touchUpdated)
return Array.Empty<byte>();
Unity.TouchSensor touchComponent = this.component as Unity.TouchSensor;
byte[] bytes = new byte[1];
bytes[0] = (byte)(touchedSomething ? 1 : 0);
bytes[0] = (byte)(touchComponent.touchedSomething ? 1 : 0);
touchUpdated = false;
Console.WriteLine($"Sending touch {bytes[0]}");
return bytes;
}
private bool externalTouch = false;
/// <summary>
/// Function used to process binary data received for this touch sensor
/// </summary>
/// <param name="bytes">The binary data to process</param>
public override void ProcessBinary(byte[] bytes) {
this.touchedSomething |= (bytes[0] == 1);
this.externalTouch = (bytes[0] == 1);
}
}
}