Merged changes

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

Messages/BinaryMsg.py

@@ -1,5 +1,4 @@
-# from ..Messages import IMessage
-from ..Thing import Thing
+from Thing import Thing
 
 class BinaryMsg():
     id = 0xB1

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

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
 

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):

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)        
@@ -27,6 +29,8 @@ class SiteServer(Participant):
         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}')
         remote_participant = SiteServer(ip_address, port, remote=True, udp_socket=self.udp_socket)
@@ -43,3 +47,20 @@ class SiteServer(Participant):
     
     def ProcessNetworkId(self, msg):
         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)
+

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
 #

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