Merge branch 'main' of http://gitlab.passervr.com/passer/python/controlcore

.gitattributes

@@ -0,0 +1 @@
+* text=auto

.gitignore

@@ -1,2 +1,5 @@
 __pycache__/*
-test/__pycache__/*
+test/__pycache__/*
+.vscode/*
+DoxyGen/DoxyWarnLogfile.txt
+**/__pycache__

DoxyGen/DoxyWarnLogfile.txt

@@ -1,51 +1,2 @@
-/Users/pascal/Documents/Dev/Python/controlcore/Participant.py:31: warning: documented symbol 'controlcore.Participant.Participant::bool' was not declared or defined.
-error: Project logo '//intranet/home/Afbeeldingen/PasserVR/Logos/Logo3NameRight100.png' specified by PROJECT_LOGO does not exist!
-/Users/pascal/Documents/Dev/Python/controlcore/Participant.py:26: warning: Member __init__(self, ipAddress="0.0.0.0", port=7681, remote=False, udp_socket=None) (function) of class controlcore.Participant.Participant is not documented.
-/Users/pascal/Documents/Dev/Python/controlcore/Participant.py:47: warning: Member GetParticipant(self, ip_address, port) (function) of class controlcore.Participant.Participant is not documented.
-/Users/pascal/Documents/Dev/Python/controlcore/Participant.py:57: warning: Member AddParticipant(self, ip_address, port) (function) of class controlcore.Participant.Participant is not documented.
-/Users/pascal/Documents/Dev/Python/controlcore/Participant.py:64: warning: Member Update(self, currentTimeMs) (function) of class controlcore.Participant.Participant is not documented.
-/Users/pascal/Documents/Dev/Python/controlcore/Participant.py:76: warning: Member SendThingInfo(self, thing) (function) of class controlcore.Participant.Participant is not documented.
-/Users/pascal/Documents/Dev/Python/controlcore/Participant.py:81: warning: Member Send(self, msg) (function) of class controlcore.Participant.Participant is not documented.
-/Users/pascal/Documents/Dev/Python/controlcore/Participant.py:90: warning: Member Publish(self, msg) (function) of class controlcore.Participant.Participant is not documented.
-/Users/pascal/Documents/Dev/Python/controlcore/Participant.py:103: warning: Member Receiver(self) (function) of class controlcore.Participant.Participant is not documented.
-/Users/pascal/Documents/Dev/Python/controlcore/Participant.py:115: warning: Member ReceiveData(self, data, remote_participant) (function) of class controlcore.Participant.Participant is not documented.
-/Users/pascal/Documents/Dev/Python/controlcore/Participant.py:135: warning: Member ProcessClientMsg(self, sender, ParticipantMsg msg) (function) of class controlcore.Participant.Participant is not documented.
-/Users/pascal/Documents/Dev/Python/controlcore/Participant.py:138: warning: Member ProcessNetworkIdMsg(self, sender, NetworkIdMsg msg) (function) of class controlcore.Participant.Participant is not documented.
-/Users/pascal/Documents/Dev/Python/controlcore/Participant.py:147: warning: Member ProcessInvestigateMsg(self, bytearray data) (function) of class controlcore.Participant.Participant is not documented.
-/Users/pascal/Documents/Dev/Python/controlcore/Participant.py:150: warning: Member ProcessThingMsg(self, ThingMsg msg) (function) of class controlcore.Participant.Participant is not documented.
-/Users/pascal/Documents/Dev/Python/controlcore/Participant.py:153: warning: Member ProcessNameMsg(self, NameMsg msg) (function) of class controlcore.Participant.Participant is not documented.
-/Users/pascal/Documents/Dev/Python/controlcore/Participant.py:156: warning: Member ProcessModelUrlMsg(self, ModelUrlMsg msg) (function) of class controlcore.Participant.Participant is not documented.
-/Users/pascal/Documents/Dev/Python/controlcore/Participant.py:27: warning: Member ip_address (variable) of class controlcore.Participant.Participant is not documented.
-/Users/pascal/Documents/Dev/Python/controlcore/Participant.py:28: warning: Member port (variable) of class controlcore.Participant.Participant is not documented.
-/Users/pascal/Documents/Dev/Python/controlcore/Participant.py:34: warning: Member udp_socket (variable) of class controlcore.Participant.Participant is not documented.
-/Users/pascal/Documents/Dev/Python/controlcore/Participant.py:16: warning: Member publishInterval (variable) of class controlcore.Participant.Participant is not documented.
-/Users/pascal/Documents/Dev/Python/controlcore/Participant.py:18: warning: Member buffer (variable) of class controlcore.Participant.Participant is not documented.
-/Users/pascal/Documents/Dev/Python/controlcore/Participant.py:21: warning: Member nextPublishMe (variable) of class controlcore.Participant.Participant is not documented.
-/Users/pascal/Documents/Dev/Python/controlcore/Participant.py:22: warning: Member others (variable) of class controlcore.Participant.Participant is not documented.
-/Users/pascal/Documents/Dev/Python/controlcore/Participant.py:23: warning: Member thread (variable) of class controlcore.Participant.Participant is not documented.
-/Users/pascal/Documents/Dev/Python/controlcore/Participant.py:24: warning: Member name (variable) of class controlcore.Participant.Participant is not documented.
-/Users/pascal/Documents/Dev/Python/controlcore/Thing.py:21: warning: Member __init__(self, type=Type.Undetermined, parent=None, name=None) (function) of class controlcore.Thing.Thing is not documented.
-/Users/pascal/Documents/Dev/Python/controlcore/Thing.py:42: warning: Member update(self, currentTime) (function) of class controlcore.Thing.Thing is not documented.
-/Users/pascal/Documents/Dev/Python/controlcore/Thing.py:45: warning: Member ProcessBinary(self, data) (function) of class controlcore.Thing.Thing is not documented.
-/Users/pascal/Documents/Dev/Python/controlcore/Thing.py:51: warning: Member Add(thing) (function) of class controlcore.Thing.Thing is not documented.
-/Users/pascal/Documents/Dev/Python/controlcore/Thing.py:56: warning: Member Get(networkId, thingId) (function) of class controlcore.Thing.Thing is not documented.
-/Users/pascal/Documents/Dev/Python/controlcore/Thing.py:22: warning: Member networkId (variable) of class controlcore.Thing.Thing is not documented.
-/Users/pascal/Documents/Dev/Python/controlcore/Thing.py:23: warning: Member id (variable) of class controlcore.Thing.Thing is not documented.
-/Users/pascal/Documents/Dev/Python/controlcore/Thing.py:24: warning: Member type (variable) of class controlcore.Thing.Thing is not documented.
-/Users/pascal/Documents/Dev/Python/controlcore/Thing.py:26: warning: Member parent_id (variable) of class controlcore.Thing.Thing is not documented.
-/Users/pascal/Documents/Dev/Python/controlcore/Thing.py:30: warning: Member name (variable) of class controlcore.Thing.Thing is not documented.
-/Users/pascal/Documents/Dev/Python/controlcore/Thing.py:31: warning: Member model_url (variable) of class controlcore.Thing.Thing is not documented.
-/Users/pascal/Documents/Dev/Python/controlcore/Thing.py:33: warning: Member position (variable) of class controlcore.Thing.Thing is not documented.
-/Users/pascal/Documents/Dev/Python/controlcore/Thing.py:34: warning: Member orientation (variable) of class controlcore.Thing.Thing is not documented.
-/Users/pascal/Documents/Dev/Python/controlcore/Thing.py:35: warning: Member linearVelocity (variable) of class controlcore.Thing.Thing is not documented.
-/Users/pascal/Documents/Dev/Python/controlcore/Thing.py:36: warning: Member angularVelocity (variable) of class controlcore.Thing.Thing is not documented.
-/Users/pascal/Documents/Dev/Python/controlcore/Thing.py:38: warning: Member pose_updated (variable) of class controlcore.Thing.Thing is not documented.
-/Users/pascal/Documents/Dev/Python/controlcore/Thing.py:16: warning: Member Position (variable) of class controlcore.Thing.Thing is not documented.
-/Users/pascal/Documents/Dev/Python/controlcore/Thing.py:17: warning: Member Orientation (variable) of class controlcore.Thing.Thing is not documented.
-/Users/pascal/Documents/Dev/Python/controlcore/Thing.py:18: warning: Member LinearVelocity (variable) of class controlcore.Thing.Thing is not documented.
-/Users/pascal/Documents/Dev/Python/controlcore/Thing.py:19: warning: Member AngularVelocity (variable) of class controlcore.Thing.Thing is not documented.
-/Users/pascal/Documents/Dev/Python/controlcore/Thing.py:48: warning: Member allThings (variable) of class controlcore.Thing.Thing is not documented.
-/Users/pascal/Documents/Dev/Python/controlcore/Messages/NetworkIdMsg.py:7: warning: Member id (variable) of class NetworkIdMsg.NetworkIdMsg is not documented.
-/Users/pascal/Documents/Dev/Python/controlcore/Messages/NetworkIdMsg.py:8: warning: Member length (variable) of class NetworkIdMsg.NetworkIdMsg is not documented.
-/Users/pascal/Documents/Dev/Python/controlcore/Messages/NetworkIdMsg.py:10: warning: Member network_id (variable) of class NetworkIdMsg.NetworkIdMsg is not documented.
-/Users/pascal/Documents/Dev/Python/controlcore/LinearAlgebra/SwingTwist.py:6: warning: Member __init__(self, Direction swing, float twist) (function) of class SwingTwist.SwingTwist is not documented.
+error: tag OUTPUT_DIRECTORY: Output directory '//intranet/web/passer_life/apis/ControlCore/Python/' does not exist and cannot be created
+       Exiting...

Messages/BinaryMsg.py

@@ -0,0 +1,26 @@
+from Thing import Thing
+
+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/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]

Messages/LowLevelMessages.py

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

Messages/Messages.py

@@ -51,13 +51,13 @@ class PoseMsg(IMessage):
         ]
         ix = [4]
         if self.thing.pose_updated & Thing.Position:
-            LowLevelMessages.SendSpherical(buffer, ix, self.thing.position)
+            Messages.LowLevelMessages.SendSpherical(buffer, ix, self.thing.position)
         if self.thing.pose_updated & Thing.Orientation:
-            LowLevelMessages.SendQuat32(buffer, ix, self.thing.orientation)
+            Messages.LowLevelMessages.SendQuat32(buffer, ix, self.thing.orientation)
         if self.thing.pose_updated & Thing.LinearVelocity:
-            LowLevelMessages.SendSpherical(buffer, ix, self.thing.linearVelocity)
+            Messages.LowLevelMessages.SendSpherical(buffer, ix, self.thing.linearVelocity)
         if self.thing.pose_updated & Thing.AngularVelocity:
-            LowLevelMessages.SendSpherical(buffer, ix, self.thing.angularVelocity)
+            Messages.LowLevelMessages.SendSpherical(buffer, ix, self.thing.angularVelocity)
         return ix[0]
 
 class BinaryMsg():

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]

Messages/__init__.py

@@ -0,0 +1,16 @@
+__all__ = ['BinaryMsg',
+           'NetworkIdMsg',
+           'InvestigateMsg',
+           'ThingMsg',
+           'NameMsg',
+           'ModelUrlMsg',
+           'PoseMsg']
+
+from .BinaryMsg import BinaryMsg
+from .ParticipantMsg import ParticipantMsg
+from .InvestigateMsg import InvestigateMsg
+from .ThingMsg import ThingMsg
+from .NetworkIdMsg import NetworkIdMsg
+from .NameMsg import NameMsg
+from .ModelUrlMsg import ModelUrlMsg
+from .PoseMsg import PoseMsg

Participant.py

@@ -6,6 +6,7 @@ from Messages.NetworkIdMsg import NetworkIdMsg
 from Messages.ThingMsg import ThingMsg
 from Messages.NameMsg import NameMsg
 from Messages.ModelUrlMsg import ModelUrlMsg
+from Messages import *
 from Thing import Thing
 
 class Participant:
@@ -29,11 +30,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)
@@ -61,7 +64,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(ParticipantMsg(self.network_id))
             print(f'Publish ClientMsg {self.network_id}')
@@ -148,7 +154,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}')
@@ -156,4 +171,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

Sensors/TemperatureSensor.py

@@ -0,0 +1,19 @@
+from Thing import Thing
+from Messages 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)

SiteServer.py

@@ -1,6 +1,8 @@
 from Participant import Participant
 from Messages.ParticipantMsg import ParticipantMsg
 from Messages.NetworkIdMsg import NetworkIdMsg
+from Sensors.TemperatureSensor import TemperatureSensor
+from Thing import Thing
 
 import socket
 import threading
@@ -15,11 +17,12 @@ 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)        
             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)
@@ -27,6 +30,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}')
@@ -43,4 +48,21 @@ class SiteServer(Participant):
         #     print(f'{self.name} Client')
     
     def ProcessNetworkId(self, msg):
-        pass
+        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

@@ -11,6 +11,7 @@ class Thing:
         Switch = 0x01
         DistanceSensor = 0x02
         DirectionalSensor = 0x03
+        TemperatureSensor = 0x04
         Animator = 0x40
 
     Position = 0x01
@@ -18,9 +19,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
@@ -43,6 +44,7 @@ class Thing:
         pass
 
     def ProcessBinary(self, data):
+        print('default binary processor')
         pass
 
     allThings = set({ None })
@@ -53,16 +55,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)

__init__.py

@@ -1,10 +1,6 @@
-__all__ = ['Direction',
-           'Spherical', 
-           'Thing', 
+__all__ = ['Thing', 
            'Participant', 
-           'Messages', 
-           'SiteServer', 
-           'SwingTwist']
+           'SiteServer']
 
 from .LinearAlgebra.Direction import Direction
 from .Participant import Participant

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