From e218e0ea515b7499b608241a2e70f5d267fa50a9 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Mon, 20 Jan 2025 19:26:30 +0100 Subject: [PATCH 1/4] Event handling --- .gitattributes | 1 + .gitignore | 5 ++- BinaryMsg.py | 27 ++++++++++++++++ ClientMsg.py | 2 +- LowLevelMessages.py | 5 ++- Messages.py | 29 ++--------------- ModelUrlMsg.py | 2 +- NameMsg.py | 2 +- NetworkIdMsg.py | 2 +- Participant.py | 60 ++++++++++++++++++++++++++++-------- Sensors/TemperatureSensor.py | 19 ++++++++++++ SiteServer.py | 8 ++--- Spherical.py | 2 +- SwingTwist.py | 4 +-- Thing.py | 18 ++++++----- ThingMsg.py | 2 +- 16 files changed, 127 insertions(+), 61 deletions(-) create mode 100644 .gitattributes create mode 100644 BinaryMsg.py create mode 100644 Sensors/TemperatureSensor.py diff --git a/.gitattributes b/.gitattributes new file mode 100644 index 0000000..2125666 --- /dev/null +++ b/.gitattributes @@ -0,0 +1 @@ +* text=auto \ No newline at end of file diff --git a/.gitignore b/.gitignore index e5ddc82..d31799e 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,5 @@ __pycache__/* -test/__pycache__/* \ No newline at end of file +test/__pycache__/* +.vscode/* +DoxyGen/DoxyWarnLogfile.txt +**/__pycache__ \ No newline at end of file diff --git a/BinaryMsg.py b/BinaryMsg.py new file mode 100644 index 0000000..ad9248d --- /dev/null +++ b/BinaryMsg.py @@ -0,0 +1,27 @@ +from .Messages import IMessage +from .Thing import Thing + +class BinaryMsg(IMessage): + id = 0xB1 + + def __init__(self, buffer): + self.network_id = buffer[1] + self.thing_id = buffer[2] + self.thing: Thing = Thing.Get(self.network_id, self.thing_id) + self.data = buffer[3:] + + def SendTo(participant, thing, data: bytearray): + length = 3 + + if thing.network_id is None or thing is None or data is None: + return False + + participant.buffer[0:length] = [ + BinaryMsg.id, + participant.network_id, + thing.id + ] + full_length = length + len(data) + participant.buffer[length:full_length] = data + participant.SendBuffer(full_length) + return True \ No newline at end of file diff --git a/ClientMsg.py b/ClientMsg.py index a431436..0b51580 100644 --- a/ClientMsg.py +++ b/ClientMsg.py @@ -1,4 +1,4 @@ -from Messages import IMessage +from .Messages import IMessage ## A client message announces the presence of a participant # diff --git a/LowLevelMessages.py b/LowLevelMessages.py index 5545141..114107d 100644 --- a/LowLevelMessages.py +++ b/LowLevelMessages.py @@ -1,5 +1,5 @@ import numpy as np -from SwingTwist import SwingTwist +from .SwingTwist import SwingTwist def SendAngle8(buffer, ix_ref, angle): # Normalize angle @@ -24,7 +24,10 @@ def SendFloat16(buffer, ix_ref, value): def ReceiveFloat16(buffer, ix_ref) -> float: ix = ix_ref[0] + # if ix < len(buffer) - 1: binary = (buffer[ix] << 8) + buffer[ix+1] + # else: + # binary = 0 value16 = np.uint16(binary).view(np.float16) ix_ref[0] += 2 return float(value16) diff --git a/Messages.py b/Messages.py index 4aff96f..5206a60 100644 --- a/Messages.py +++ b/Messages.py @@ -1,5 +1,5 @@ -import LowLevelMessages -from Thing import Thing +from . import LowLevelMessages +from .Thing import Thing class IMessage: id = 0x00 @@ -59,28 +59,3 @@ class PoseMsg(IMessage): if self.thing.pose_updated & Thing.AngularVelocity: LowLevelMessages.SendSpherical(buffer, ix, self.thing.angularVelocity) return ix[0] - -class BinaryMsg(): - id = 0xB1 - - def __init__(self, buffer): - self.network_id = buffer[1] - self.thing_id = buffer[2] - self.thing: Thing = Thing.Get(self.network_id, self.thing_id) - self.data = buffer[3:] - - def SendTo(participant, thing, data: bytearray): - length = 3 - - if thing.network_id is None or thing is None or data is None: - return False - - participant.buffer[0:length] = [ - BinaryMsg.id, - participant.network_id, - thing.id - ] - full_length = length + len(data) - participant.buffer[length:full_length] = data - participant.SendBuffer(full_length) - return True \ No newline at end of file diff --git a/ModelUrlMsg.py b/ModelUrlMsg.py index bd670e2..349729a 100644 --- a/ModelUrlMsg.py +++ b/ModelUrlMsg.py @@ -1,4 +1,4 @@ -from Messages import IMessage +from .Messages import IMessage class ModelUrlMsg(IMessage): id = 0x90 diff --git a/NameMsg.py b/NameMsg.py index 5acb8bd..4908748 100644 --- a/NameMsg.py +++ b/NameMsg.py @@ -1,4 +1,4 @@ -from Messages import IMessage +from .Messages import IMessage class NameMsg(IMessage): id = 0x91 diff --git a/NetworkIdMsg.py b/NetworkIdMsg.py index 280fae5..524baf6 100644 --- a/NetworkIdMsg.py +++ b/NetworkIdMsg.py @@ -1,4 +1,4 @@ -from Messages import IMessage +from .Messages import IMessage ## A network id message invites another participant to a site # diff --git a/Participant.py b/Participant.py index 9d1fc3a..5261c7d 100644 --- a/Participant.py +++ b/Participant.py @@ -1,12 +1,14 @@ import socket import threading +import time -from ClientMsg import ClientMsg -from NetworkIdMsg import NetworkIdMsg -from ThingMsg import ThingMsg -from NameMsg import NameMsg -from ModelUrlMsg import ModelUrlMsg -from Thing import Thing +from .ClientMsg import ClientMsg +from .NetworkIdMsg import NetworkIdMsg +from .ThingMsg import ThingMsg +from .NameMsg import NameMsg +from .ModelUrlMsg import ModelUrlMsg +from .BinaryMsg import BinaryMsg +from .Thing import Thing ## A participant is device which can communicate with other participants # @@ -27,11 +29,13 @@ class Participant: self.others = [] self.network_id = 0 self.buffer = bytearray(256) + self.thing_msg_processors = {} + self.new_thing_handlers = [] if remote == False: self.udp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) self.udp_socket.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1) - self.udp_socket.bind(("0.0.0.0", 0)) + self.udp_socket.bind(("0.0.0.0", port)) self.AddParticipant(self.ip_address, self.port) self.thread = threading.Thread(target = self.Receiver) @@ -59,7 +63,10 @@ class Participant: self.others.append(remote_participant) return remote_participant - def Update(self, currentTimeMs): + def Update(self, currentTimeMs = None): + if currentTimeMs is None: + currentTimeMs = time.time() * 1000 + if self.publishInterval > 0 and currentTimeMs > self.nextPublishMe: self.Publish(ClientMsg(self.network_id)) print(f'Publish ClientMsg {self.network_id}') @@ -126,9 +133,11 @@ class Participant: self.ProcessNameMsg(NameMsg(data)) case ModelUrlMsg.id: self.ProcessModelUrlMsg(ModelUrlMsg(data)) - # case Messages.BinaryMsg.id: - # msg = Messages.BinaryMsg(data) - # msg.thing.ProcessBinary(msg.data) + case BinaryMsg.id: + self.ProcessBinary(BinaryMsg(data)) + # msg = BinaryMsg(data) + # if msg.thing != None: + # msg.thing.ProcessBinary(msg.data) def ProcessClientMsg(self, sender, msg: ClientMsg): pass @@ -146,7 +155,16 @@ class Participant: pass def ProcessThingMsg(self, msg: ThingMsg): - print(f'received thing {msg.thing_id}') + print(f'received thing {msg.network_id} {msg.thing_id}') + if msg.thing_type in self.thing_msg_processors: + constructor = self.thing_msg_processors[msg.thing_type] + constructor(msg.network_id, msg.thing_id) + + # not really 'new' thing, but it is a start + thing = Thing.Get(msg.network_id, msg.thing_id) + if thing is not None: + for handler in self.new_thing_handlers: + handler(thing) def ProcessNameMsg(self, msg: NameMsg): print(f'received name {msg.name}') @@ -154,4 +172,22 @@ class Participant: def ProcessModelUrlMsg(self, msg: ModelUrlMsg): print(f'received model url: {msg.url}') + def ProcessBinary(self, msg: BinaryMsg): + print('received binary data') + if msg.thing != None: + msg.thing.ProcessBinary(msg.data) + + + def Register(self, constructor, thing_type): + self.thing_msg_processors[thing_type] = constructor + + def OnNewThing(self, event_handler): + self.new_thing_handlers.append(event_handler) + + def OnNewThingType(self, thing_type, event_handler): + def ConditionalHandler(thing): + if thing.type == thing_type: + event_handler(thing) + self.new_thing_handlers.append(ConditionalHandler) + #endregion \ No newline at end of file diff --git a/Sensors/TemperatureSensor.py b/Sensors/TemperatureSensor.py new file mode 100644 index 0000000..ef34910 --- /dev/null +++ b/Sensors/TemperatureSensor.py @@ -0,0 +1,19 @@ +from ..Thing import Thing +from .. import LowLevelMessages + +class TemperatureSensor(Thing): + def __init__(self, network_id, thing_id): + super().__init__(network_id, thing_id, type = Thing.Type.TemperatureSensor) + self.temp = 0 + self._watchers = [] + + def OnUpdate(self, handler): + self._watchers.append(handler) + def CancelOnUpdate(self, handler): + self._watchers.remove(handler) + + def ProcessBinary(self, data): + ix = 0 + self.temp = LowLevelMessages.ReceiveFloat16(data, [ix]) + for watcher in self._watchers: + watcher(self.temp) diff --git a/SiteServer.py b/SiteServer.py index 3635347..861ee95 100644 --- a/SiteServer.py +++ b/SiteServer.py @@ -1,6 +1,6 @@ -from Participant import Participant -from ClientMsg import ClientMsg -from NetworkIdMsg import NetworkIdMsg +from .Participant import Participant +from .ClientMsg import ClientMsg +from .NetworkIdMsg import NetworkIdMsg import socket import threading @@ -19,7 +19,7 @@ class SiteServer(Participant): if remote == False: self.udp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) self.udp_socket.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1) - self.udp_socket.bind(("0.0.0.0", 7681)) + self.udp_socket.bind(("0.0.0.0", port)) self.AddParticipant(self.ip_address, self.port) self.thread = threading.Thread(target = self.Receiver) diff --git a/Spherical.py b/Spherical.py index 75fe54a..6701afd 100644 --- a/Spherical.py +++ b/Spherical.py @@ -1,5 +1,5 @@ import math -from Direction import Direction +from .Direction import Direction class Spherical: def __init__(self, distance, direction): diff --git a/SwingTwist.py b/SwingTwist.py index 5e0c893..e312b15 100644 --- a/SwingTwist.py +++ b/SwingTwist.py @@ -1,5 +1,5 @@ -from Direction import Direction -from Quaternion import Quaternion +from .Direction import Direction +from .Quaternion import Quaternion class SwingTwist: def __init__(self, swing: Direction, twist: float): diff --git a/Thing.py b/Thing.py index 69a0a51..a01b6ce 100644 --- a/Thing.py +++ b/Thing.py @@ -1,5 +1,5 @@ -from Spherical import Spherical -from Quaternion import Quaternion +from .Spherical import Spherical +from .Quaternion import Quaternion ## A thing is the basic building block # @@ -10,6 +10,7 @@ class Thing: Switch = 0x01 DistanceSensor = 0x02 DirectionalSensor = 0x03 + TemperatureSensor = 0x04 Animator = 0x40 Position = 0x01 @@ -17,9 +18,9 @@ class Thing: LinearVelocity = 0x04 AngularVelocity = 0x08 - def __init__(self, type=Type.Undetermined, parent=None, name=None): - self.networkId = 0 - self.id = 0 + def __init__(self, network_id = 0, thing_id = 0, type=Type.Undetermined, parent=None, name=None): + self.network_id = network_id + self.id = thing_id self.type = type if parent is None: self.parent_id = 0 @@ -42,6 +43,7 @@ class Thing: pass def ProcessBinary(self, data): + print('default binary processor') pass allThings = set({ None }) @@ -52,16 +54,16 @@ class Thing: Thing.allThings.add(thing) @staticmethod - def Get(networkId, thingId): + def Get(network_id, thing_id): for thing in Thing.allThings: if thing is not None: - if thing.networkId == networkId and thing.id == thingId: + if thing.network_id == network_id and thing.id == thing_id: return thing return None ## Update all things @staticmethod def UpdateAll(currentTime): - for thing in Thing.allThings: + for thing in list(Thing.allThings): if thing is not None: thing.update(currentTime) diff --git a/ThingMsg.py b/ThingMsg.py index b97f597..eb72008 100644 --- a/ThingMsg.py +++ b/ThingMsg.py @@ -1,4 +1,4 @@ -from Messages import IMessage +from .Messages import IMessage class ThingMsg(IMessage): id = 0x80 From fa2895dd814ab34e78e7e5b91c46319ca659748a Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Mon, 20 Jan 2025 20:09:53 +0100 Subject: [PATCH 2/4] Refactoring --- Messages.py | 76 +++++-------------- BinaryMsg.py => Messages/BinaryMsg.py | 6 +- ClientMsg.py => Messages/ClientMsg.py | 4 +- Messages/InvestigateMsg.py | 8 ++ .../LowLevelMessages.py | 2 +- ModelUrlMsg.py => Messages/ModelUrlMsg.py | 4 +- NameMsg.py => Messages/NameMsg.py | 4 +- NetworkIdMsg.py => Messages/NetworkIdMsg.py | 4 +- Messages/PoseMsg.py | 30 ++++++++ ThingMsg.py => Messages/ThingMsg.py | 4 +- Messages/__init__.py | 17 +++++ Participant.py | 7 +- Sensors/TemperatureSensor.py | 2 +- SiteServer.py | 3 +- 14 files changed, 91 insertions(+), 80 deletions(-) rename BinaryMsg.py => Messages/BinaryMsg.py (89%) rename ClientMsg.py => Messages/ClientMsg.py (97%) create mode 100644 Messages/InvestigateMsg.py rename LowLevelMessages.py => Messages/LowLevelMessages.py (97%) rename ModelUrlMsg.py => Messages/ModelUrlMsg.py (95%) rename NameMsg.py => Messages/NameMsg.py (95%) rename NetworkIdMsg.py => Messages/NetworkIdMsg.py (95%) create mode 100644 Messages/PoseMsg.py rename ThingMsg.py => Messages/ThingMsg.py (94%) create mode 100644 Messages/__init__.py diff --git a/Messages.py b/Messages.py index 5206a60..9bf0f93 100644 --- a/Messages.py +++ b/Messages.py @@ -1,61 +1,23 @@ -from . import LowLevelMessages -from .Thing import Thing +# from . import LowLevelMessages +# from .Thing import Thing -class IMessage: - id = 0x00 +# class IMessage: +# id = 0x00 - ## Serialize the message into the given buffer - # - ## @returns: the length of the message - def Serialize(buffer): - return 0 +# ## Serialize the message into the given buffer +# # +# ## @returns: the length of the message +# def Serialize(buffer): +# return 0 - def SendTo(self, participant): - buffer_size = self.Serialize([participant.buffer]) - if buffer_size == 0: - return False - return participant.SendBuffer(buffer_size) +# def SendTo(self, participant): +# buffer_size = self.Serialize([participant.buffer]) +# if buffer_size == 0: +# return False +# return participant.SendBuffer(buffer_size) - def Publish(self, participant): - bufferSize = self.Serialize([participant.buffer]) - if bufferSize == 0: - return False - return participant.PublishBuffer(bufferSize) - -class InvestigateMsg(): - id = 0x81 - length = 3 - - def __init__(self, buffer): - self.network_id = buffer[1] - self.thing_id = buffer[2] - -class PoseMsg(IMessage): - id = 0x10 - length = 4 - - def __init__(self, network_id, thing): - self.network_id = network_id - self.thing = thing - - def Serialize(self, buffer_ref): - if (self.network_id is None) or (self.thing is None): - return 0 - - buffer: bytearray = buffer_ref[0] - buffer[0:PoseMsg.length] = [ - PoseMsg.id, - self.network_id, - self.thing.id, - self.thing.pose_updated - ] - ix = [4] - if self.thing.pose_updated & Thing.Position: - LowLevelMessages.SendSpherical(buffer, ix, self.thing.position) - if self.thing.pose_updated & Thing.Orientation: - LowLevelMessages.SendQuat32(buffer, ix, self.thing.orientation) - if self.thing.pose_updated & Thing.LinearVelocity: - LowLevelMessages.SendSpherical(buffer, ix, self.thing.linearVelocity) - if self.thing.pose_updated & Thing.AngularVelocity: - LowLevelMessages.SendSpherical(buffer, ix, self.thing.angularVelocity) - return ix[0] +# def Publish(self, participant): +# bufferSize = self.Serialize([participant.buffer]) +# if bufferSize == 0: +# return False +# return participant.PublishBuffer(bufferSize) diff --git a/BinaryMsg.py b/Messages/BinaryMsg.py similarity index 89% rename from BinaryMsg.py rename to Messages/BinaryMsg.py index ad9248d..e7b6d24 100644 --- a/BinaryMsg.py +++ b/Messages/BinaryMsg.py @@ -1,7 +1,7 @@ -from .Messages import IMessage -from .Thing import Thing +# from ..Messages import IMessage +from ..Thing import Thing -class BinaryMsg(IMessage): +class BinaryMsg(): id = 0xB1 def __init__(self, buffer): diff --git a/ClientMsg.py b/Messages/ClientMsg.py similarity index 97% rename from ClientMsg.py rename to Messages/ClientMsg.py index 0b51580..ca19a39 100644 --- a/ClientMsg.py +++ b/Messages/ClientMsg.py @@ -1,10 +1,10 @@ -from .Messages import IMessage +# from . import IMessage ## A client message announces the presence of a participant # ## When received by another participant, it can be followed by a NetworkIdMsg ## to announce that participant to this client such that it can join privately -class ClientMsg(IMessage): +class ClientMsg(): id = 0xA0 length = 2 diff --git a/Messages/InvestigateMsg.py b/Messages/InvestigateMsg.py new file mode 100644 index 0000000..bd7bfd0 --- /dev/null +++ b/Messages/InvestigateMsg.py @@ -0,0 +1,8 @@ + +class InvestigateMsg(): + id = 0x81 + length = 3 + + def __init__(self, buffer): + self.network_id = buffer[1] + self.thing_id = buffer[2] diff --git a/LowLevelMessages.py b/Messages/LowLevelMessages.py similarity index 97% rename from LowLevelMessages.py rename to Messages/LowLevelMessages.py index 114107d..b02616c 100644 --- a/LowLevelMessages.py +++ b/Messages/LowLevelMessages.py @@ -1,5 +1,5 @@ import numpy as np -from .SwingTwist import SwingTwist +from ..SwingTwist import SwingTwist def SendAngle8(buffer, ix_ref, angle): # Normalize angle diff --git a/ModelUrlMsg.py b/Messages/ModelUrlMsg.py similarity index 95% rename from ModelUrlMsg.py rename to Messages/ModelUrlMsg.py index 349729a..ba7648c 100644 --- a/ModelUrlMsg.py +++ b/Messages/ModelUrlMsg.py @@ -1,6 +1,6 @@ -from .Messages import IMessage +# from . import IMessage -class ModelUrlMsg(IMessage): +class ModelUrlMsg(): id = 0x90 length = 4 diff --git a/NameMsg.py b/Messages/NameMsg.py similarity index 95% rename from NameMsg.py rename to Messages/NameMsg.py index 4908748..db2ebb9 100644 --- a/NameMsg.py +++ b/Messages/NameMsg.py @@ -1,6 +1,6 @@ -from .Messages import IMessage +# from . import IMessage -class NameMsg(IMessage): +class NameMsg(): id = 0x91 length = 4 diff --git a/NetworkIdMsg.py b/Messages/NetworkIdMsg.py similarity index 95% rename from NetworkIdMsg.py rename to Messages/NetworkIdMsg.py index 524baf6..f1549dc 100644 --- a/NetworkIdMsg.py +++ b/Messages/NetworkIdMsg.py @@ -1,9 +1,9 @@ -from .Messages import IMessage +# from . import IMessage ## A network id message invites another participant to a site # ## This can be sent in response to a ClientMsg -class NetworkIdMsg(IMessage): +class NetworkIdMsg(): id = 0xA1 length = 2 diff --git a/Messages/PoseMsg.py b/Messages/PoseMsg.py new file mode 100644 index 0000000..a23ec51 --- /dev/null +++ b/Messages/PoseMsg.py @@ -0,0 +1,30 @@ + +class PoseMsg(): + id = 0x10 + length = 4 + + def __init__(self, network_id, thing): + self.network_id = network_id + self.thing = thing + + def Serialize(self, buffer_ref): + if (self.network_id is None) or (self.thing is None): + return 0 + + buffer: bytearray = buffer_ref[0] + buffer[0:PoseMsg.length] = [ + PoseMsg.id, + self.network_id, + self.thing.id, + self.thing.pose_updated + ] + ix = [4] + if self.thing.pose_updated & Thing.Position: + LowLevelMessages.SendSpherical(buffer, ix, self.thing.position) + if self.thing.pose_updated & Thing.Orientation: + LowLevelMessages.SendQuat32(buffer, ix, self.thing.orientation) + if self.thing.pose_updated & Thing.LinearVelocity: + LowLevelMessages.SendSpherical(buffer, ix, self.thing.linearVelocity) + if self.thing.pose_updated & Thing.AngularVelocity: + LowLevelMessages.SendSpherical(buffer, ix, self.thing.angularVelocity) + return ix[0] diff --git a/ThingMsg.py b/Messages/ThingMsg.py similarity index 94% rename from ThingMsg.py rename to Messages/ThingMsg.py index eb72008..34e1160 100644 --- a/ThingMsg.py +++ b/Messages/ThingMsg.py @@ -1,6 +1,6 @@ -from .Messages import IMessage +# from . import IMessage -class ThingMsg(IMessage): +class ThingMsg(): id = 0x80 length = 5 diff --git a/Messages/__init__.py b/Messages/__init__.py new file mode 100644 index 0000000..b809675 --- /dev/null +++ b/Messages/__init__.py @@ -0,0 +1,17 @@ +__all__ = ['BinaryMsg', + 'ClientMsg', + 'NetworkIdMsg', + 'InvestigateMsg', + 'ThingMsg', + 'NameMsg', + 'ModelUrlMsg', + 'PoseMsg'] + +from .BinaryMsg import BinaryMsg +from .ClientMsg import ClientMsg +from .InvestigateMsg import InvestigateMsg +from .ThingMsg import ThingMsg +from .NetworkIdMsg import NetworkIdMsg +from .NameMsg import NameMsg +from .ModelUrlMsg import ModelUrlMsg +from .PoseMsg import PoseMsg \ No newline at end of file diff --git a/Participant.py b/Participant.py index 5261c7d..960d2eb 100644 --- a/Participant.py +++ b/Participant.py @@ -2,12 +2,7 @@ import socket import threading import time -from .ClientMsg import ClientMsg -from .NetworkIdMsg import NetworkIdMsg -from .ThingMsg import ThingMsg -from .NameMsg import NameMsg -from .ModelUrlMsg import ModelUrlMsg -from .BinaryMsg import BinaryMsg +from .Messages import * from .Thing import Thing ## A participant is device which can communicate with other participants diff --git a/Sensors/TemperatureSensor.py b/Sensors/TemperatureSensor.py index ef34910..12d13bc 100644 --- a/Sensors/TemperatureSensor.py +++ b/Sensors/TemperatureSensor.py @@ -1,5 +1,5 @@ from ..Thing import Thing -from .. import LowLevelMessages +from ..Messages import LowLevelMessages class TemperatureSensor(Thing): def __init__(self, network_id, thing_id): diff --git a/SiteServer.py b/SiteServer.py index 861ee95..5d67ddc 100644 --- a/SiteServer.py +++ b/SiteServer.py @@ -1,6 +1,5 @@ from .Participant import Participant -from .ClientMsg import ClientMsg -from .NetworkIdMsg import NetworkIdMsg +from .Messages import * import socket import threading From eaf4fec9e0375405dcdfd99f4b16e62bcdabe8ac Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Mon, 20 Jan 2025 20:15:58 +0100 Subject: [PATCH 3/4] Refactoring --- Direction.py => LinearAlgebra/Direction.py | 0 Quaternion.py => LinearAlgebra/Quaternion.py | 0 Spherical.py => LinearAlgebra/Spherical.py | 0 SwingTwist.py => LinearAlgebra/SwingTwist.py | 0 Messages.py | 23 -------------------- Messages/LowLevelMessages.py | 2 +- Thing.py | 4 ++-- __init__.py | 11 ++-------- 8 files changed, 5 insertions(+), 35 deletions(-) rename Direction.py => LinearAlgebra/Direction.py (100%) rename Quaternion.py => LinearAlgebra/Quaternion.py (100%) rename Spherical.py => LinearAlgebra/Spherical.py (100%) rename SwingTwist.py => LinearAlgebra/SwingTwist.py (100%) delete mode 100644 Messages.py diff --git a/Direction.py b/LinearAlgebra/Direction.py similarity index 100% rename from Direction.py rename to LinearAlgebra/Direction.py diff --git a/Quaternion.py b/LinearAlgebra/Quaternion.py similarity index 100% rename from Quaternion.py rename to LinearAlgebra/Quaternion.py diff --git a/Spherical.py b/LinearAlgebra/Spherical.py similarity index 100% rename from Spherical.py rename to LinearAlgebra/Spherical.py diff --git a/SwingTwist.py b/LinearAlgebra/SwingTwist.py similarity index 100% rename from SwingTwist.py rename to LinearAlgebra/SwingTwist.py diff --git a/Messages.py b/Messages.py deleted file mode 100644 index 9bf0f93..0000000 --- a/Messages.py +++ /dev/null @@ -1,23 +0,0 @@ -# from . import LowLevelMessages -# from .Thing import Thing - -# class IMessage: -# id = 0x00 - -# ## Serialize the message into the given buffer -# # -# ## @returns: the length of the message -# def Serialize(buffer): -# return 0 - -# def SendTo(self, participant): -# buffer_size = self.Serialize([participant.buffer]) -# if buffer_size == 0: -# return False -# return participant.SendBuffer(buffer_size) - -# def Publish(self, participant): -# bufferSize = self.Serialize([participant.buffer]) -# if bufferSize == 0: -# return False -# return participant.PublishBuffer(bufferSize) diff --git a/Messages/LowLevelMessages.py b/Messages/LowLevelMessages.py index b02616c..eb6a560 100644 --- a/Messages/LowLevelMessages.py +++ b/Messages/LowLevelMessages.py @@ -1,5 +1,5 @@ import numpy as np -from ..SwingTwist import SwingTwist +from ..LinearAlgebra.SwingTwist import SwingTwist def SendAngle8(buffer, ix_ref, angle): # Normalize angle diff --git a/Thing.py b/Thing.py index a01b6ce..b719af7 100644 --- a/Thing.py +++ b/Thing.py @@ -1,5 +1,5 @@ -from .Spherical import Spherical -from .Quaternion import Quaternion +from .LinearAlgebra.Spherical import Spherical +from .LinearAlgebra.Quaternion import Quaternion ## A thing is the basic building block # diff --git a/__init__.py b/__init__.py index b2489e2..0584e21 100644 --- a/__init__.py +++ b/__init__.py @@ -1,14 +1,7 @@ -__all__ = ['Direction', - 'Spherical', - 'Thing', +__all__ = ['Thing', 'Participant', - 'Messages', - 'SiteServer', - 'SwingTwist'] + 'SiteServer'] -from .Direction import Direction from .Participant import Participant from .Thing import Thing -from .Spherical import Spherical -from .SwingTwist import SwingTwist from .SiteServer import SiteServer \ No newline at end of file From d4d67e8233c8ee34d7c6da52189ffb0857b4e6ee Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Tue, 21 Jan 2025 11:05:19 +0100 Subject: [PATCH 4/4] Merged changes --- Messages.py | 87 ++++++++++++++++++++++++++++++++++++ Messages/BinaryMsg.py | 3 +- Messages/LowLevelMessages.py | 2 +- Participant.py | 13 +++++- Sensors/TemperatureSensor.py | 4 +- SiteServer.py | 29 ++++++++++-- Thing.py | 4 +- test/thing_test.py | 8 +++- 8 files changed, 136 insertions(+), 14 deletions(-) create mode 100644 Messages.py diff --git a/Messages.py b/Messages.py new file mode 100644 index 0000000..48bf882 --- /dev/null +++ b/Messages.py @@ -0,0 +1,87 @@ +#import Messages.LowLevelMessages as LowLevelMessages +import Messages +from Thing import Thing + +class IMessage: + id = 0x00 + + ## Serialize the message into the given buffer + # + ## @returns: the length of the message + def Serialize(buffer): + return 0 + + def SendTo(self, participant): + buffer_size = self.Serialize([participant.buffer]) + if buffer_size == 0: + return False + return participant.SendBuffer(buffer_size) + + def Publish(self, participant): + bufferSize = self.Serialize([participant.buffer]) + if bufferSize == 0: + return False + return participant.PublishBuffer(bufferSize) + +class InvestigateMsg(): + id = 0x81 + length = 3 + + def __init__(self, buffer): + self.network_id = buffer[1] + self.thing_id = buffer[2] + +class PoseMsg(IMessage): + id = 0x10 + length = 4 + + def __init__(self, network_id, thing): + self.network_id = network_id + self.thing = thing + + def Serialize(self, buffer_ref): + if (self.network_id is None) or (self.thing is None): + return 0 + + buffer: bytearray = buffer_ref[0] + buffer[0:PoseMsg.length] = [ + PoseMsg.id, + self.network_id, + self.thing.id, + self.thing.pose_updated + ] + ix = [4] + if self.thing.pose_updated & Thing.Position: + Messages.LowLevelMessages.SendSpherical(buffer, ix, self.thing.position) + if self.thing.pose_updated & Thing.Orientation: + Messages.LowLevelMessages.SendQuat32(buffer, ix, self.thing.orientation) + if self.thing.pose_updated & Thing.LinearVelocity: + Messages.LowLevelMessages.SendSpherical(buffer, ix, self.thing.linearVelocity) + if self.thing.pose_updated & Thing.AngularVelocity: + Messages.LowLevelMessages.SendSpherical(buffer, ix, self.thing.angularVelocity) + return ix[0] + +class BinaryMsg(): + id = 0xB1 + + def __init__(self, buffer): + self.network_id = buffer[1] + self.thing_id = buffer[2] + self.thing: Thing = Thing.Get(self.network_id, self.thing_id) + self.data = buffer[3:] + + def SendTo(participant, thing, data: bytearray): + length = 3 + + if thing.network_id is None or thing is None or data is None: + return False + + participant.buffer[0:length] = [ + BinaryMsg.id, + participant.network_id, + thing.id + ] + full_length = length + len(data) + participant.buffer[length:full_length] = data + participant.SendBuffer(full_length) + return True \ No newline at end of file diff --git a/Messages/BinaryMsg.py b/Messages/BinaryMsg.py index e7b6d24..fba2786 100644 --- a/Messages/BinaryMsg.py +++ b/Messages/BinaryMsg.py @@ -1,5 +1,4 @@ -# from ..Messages import IMessage -from ..Thing import Thing +from Thing import Thing class BinaryMsg(): id = 0xB1 diff --git a/Messages/LowLevelMessages.py b/Messages/LowLevelMessages.py index eb6a560..60a9abe 100644 --- a/Messages/LowLevelMessages.py +++ b/Messages/LowLevelMessages.py @@ -1,5 +1,5 @@ import numpy as np -from ..LinearAlgebra.SwingTwist import SwingTwist +from LinearAlgebra.SwingTwist import SwingTwist def SendAngle8(buffer, ix_ref, angle): # Normalize angle diff --git a/Participant.py b/Participant.py index 960d2eb..5ce7b7e 100644 --- a/Participant.py +++ b/Participant.py @@ -2,8 +2,8 @@ import socket import threading import time -from .Messages import * -from .Thing import Thing +from Messages import * +from Thing import Thing ## A participant is device which can communicate with other participants # @@ -134,6 +134,15 @@ class Participant: # if msg.thing != None: # msg.thing.ProcessBinary(msg.data) + # For consistency with the C# and C++ versions + def Process(self, msg): + if isinstance(ThingMsg): + self.ProcessThingMsg(msg) + elif isinstance(NameMsg): + self.ProcessNameMsg(msg) + elif isinstance(ModelUrlMsg): + self.ProcessModelUrlMsg(msg) + def ProcessClientMsg(self, sender, msg: ClientMsg): pass diff --git a/Sensors/TemperatureSensor.py b/Sensors/TemperatureSensor.py index 12d13bc..28f7067 100644 --- a/Sensors/TemperatureSensor.py +++ b/Sensors/TemperatureSensor.py @@ -1,5 +1,5 @@ -from ..Thing import Thing -from ..Messages import LowLevelMessages +from Thing import Thing +from Messages import LowLevelMessages class TemperatureSensor(Thing): def __init__(self, network_id, thing_id): diff --git a/SiteServer.py b/SiteServer.py index 5d67ddc..b15c81c 100644 --- a/SiteServer.py +++ b/SiteServer.py @@ -1,6 +1,7 @@ -from .Participant import Participant -from .Messages import * - +from Participant import Participant +from Messages import * +from Thing import Thing +from Sensors.TemperatureSensor import TemperatureSensor import socket import threading @@ -14,6 +15,7 @@ class SiteServer(Participant): self.others = [] self.network_id = 0 self.buffer = bytearray(256) + self.thing_msg_constructors = {} if remote == False: self.udp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) @@ -26,6 +28,8 @@ class SiteServer(Participant): self.thread.start() else: self.udp_socket = udp_socket + + self.Register(TemperatureSensor, Thing.Type.TemperatureSensor) def AddParticipant(self, ip_address, port): # print(f'{self.name} Add site participant {ip_address} {port}') @@ -42,4 +46,21 @@ class SiteServer(Participant): # print(f'{self.name} Client') def ProcessNetworkId(self, msg): - pass \ No newline at end of file + pass + + # Register function + def Register(self, ThingClass, thing_type): + self.thing_msg_constructors[thing_type] = lambda network_id, thing_id: ThingClass(network_id, thing_id) + + def Process(self, msg): + if isinstance(ThingMsg): + self.ProcessThingMsg(msg) + else: + super().Process(msg) + + def ProcessThingMsg(self, msg): + if msg.thingType in self.thing_msg_constructors: + self.thing_msg_constructors[msg.thing_type](msg.network_id, msg.thing_id) + else: + Thing(msg.network_id, msg.thing_id, msg.thing_type) + diff --git a/Thing.py b/Thing.py index b719af7..6ae8505 100644 --- a/Thing.py +++ b/Thing.py @@ -1,5 +1,5 @@ -from .LinearAlgebra.Spherical import Spherical -from .LinearAlgebra.Quaternion import Quaternion +from LinearAlgebra.Spherical import Spherical +from LinearAlgebra.Quaternion import Quaternion ## A thing is the basic building block # diff --git a/test/thing_test.py b/test/thing_test.py index f99102a..bbc7fb6 100644 --- a/test/thing_test.py +++ b/test/thing_test.py @@ -1,5 +1,11 @@ -import unittest import time +import sys +from pathlib import Path + +# Add the project root to sys.path +sys.path.append(str(Path(__file__).resolve().parent.parent)) + +import unittest from Thing import Thing from Participant import Participant