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

ESP32 tracker sync.

Browse Source
This commit is contained in:
Pascal Serrarens 2025-04-09 11:07:43 +02:00
parent e51159bd1b
commit 6d58b741e1
5 changed files with 80 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
src/Messages/PoseMsg.cs
View File

@ -88,6 +88,34 @@ namespace RoboidControl {
this.linearVelocity = linearVelocity;
this.angularVelocity = angularVelocity;
}
public PoseMsg(byte networkId, Thing thing, bool force = false) {
this.networkId = networkId;
this.thingId = thing.id;
this.poseType = 0;
if (thing.positionUpdated || force) {
this.position = thing.position;
this.poseType |= Pose_Position;
//thing.positionUpdated = false; // this is also reset in Thing.update, leave it out here?
}
if (thing.orientationUpdated || force) {
this.orientation = thing.orientation;
this.poseType |= Pose_Orientation;
//thing.orientationUpdated = false; // this is also reset in Thing.update, leave it out here?
}
if (thing.linearVelocityUpdated) {
this.linearVelocity = thing.linearVelocity;
this.poseType |= Pose_LinearVelocity;
thing.linearVelocityUpdated = false;
}
if (thing.angularVelocityUpdated) {
this.angularVelocity = thing.angularVelocity;
this.poseType |= Pose_AngularVelocity;
thing.angularVelocityUpdated = false;
}
}
/// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer)
public PoseMsg(byte[] buffer) : base(buffer) {
byte ix = 1;

10
src/Participant.cs
View File

@ -44,6 +44,16 @@ namespace RoboidControl {
/// </summary>
protected readonly List<Thing> things = new List<Thing>();
public virtual void Update(ulong currentTimeMS = 0) {
int n = this.things.Count;
for (int ix = 0; ix < n; ix++) {
Thing thing = this.things[ix];
if (thing != null)
thing.Update(currentTimeMS, true);
}
}
/// <summary>
/// Get a thing with the given ids
/// </summary>

48
src/ParticipantUDP.cs
View File

@ -94,20 +94,20 @@ namespace RoboidControl {
protected readonly Dictionary<byte, Func<Participant, byte, byte, Thing>> thingMsgProcessors = new();
public delegate Thing ThingConstructor(Participant sender, byte networkId, byte thingId);
public void Register(byte thingType, ThingConstructor constr) {
thingMsgProcessors[thingType] = new Func<Participant, byte, byte, Thing>(constr);
}
// public delegate Thing ThingConstructor(Participant sender, byte networkId, byte thingId);
// public void Register(byte thingType, ThingConstructor constr) {
// thingMsgProcessors[thingType] = new Func<Participant, byte, byte, Thing>(constr);
// }
public void Register<ThingClass>(Thing.Type thingType) where ThingClass : Thing {
Register<ThingClass>((byte)thingType);
}
// public void Register<ThingClass>(Thing.Type thingType) where ThingClass : Thing {
// Register<ThingClass>((byte)thingType);
// }
public void Register<ThingClass>(byte thingType) where ThingClass : Thing {
thingMsgProcessors[thingType] = (participant, networkId, thingId) =>
Activator.CreateInstance(typeof(ThingClass), participant, networkId, thingId) as ThingClass;
Console.WriteLine($"Registering {typeof(ThingClass)} for thing type {thingType}");
}
// public void Register<ThingClass>(byte thingType) where ThingClass : Thing {
// thingMsgProcessors[thingType] = (participant, networkId, thingId) =>
// Activator.CreateInstance(typeof(ThingClass), participant, networkId, thingId) as ThingClass;
// Console.WriteLine($"Registering {typeof(ThingClass)} for thing type {thingType}");
// }
#endregion Init
@ -134,7 +134,7 @@ namespace RoboidControl {
}
protected ulong nextPublishMe = 0;
public virtual void Update(ulong currentTimeMS = 0) {
public override void Update(ulong currentTimeMS = 0) {
if (currentTimeMS == 0) {
#if UNITY_5_3_OR_NEWER
currentTimeMS = (ulong)(UnityEngine.Time.time * 1000);
@ -152,12 +152,19 @@ namespace RoboidControl {
Thing thing = this.things[ix];
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);
// }
// if (this.isIsolated == false) {
if (thing.owner != this) { // should not happen....
PoseMsg poseMsg = new(thing.owner.networkId, thing);
this.Send(thing.owner, poseMsg);
BinaryMsg binaryMsg = new(thing.owner.networkId, thing);
this.Send(thing.owner, binaryMsg);
}
}
}
for (int ownerIx = 0; ownerIx < this.owners.Count; ownerIx++) {
Participant owner = this.owners[ownerIx];
owner.Update(currentTimeMS);
}
}
public virtual void Publish() {
@ -309,11 +316,13 @@ namespace RoboidControl {
}
protected virtual void Process(Participant sender, PoseMsg msg) {
//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);
if (thing != null) {
if ((msg.poseType & PoseMsg.Pose_Position) != 0)
if ((msg.poseType & PoseMsg.Pose_Position) != 0) {
thing.position = msg.position;
Console.Write($"{thing.id} position changed");
}
if ((msg.poseType & PoseMsg.Pose_Orientation) != 0) {
thing.orientation = msg.orientation;
Console.Write($"{thing.id} orientation changed");
@ -322,7 +331,6 @@ namespace RoboidControl {
thing.linearVelocity = msg.linearVelocity;
if ((msg.poseType & PoseMsg.Pose_AngularVelocity) != 0)
thing.angularVelocity = msg.angularVelocity;
}
}

18
src/SiteServer.cs
View File

@ -29,7 +29,7 @@ namespace RoboidControl {
new AsyncCallback(result => ReceiveUDP(result)),
new Tuple<UdpClient, IPEndPoint>(this.udpClient, new(IPAddress.Any, port)));
Register<TouchSensor>(Thing.Type.TouchSensor);
// Register<TouchSensor>(Thing.Type.TouchSensor);
}
/// <summary>
@ -45,7 +45,7 @@ namespace RoboidControl {
protected override void Process(Participant sender, ParticipantMsg msg) {
base.Process(sender, msg);
//if (msg.networkId == 0) {
Console.WriteLine($"{this.name} received New Participant -> {sender.networkId}");
//Console.WriteLine($"{this.name} received New Participant -> {sender.networkId}");
this.Send(sender, new NetworkIdMsg(sender.networkId));
//}
}
@ -57,15 +57,15 @@ namespace RoboidControl {
Thing thing = sender.Get(msg.networkId, msg.thingId);
if (thing == null) {
Thing newThing = null;
if (thingMsgProcessors.TryGetValue(msg.thingType, out Func<Participant, byte, byte, Thing> msgProcessor)) {
//Console.WriteLine("Found thing message processor");
if (msgProcessor != null)
newThing = msgProcessor(sender, msg.networkId, msg.thingId);
}
if (newThing == null) {
// if (thingMsgProcessors.TryGetValue(msg.thingType, out Func<Participant, byte, byte, Thing> msgProcessor)) {
// //Console.WriteLine("Found thing message processor");
// if (msgProcessor != null)
// newThing = msgProcessor(sender, msg.networkId, msg.thingId);
// }
// if (newThing == null) {
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)

14
src/Thing.cs
View File

@ -53,7 +53,7 @@ namespace RoboidControl {
if (invokeEvent)
InvokeNewThing(this);
}
public Thing(Participant owner) : this(owner, Type.Undetermined) {}
public Thing(Participant owner) : this(owner, Type.Undetermined) { }
/// <summary>
/// Create a new thing for a participant
/// </summary>
@ -286,10 +286,6 @@ namespace RoboidControl {
}
}
/// <summary>
/// Event triggered when the orientation has changed
/// </summary>
//public event ChangeHandler OnOrientationChanged = delegate { };
/// <summary>
/// Boolean indicating the thing has an updated orientation
/// </summary>
public bool orientationUpdated = false;
@ -303,7 +299,7 @@ namespace RoboidControl {
set {
if (_linearVelocity != value) {
_linearVelocity = value;
hasLinearVelocity = _linearVelocity.distance != 0;
linearVelocityUpdated = true;
OnLinearVelocityChanged?.Invoke(_linearVelocity);
}
}
@ -315,7 +311,7 @@ namespace RoboidControl {
/// <summary>
/// Boolean indicating the thing has an updated linear velocity
/// </summary>
public bool hasLinearVelocity = false;
public bool linearVelocityUpdated = false;
private Spherical _angularVelocity = Spherical.zero;
/// <summary>
@ -326,7 +322,7 @@ namespace RoboidControl {
set {
if (_angularVelocity != value) {
_angularVelocity = value;
hasAngularVelocity = _angularVelocity.distance != 0;
angularVelocityUpdated = true;
OnAngularVelocityChanged?.Invoke(_angularVelocity);
}
}
@ -338,7 +334,7 @@ namespace RoboidControl {
/// <summary>
/// Boolean indicating the thing has an updated angular velocity
/// </summary>
public bool hasAngularVelocity = false;
public bool angularVelocityUpdated = false;
#if UNITY_5_3_OR_NEWER
/// <summary>