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.linearVelocity = linearVelocity;
this.angularVelocity = angularVelocity; 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) /// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer)
public PoseMsg(byte[] buffer) : base(buffer) { public PoseMsg(byte[] buffer) : base(buffer) {
byte ix = 1; byte ix = 1;

10
src/Participant.cs
View File

@ -44,6 +44,16 @@ namespace RoboidControl {
/// </summary> /// </summary>
protected readonly List<Thing> things = new List<Thing>(); 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> /// <summary>
/// Get a thing with the given ids /// Get a thing with the given ids
/// </summary> /// </summary>

48
src/ParticipantUDP.cs
View File

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

18
src/SiteServer.cs
View File

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

14
src/Thing.cs
View File

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