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

NW PoC works

Browse Source
This commit is contained in:
Pascal Serrarens 2025-01-08 17:11:54 +01:00
parent 735ad3b6dc
commit dc165edf79
10 changed files with 166 additions and 127 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
CustomMsg.cs Normal file
View File

@ -0,0 +1,45 @@
namespace Passer.Control.Core {
public class CustomMsg : IMessage {
public const byte Id = 0xB1;
public byte networkId;
public byte thingId;
public byte[] bytes;
public CustomMsg(byte[] buffer) {
byte ix = 1;
this.networkId = buffer[ix++];
this.thingId = buffer[ix++];
byte length = (byte)(buffer.Length - ix);
this.bytes = new byte[length];
for (uint bytesIx = 0; bytesIx < length; bytesIx++)
this.bytes[bytesIx] = buffer[ix++];
}
public CustomMsg(byte networkId, byte thingId, byte[] bytes) : base() {
this.networkId = networkId;
this.thingId = thingId;
this.bytes = bytes;
}
public override byte Serialize(ref byte[] buffer) {
byte ix = 0;
buffer[ix++] = CustomMsg.Id;
buffer[ix++] = this.networkId;
buffer[ix++] = this.thingId;
//buffer[ix++] = (byte)bytes.Length;
foreach (byte b in bytes)
buffer[ix++] = b;
return ix;
}
//public static async Task<bool> Receive(Stream dataStream, Participant client, byte packetSize) {
// byte[] buffer = await Receive(dataStream, packetSize);
// CustomMsg msg = new(buffer);
// client.messageQueue.Enqueue(msg);
// return true;
//}
}
}

2
CustomMsg.cs.meta Normal file
View File

@ -0,0 +1,2 @@
fileFormatVersion: 2
guid: b9dcd611539afab429456b08500ae152

46
Messages.cs
View File

@ -197,52 +197,6 @@ namespace Passer.Control.Core {
#endregion Pose
#region Custom
public class CustomMsg : IMessage {
public const byte Id = 0xB1;
public byte networkId;
public byte thingId;
public byte[] bytes;
public CustomMsg(byte[] buffer) {
byte ix = 0;
this.networkId = buffer[ix++];
this.thingId = buffer[ix++];
byte length = (byte)(buffer.Length - ix);
this.bytes = new byte[length];
for (uint bytesIx = 0; bytesIx < length; bytesIx++)
this.bytes[bytesIx] = buffer[ix++];
}
public CustomMsg(byte networkId, byte thingId, byte[] bytes) : base() {
this.networkId = networkId;
this.thingId = thingId;
this.bytes = bytes;
}
public override byte Serialize(ref byte[] buffer) {
byte ix = 0;
buffer[ix++] = CustomMsg.Id;
buffer[ix++] = this.networkId;
buffer[ix++] = this.thingId;
//buffer[ix++] = (byte)bytes.Length;
foreach (byte b in bytes)
buffer[ix++] = b;
return ix;
}
public static async Task<bool> Receive(Stream dataStream, Participant client, byte packetSize) {
byte[] buffer = await Receive(dataStream, packetSize);
CustomMsg msg = new(buffer);
client.messageQueue.Enqueue(msg);
return true;
}
}
#endregion Custom
#region Text
public class TextMsg : IMessage {

12
Participant.cs
View File

@ -189,7 +189,7 @@ namespace Passer.Control.Core {
// result = await PoseMsg.Receive(dataStream, client, packetSize);
break;
case CustomMsg.Id: // 0xB1 / 177
// result = await CustomMsg.Receive(dataStream, client, packetSize);
this.Process(new CustomMsg(data));
break;
case TextMsg.Id: // 0xB0 / 176
// result = await TextMsg.Receive(dataStream, client, packetSize);
@ -224,7 +224,10 @@ namespace Passer.Control.Core {
}
protected virtual void Process(NameMsg msg) {
Console.WriteLine($"received name {msg.name}");
//Console.WriteLine($"received name {msg.name}");
Thing thing = Thing.Get(msg.networkId, msg.thingId);
if (thing != null)
thing.name = msg.name;
}
protected virtual void Process(ModelUrlMsg msg) {
@ -233,7 +236,10 @@ namespace Passer.Control.Core {
protected virtual void ProcessPoseMsg(PoseMsg msg) { }
protected virtual void ProcessCustomMsg(CustomMsg msg) { }
protected virtual void Process(CustomMsg msg) {
Thing thing = Thing.Get(msg.networkId, msg.thingId);
thing?.ProcessBytes(msg.bytes);
}
protected virtual void ProcessTextMsg(TextMsg temsgxt) { }

8
Sensors.meta Normal file
View File

@ -0,0 +1,8 @@
fileFormatVersion: 2
guid: a5a7a42365df0d0459195576ad3bb50b
folderAsset: yes
DefaultImporter:
externalObjects: {}
userData:
assetBundleName:
assetBundleVariant:

14
Sensors/TemperatureSensor.cs Normal file
View File

@ -0,0 +1,14 @@
namespace Passer.Control.Core {
public class TemperatureSensor : Thing {
public TemperatureSensor(byte networkId, byte thingId) : base(null, networkId, thingId, (byte)Type.TemperatureSensor) {
}
public override void ProcessBytes(byte[] bytes) {
byte ix = 0;
float temp = LowLevelMessages.ReceiveFloat16(bytes, ref ix);
UnityEngine.Debug.Log($"temperature {this.name} = {temp} C");
}
}
}

2
Sensors/TemperatureSensor.cs.meta Normal file
View File

@ -0,0 +1,2 @@
fileFormatVersion: 2
guid: 5cd9e2534695ec844bd23b7a48ad498a

22
SiteServer.cs
View File

@ -17,13 +17,6 @@ namespace Passer.Control.Core {
}
public override void Update(long currentTimeMs) {
// for (int ix = 0; ix < this.others.Count; ix++) {
// Participant client = this.others[ix];
// if (client == null)
// continue;
// this.ProcessMessages(client);
// }
Thing.UpdateAll(currentTimeMs);
}
@ -36,5 +29,20 @@ namespace Passer.Control.Core {
protected override void Process(Participant sender, NetworkIdMsg msg) {
}
protected override void Process(ThingMsg msg) {
Thing thing = Thing.Get(msg.networkId, msg.thingId);
if (thing == null) {
switch ((Thing.Type) msg.thingType) {
case Thing.Type.TemperatureSensor:
new TemperatureSensor(msg.networkId, msg.thingId);
break;
default:
new Thing(this, msg.networkId, msg.thingId, msg.thingType);
break;
}
}
}
}
}

138
Thing.cs
View File

@ -1,14 +1,36 @@
using System.Collections.Generic;
using Passer.LinearAlgebra;
namespace Passer.Control.Core
{
namespace Passer.Control.Core {
/// <summary>
/// A thing is the basic building block
/// </summary>
public class Thing
{
public class Thing {
#region Types
public enum Type {
Undeterment,
// Sensor
Switch,
DistanceSensor,
DirectionalSensor,
TemperatureSensor,
// Motor
ControlledMotor,
UncontrolledMotor,
Servo,
// Other
Roboid,
Humanoid,
ExternalSensor
};
#endregion Types
#region Properties
public Participant participant;
public delegate void ChangeHandler();
@ -19,36 +41,30 @@ namespace Passer.Control.Core
public event ChangeHandler OnParentChanged;
private Thing _parent;
public Thing parent
{
public Thing parent {
get => _parent;
set
{
set {
if (_parent == value)
return;
if (value == null)
{
if (value == null) {
_parent.RemoveChild(this);
_parent = null;
}
else
{
else {
value.AddChild(this);
OnParentChanged?.Invoke();
}
}
}
public void AddChild(Thing child)
{
public void AddChild(Thing child) {
if (children.Find(thing => thing == child) != null)
return;
child._parent = this;
children.Add(child);
}
public void RemoveChild(Thing child)
{
public void RemoveChild(Thing child) {
children.Remove(child);
}
@ -58,13 +74,10 @@ namespace Passer.Control.Core
public event ChangeHandler OnNameChanged;
private string _name;
public string name
{
public string name {
get => _name;
set
{
if (_name != value)
{
set {
if (_name != value) {
_name = value;
OnNameChanged?.Invoke();
}
@ -76,13 +89,10 @@ namespace Passer.Control.Core
public byte poseUpdated = 0x00;
public event ChangeHandler OnPositionChanged;
private Spherical _position;
public Spherical position
{
public Spherical position {
get { return _position; }
set
{
if (_position != value)
{
set {
if (_position != value) {
_position = value;
OnPositionChanged?.Invoke();
}
@ -91,13 +101,10 @@ namespace Passer.Control.Core
public event ChangeHandler OnOrientationChanged;
private SwingTwist _orientation;
public SwingTwist orientation
{
public SwingTwist orientation {
get { return _orientation; }
set
{
if (_orientation != value)
{
set {
if (_orientation != value) {
_orientation = value;
OnOrientationChanged?.Invoke();
}
@ -106,13 +113,10 @@ namespace Passer.Control.Core
public event SphericalHandler OnLinearVelocityChanged;
private Spherical _linearVelocity;
public Spherical linearVelocity
{
public Spherical linearVelocity {
get => _linearVelocity;
set
{
if (_linearVelocity != value)
{
set {
if (_linearVelocity != value) {
_linearVelocity = value;
OnLinearVelocityChanged?.Invoke(_linearVelocity);
}
@ -120,21 +124,21 @@ namespace Passer.Control.Core
}
public Spherical angularVelocity;
public virtual void Init(bool invokeEvent = true)
{
#endregion Properties
#region Init
public virtual void Init(bool invokeEvent = true) {
Thing.Add(this, invokeEvent);
}
public Thing(bool initialize = true)
{
if (initialize)
{
public Thing(bool initialize = true) {
if (initialize) {
//this.Init();
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) {
this.participant = client;
this.id = thingId;
this.type = thingType;
@ -143,17 +147,21 @@ namespace Passer.Control.Core
Thing.Add(this);
}
public virtual void Update(float currentTime)
{
#endregion Init
#region Update
public virtual void Update(float currentTime) {
// should recurse over children...
}
public virtual void ProcessBytes(byte[] bytes)
{
public virtual void ProcessBytes(byte[] bytes) {
//if (sensor != null)
// sensor.ProcessBytes(bytes);
}
#endregion Update
// Experimental
// public float stressLevel = 0;
@ -176,48 +184,40 @@ namespace Passer.Control.Core
public delegate void ThingHandler(Thing t);
public static event ThingHandler OnNewThing;
public static void Add(Thing thing, bool invokeEvent = true)
{
public static void Add(Thing thing, bool invokeEvent = true) {
Thing foundThing = Get(thing.networkId, thing.id);
if (foundThing == null)
{
if (foundThing == null) {
if (thing.id == 0)
thing.id = (byte)(allThings.Count + 1);
allThings.Add(thing);
if (invokeEvent)
OnNewThing?.Invoke(thing);
// UnityEngine.Debug.Log($"Add thing [{thing.networkId}/{thing.id}] {thing.name}");
UnityEngine.Debug.Log($"Add thing [{thing.networkId}/{thing.id}] {thing.name}");
}
}
public static Thing Get(byte networkId, byte thingId)
{
public static Thing Get(byte networkId, byte thingId) {
Thing thing = allThings.Find(aThing => IsThing(aThing, networkId, thingId));
return thing;
}
private static bool IsThing(Thing thing, byte networkId, byte thingId)
{
private static bool IsThing(Thing thing, byte networkId, byte thingId) {
if (thing == null)
return false;
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);
}
public static Thing[] GetAllThings()
{
public static Thing[] GetAllThings() {
return allThings.ToArray();
}
public static void UpdateAll(float currentTime)
{
foreach (Thing thing in allThings)
{
public static void UpdateAll(float currentTime) {
foreach (Thing thing in allThings) {
if (thing.parent == null) // update only root things
thing.Update(currentTime);
}

2
ThingMsg.cs
View File

@ -24,7 +24,7 @@ namespace Passer.Control.Core {
this.parentId = parentId;
}
public ThingMsg(byte[] buffer) {
uint ix = 0;
uint ix = 1;
this.networkId = buffer[ix++];
this.thingId = buffer[ix++];
this.thingType = buffer[ix++];