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

Simulation basics

Browse Source
This commit is contained in:
Pascal Serrarens 2025-02-05 09:34:54 +01:00
parent b1c324f36c
commit ac2785a440
5 changed files with 112 additions and 104 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

98
Participant.cs
View File

@ -25,7 +25,7 @@ namespace Passer.Control.Core {
/// Create a porticiapnt /// Create a porticiapnt
/// </summary> /// </summary>
public Participant() { public Participant() {
others.Add(this); senders.Add(this);
} }
/// <summary> /// <summary>
@ -62,19 +62,22 @@ namespace Passer.Control.Core {
this.port = port; this.port = port;
} }
public List<RemoteParticipant> others = new List<RemoteParticipant>(); public List<RemoteParticipant> senders = new();
public RemoteParticipant GetParticipant(string ipAddress, int port) { public RemoteParticipant GetParticipant(string ipAddress, int port) {
foreach (RemoteParticipant c in others) { //Console.WriteLine($"Get Participant {ipAddress}:{port}");
if (c.ipAddress == ipAddress && c.port == port) foreach (RemoteParticipant sender in senders) {
return c; if (sender.ipAddress == ipAddress && sender.port == port)
return sender;
} }
return null; return null;
} }
public RemoteParticipant AddParticipant(string ipAddress, int port) { public RemoteParticipant AddParticipant(string ipAddress, int port) {
RemoteParticipant participant = new(ipAddress, port); Console.WriteLine($"New Participant {ipAddress}:{port}");
participant.networkId = (byte)this.others.Count; RemoteParticipant participant = new(ipAddress, port) {
//others.Add(); networkId = (byte)this.senders.Count
};
senders.Add(participant);
return participant; return participant;
} }
@ -95,28 +98,6 @@ namespace Passer.Control.Core {
Console.WriteLine($"Registering {typeof(ThingClass)} for thing type {thingType}"); Console.WriteLine($"Registering {typeof(ThingClass)} for thing type {thingType}");
} }
private List<Thing> things;
public static event Thing.ThingHandler OnNewThing;
public Thing Get(byte networkId, byte thingId) {
Thing thing = things.Find(aThing => Thing.IsThing(aThing, networkId, thingId));
return thing;
}
public void Add(Thing thing, bool invokeEvent = true) {
Console.WriteLine("added thing");
Thing foundThing = Get(thing.networkId, thing.id);
if (foundThing == null) {
if (thing.id == 0)
thing.id = (byte)(things.Count + 1);
things.Add(thing);
if (invokeEvent)
OnNewThing?.Invoke(thing);
Console.Write($"Add thing [{thing.networkId}/{thing.id}] {thing.name}");
}
}
#endregion Init #endregion Init
#region Update #region Update
@ -139,28 +120,24 @@ namespace Passer.Control.Core {
private ulong nextPublishMe = 0; private ulong nextPublishMe = 0;
public virtual void Update(ulong currentTimeMS = 0) {
public virtual 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);
#endif #endif
} }
if (this.publishInterval > 0 && currentTimeMs > this.nextPublishMe) { if (this.publishInterval > 0 && currentTimeMS > this.nextPublishMe) {
this.Publish(new ClientMsg(this.networkId)); this.Publish(new ClientMsg(this.networkId));
// Console.WriteLine($"{this.name} Sent ClientMsg {this.networkId}"); // Console.WriteLine($"{this.name} Publish ClientMsg {this.networkId}");
this.nextPublishMe = currentTimeMs + this.publishInterval; this.nextPublishMe = currentTimeMS + this.publishInterval;
} }
// for (int ix = 0; ix < this.others.Count; ix++) { foreach (Thing thing in this.things) {
// Participant client = this.others[ix]; if (thing != null && thing.parent == null) // update only root things
// if (client == null) thing.Update(currentTimeMS);
// continue; }
// this.ProcessMessages(client);
// }
Thing.UpdateAll(currentTimeMs);
} }
#endregion Update #endregion Update
@ -229,7 +206,6 @@ namespace Passer.Control.Core {
public void ReceiveData(byte[] data, RemoteParticipant remoteParticipant) { public void ReceiveData(byte[] data, RemoteParticipant remoteParticipant) {
byte msgId = data[0]; byte msgId = data[0];
Console.Write($"received msg {msgId}");
if (msgId == 0xFF) { if (msgId == 0xFF) {
// Timeout // Timeout
return; return;
@ -246,19 +222,19 @@ namespace Passer.Control.Core {
// result = await InvestigateMsg.Receive(dataStream, client, packetSize); // result = await InvestigateMsg.Receive(dataStream, client, packetSize);
break; break;
case ThingMsg.id: // 0x80 / 128 case ThingMsg.id: // 0x80 / 128
this.Process(new ThingMsg(data)); this.Process(remoteParticipant, new ThingMsg(data));
break; break;
case NameMsg.Id: // 0x91 / 145 case NameMsg.Id: // 0x91 / 145
this.Process(new NameMsg(data)); this.Process(remoteParticipant, new NameMsg(data));
break; break;
case ModelUrlMsg.Id: // 0x90 / 144 case ModelUrlMsg.Id: // 0x90 / 144
this.Process(new ModelUrlMsg(data)); this.Process(remoteParticipant, new ModelUrlMsg(data));
break; break;
case PoseMsg.Id: // 0x10 / 16 case PoseMsg.Id: // 0x10 / 16
// result = await PoseMsg.Receive(dataStream, client, packetSize); // result = await PoseMsg.Receive(dataStream, client, packetSize);
break; break;
case CustomMsg.Id: // 0xB1 / 177 case CustomMsg.Id: // 0xB1 / 177
this.Process(new CustomMsg(data)); this.Process(remoteParticipant, new CustomMsg(data));
break; break;
case TextMsg.Id: // 0xB0 / 176 case TextMsg.Id: // 0xB0 / 176
// result = await TextMsg.Receive(dataStream, client, packetSize); // result = await TextMsg.Receive(dataStream, client, packetSize);
@ -281,33 +257,33 @@ namespace Passer.Control.Core {
Console.WriteLine($"{this.name} receive network id {this.networkId} {msg.networkId}"); Console.WriteLine($"{this.name} receive network id {this.networkId} {msg.networkId}");
if (this.networkId != msg.networkId) { if (this.networkId != msg.networkId) {
this.networkId = msg.networkId; this.networkId = msg.networkId;
foreach (Thing thing in Thing.GetAllThings()) foreach (Thing thing in this.things) //Thing.GetAllThings())
this.SendThingInfo(sender, thing); this.SendThingInfo(sender, thing);
} }
} }
protected virtual void Process(InvestigateMsg msg) { } protected virtual void Process(InvestigateMsg msg) { }
protected virtual void Process(ThingMsg msg) { protected virtual void Process(RemoteParticipant sender, ThingMsg msg) {
Console.WriteLine($"received thing {msg.thingId}"); Console.WriteLine($"Participant: Process thing [{msg.networkId}/{msg.thingId}]");
} }
protected virtual void Process(NameMsg msg) { protected virtual void Process(RemoteParticipant sender, NameMsg msg) {
Console.WriteLine($"received name {msg.name}"); Console.WriteLine($"Participant: Process name [{msg.networkId}/{msg.thingId}] {msg.name}");
Thing thing = Thing.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(ModelUrlMsg msg) { protected virtual void Process(RemoteParticipant sender, ModelUrlMsg msg) {
Console.WriteLine($"received name {msg.url}"); Console.WriteLine($"Participant: Process model [{msg.networkId}/{msg.thingId}] {msg.url}");
} }
protected virtual void Process(PoseMsg msg) { } protected virtual void Process(PoseMsg msg) { }
protected virtual void Process(CustomMsg msg) { protected virtual void Process(RemoteParticipant sender, CustomMsg msg) {
Console.WriteLine($"receive custom msg"); Console.WriteLine($"Participant: Process binary [{msg.networkId}/{msg.thingId}]");
Thing thing = Thing.Get(msg.networkId, msg.thingId); Thing thing = sender.Get(msg.networkId, msg.thingId);
thing?.ProcessBinary(msg.bytes); thing?.ProcessBinary(msg.bytes);
} }
@ -316,7 +292,7 @@ namespace Passer.Control.Core {
protected virtual void Process(DestroyMsg msg) { } protected virtual void Process(DestroyMsg msg) { }
private void ForwardMessage(IMessage msg) { private void ForwardMessage(IMessage msg) {
foreach (Participant client in others) { foreach (Participant client in senders) {
if (client == this) if (client == this)
continue; continue;
//UnityEngine.Debug.Log($"---> {client.ipAddress}"); //UnityEngine.Debug.Log($"---> {client.ipAddress}");

28
RemoteParticipant.cs
View File

@ -1,3 +1,6 @@
using System;
using System.Collections.Generic;
namespace Passer.Control.Core { namespace Passer.Control.Core {
public class RemoteParticipant { public class RemoteParticipant {
@ -13,6 +16,31 @@ namespace Passer.Control.Core {
this.ipAddress = ipAddress; this.ipAddress = ipAddress;
this.port = port; this.port = port;
} }
protected readonly List<Thing> things = new();
public Thing Get(byte networkId, byte 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;
}
public void Add(Thing thing, bool invokeEvent = true) {
//Console.WriteLine($"added thing [{thing.networkId}/{thing.id}]");
Thing foundThing = Get(thing.networkId, thing.id);
if (foundThing == null) {
// if (thing.id == 0)
// thing.id = (byte)(things.Count + 1);
things.Add(thing);
if (invokeEvent)
Thing.InvokeNewThing(thing);
Console.Write($"Add thing [{thing.networkId}/{thing.id}] {thing.name}");
}
}
} }
} }

4
Sensors/TemperatureSensor.cs
View File

@ -10,8 +10,8 @@ namespace Passer.Control.Core {
public override void ProcessBinary(byte[] bytes) { public override void ProcessBinary(byte[] bytes) {
byte ix = 0; byte ix = 0;
float temp = LowLevelMessages.ReceiveFloat16(bytes, ref ix); this.temp = LowLevelMessages.ReceiveFloat16(bytes, ref ix);
Console.WriteLine($"temperature {this.name} = {temp} C"); Console.WriteLine($"temperature {this.name} = {this.temp} C");
} }
} }

19
SiteServer.cs
View File

@ -18,11 +18,9 @@ namespace Passer.Control.Core {
Console.Write($"Prepare receive on port {port}"); Console.Write($"Prepare receive on port {port}");
this.udpClient = new UdpClient(port); // for receiving this.udpClient = new UdpClient(port); // for receiving
//this.udpClient.Client.Bind(new IPEndPoint(IPAddress.Any, port));
IPEndPoint receiveEndpoint = new(IPAddress.Any, port);
this.udpClient.BeginReceive( this.udpClient.BeginReceive(
new AsyncCallback(result => ReceiveUDP(result)), new AsyncCallback(result => ReceiveUDP(result)),
new Tuple<UdpClient, IPEndPoint>(this.udpClient, receiveEndpoint)); new Tuple<UdpClient, IPEndPoint>(this.udpClient, new(IPAddress.Any, port)));
} }
protected override void Process(RemoteParticipant sender, ClientMsg msg) { protected override void Process(RemoteParticipant sender, ClientMsg msg) {
@ -34,13 +32,16 @@ namespace Passer.Control.Core {
protected override void Process(RemoteParticipant sender, NetworkIdMsg msg) { } protected override void Process(RemoteParticipant sender, NetworkIdMsg msg) { }
protected override void Process(ThingMsg msg) { protected override void Process(RemoteParticipant sender, ThingMsg msg) {
Thing thing = Thing.Get(msg.networkId, msg.thingId); Console.WriteLine($"SiteServer: Process thing [{msg.networkId}/{msg.thingId}]");
Thing thing = sender.Get(msg.networkId, msg.thingId);
if (thing == null) { if (thing == null) {
Thing newThing;
if (thingMsgProcessors.ContainsKey(msg.thingType)) if (thingMsgProcessors.ContainsKey(msg.thingType))
thingMsgProcessors[msg.thingType](msg.networkId, msg.thingId); newThing = thingMsgProcessors[msg.thingType](msg.networkId, msg.thingId);
else else
new Thing(this, msg.networkId, msg.thingId, msg.thingType); newThing = new Thing(this, msg.networkId, msg.thingId, msg.thingType);
sender.Add(newThing);
} }
} }

67
Thing.cs
View File

@ -130,13 +130,13 @@ namespace Passer.Control.Core {
#region Init #region Init
public virtual void Init(bool invokeEvent = true) { public virtual void Init(bool invokeEvent = true) {
Thing.Add(this, invokeEvent); //Thing.Add(this, invokeEvent);
} }
public Thing(bool initialize = true) { public Thing(bool initialize = true) {
if (initialize) { if (initialize) {
//this.Init(); //this.Init();
Thing.Add(this); //Thing.Add(this);
} }
} }
public Thing(Participant client, byte networkId, byte thingId, byte thingType = 0) { public Thing(Participant client, byte networkId, byte thingId, byte thingType = 0) {
@ -145,7 +145,7 @@ namespace Passer.Control.Core {
this.type = thingType; this.type = thingType;
this.networkId = networkId; this.networkId = networkId;
this.Init(); this.Init();
Thing.Add(this); //Thing.Add(this);
} }
public virtual void CreateComponent() {} public virtual void CreateComponent() {}
@ -193,25 +193,28 @@ namespace Passer.Control.Core {
public delegate void ThingHandler(Thing t); public delegate void ThingHandler(Thing t);
public static event ThingHandler OnNewThing; public static event ThingHandler OnNewThing;
public static void InvokeNewThing(Thing thing) {
public static void Add(Thing thing, bool invokeEvent = true) { OnNewThing?.Invoke(thing);
Console.WriteLine("added thing");
Thing foundThing = Get(thing.networkId, thing.id);
if (foundThing == null) {
if (thing.id == 0)
thing.id = (byte)(allThings.Count + 1);
allThings.Add(thing);
if (invokeEvent)
OnNewThing?.Invoke(thing);
Console.Write($"Add thing [{thing.networkId}/{thing.id}] {thing.name}");
}
} }
public static Thing Get(byte networkId, byte thingId) { // public static void Add(Thing thing, bool invokeEvent = true) {
Thing thing = allThings.Find(aThing => IsThing(aThing, networkId, thingId)); // Console.WriteLine("added thing");
return thing; // Thing foundThing = Get(thing.networkId, thing.id);
}
// if (foundThing == null) {
// if (thing.id == 0)
// thing.id = (byte)(allThings.Count + 1);
// allThings.Add(thing);
// if (invokeEvent)
// OnNewThing?.Invoke(thing);
// Console.Write($"Add thing [{thing.networkId}/{thing.id}] {thing.name}");
// }
// }
// public static Thing Get(byte networkId, byte thingId) {
// Thing thing = allThings.Find(aThing => IsThing(aThing, networkId, thingId));
// return thing;
// }
public static bool IsThing(Thing thing, byte networkId, byte thingId) { public static bool IsThing(Thing thing, byte networkId, byte thingId) {
if (thing == null) if (thing == null)
@ -219,20 +222,20 @@ namespace Passer.Control.Core {
return (thing.networkId == networkId) && (thing.id == thingId); return (thing.networkId == networkId) && (thing.id == thingId);
} }
public static void Remove(byte networkId, byte thingId) { // public static void Remove(byte networkId, byte thingId) {
allThings.RemoveAll(t => t.networkId == networkId && t.id == thingId); // allThings.RemoveAll(t => t.networkId == networkId && t.id == thingId);
} // }
public static Thing[] GetAllThings() { // public static Thing[] GetAllThings() {
return allThings.ToArray(); // return allThings.ToArray();
} // }
public static void UpdateAll(ulong currentTimeMS) { // public static void UpdateAll(ulong currentTimeMS) {
foreach (Thing thing in allThings) { // foreach (Thing thing in allThings) {
if (thing.parent == null) // update only root things // if (thing.parent == null) // update only root things
thing.Update(currentTimeMS); // thing.Update(currentTimeMS);
} // }
} // }
} }
} }