From d4d67e8233c8ee34d7c6da52189ffb0857b4e6ee Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Tue, 21 Jan 2025 11:05:19 +0100 Subject: [PATCH] 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