Alignment with C#/C++

RoboidControl/Messages/LowLevelMessages.py

@@ -4,12 +4,12 @@ from LinearAlgebra.Spherical import Spherical
 from LinearAlgebra.Quaternion import Quaternion
 from LinearAlgebra.Angle import Angle
 
-def SendSpherical(buffer, ix_ref, vector):
+def SendSpherical(buffer, ref_ix, vector: Spherical):
     """! Send a spherical vector 
     """
-    SendFloat16(buffer, ix_ref, vector.distance)
+    SendFloat16(buffer, ref_ix, vector.distance)
-    SendAngle8(buffer, ix_ref, vector.direction.horizontal)
+    SendAngle8(buffer, ref_ix, vector.direction.horizontal)
-    SendAngle8(buffer, ix_ref, vector.direction.vertical)
+    SendAngle8(buffer, ref_ix, vector.direction.vertical)
 def ReceiveSpherical(buffer: bytes, ref_ix: list[int]) -> Spherical:
     """! Receive a spherical vector
     """
@@ -19,6 +19,19 @@ def ReceiveSpherical(buffer: bytes, ref_ix: list[int]) -> Spherical:
     v: Spherical = Spherical.Degrees(distance, horizontal, vertical)
     return v
 
+def SendSwingTwist(buffer, ref_ix, orientation: SwingTwist):
+    """! Send a swing/twist orientation
+    """
+    SendAngle8(buffer, ref_ix, orientation.swing.horizontal)
+    SendAngle8(buffer, ref_ix, orientation.swing.vertical)
+    SendAngle8(buffer, ref_ix, orientation.twist)
+def ReceiveSwingTwist(buffer, ref_ix) -> SwingTwist:
+    horizontal = ReceiveAngle8(buffer, ref_ix)
+    vertical = ReceiveAngle8(buffer, ref_ix)
+    twist = ReceiveAngle8(buffer, ref_ix)
+    orientation: SwingTwist = SwingTwist.Degrees(horizontal, vertical, twist)
+    return orientation
+
 def SendQuat32(buffer, ref_ix, q):
     """! Send a 32-bit quaternion value """
     if isinstance(q, SwingTwist):

RoboidControl/Messages/PoseMsg.py

@@ -59,7 +59,7 @@ class PoseMsg(IMessage):
             if self.pose_type & PoseMsg.Pose_Position != 0:
                 self.position = LowLevelMessages.ReceiveSpherical(buffer, ix_ref)
             if self.pose_type & PoseMsg.Pose_Orientation != 0:
-                self.orientation = LowLevelMessages.ReceiveQuaternion(buffer, ix_ref)
+                self.orientation = LowLevelMessages.ReceiveSwingTwist(buffer, ix_ref)
             if self.pose_type & PoseMsg.Pose_LinearVelocity != 0:
                 self.linear_velocity = LowLevelMessages.ReceiveSpherical(buffer, ix_ref)
             if self.pose_type & PoseMsg.Pose_AngularVelocity != 0:
@@ -101,7 +101,7 @@ class PoseMsg(IMessage):
         if self.pose_type & PoseMsg.Pose_Position:
             LowLevelMessages.SendSpherical(buffer, ix, self.thing.position)
         if self.pose_type & PoseMsg.Pose_Orientation:
-            LowLevelMessages.SendQuat32(buffer, ix, self.thing.orientation)
+            LowLevelMessages.SendSwingTwist(buffer, ix, self.thing.orientation)
         if self.pose_type & PoseMsg.Pose_LinearVelocity:
             LowLevelMessages.SendSpherical(buffer, ix, self.thing.linear_velocity)
         if self.pose_type & PoseMsg.Pose_AngularVelocity:

RoboidControl/Participant.py

@@ -11,7 +11,11 @@ class Participant:
     It is used as a basis for the local participant, but also as a reference to remote participants.
     """
 
-    def __init__(self, ip_address: str, port: int) -> None:
+# region Init
+
+    #local_participant = None
+    
+    def __init__(self, ip_address: str = None, port: int = None) -> None:
         """! Create a new participant with the given communcation info
         @param ip_address The IP address of the participant
         @param port The UDP port of the participant
@@ -28,7 +32,12 @@ class Participant:
         ## The things managed by this participant
         self.things: set[Thing] = set()
 
-        self.root: Thing = Thing.CreateRoot(self)
+        Thing.CreateRoot(self)
+
+        self.buffer: bytearray = bytearray(256)
+    
+    def ReplaceLocalParticipant(newParticipant: 'Participant'):
+        Participant.localParticipant = newParticipant
     
     def Get(self, thing_id: int) -> Optional[Thing]:
         """!  Get the thing with the given properties
@@ -67,6 +76,33 @@ class Participant:
         for thing in list(self.things):
             thing.Update(currentTimeMs)
 
+# endregion Init
+
+# region Update
+
+    def Update(self):
+        """! Update all things for this participant
+        """
+        for thing in self.things:
+            thing.Update(True)        
+
+# endregion Update
+
+# region Send
+
+    def Send(self, msg) -> bool:
+        buffer_size = msg.Serialize([self.buffer])
+        if buffer_size <= 0:
+            return True
+        
+        # print(f'{self.name} send {self.buffer[0]} to {owner.ip_address} {owner.port}')
+        self.udp_socket.sendto(self.buffer[:buffer_size], (self.ip_address, self.port))
+        return True
+
+# endregion Send
+
+# region Participant Registry
+
     participants: set['Participant'] = set()
 
     @staticmethod
@@ -109,3 +145,7 @@ class Participant:
             if foundParticipant is not None:
                 Participant.participants.add(participant)
             return participant
+
+# endergion Participant Registry
+
+Participant.local_participant = Participant()

RoboidControl/Participants/ParticipantUDP.py

@@ -75,7 +75,7 @@ class ParticipantUDP(Participant):
 
         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", self.port))
+        self.udp_socket.bind(("0.0.0.0", local_port))
 
         self.thread = threading.Thread(target = self.Receiver)
         self.thread.daemon = True
@@ -93,8 +93,7 @@ class ParticipantUDP(Participant):
 
 #region Update
 
-    def Update(self, currentTimeMs: Optional[int] = None):
+    def Update(self):
-        if currentTimeMs is None:
         currentTimeMs = int(time.time() * 1000)
 
         if self.is_isolated == False:
@@ -121,7 +120,7 @@ class ParticipantUDP(Participant):
                 if self.remote_site is not None:
                     self.Send(self.remote_site, poseMsg)
 
-            thing.Update(currentTimeMs, False)
+            thing.Update(False)
             if not(self.is_isolated or self.network_id == 0):
                 if thing.terminate:
                     destroyMsg = DestroyMsg.Create(self.network_id, thing)

RoboidControl/Participants/SiteServer.py

@@ -22,7 +22,7 @@ class SiteServer(ParticipantUDP):
         """! Create a new site server
         @param port The port of which to receive the messages
         """
-        super().__init__(local_port = port)
+        super().__init__(port = port, local_port = port)
         self.isolated = False   # site servers are never isolated
         self.publishInterval = 0
 

RoboidControl/Thing.py

@@ -1,6 +1,6 @@
 #from RoboidControl.Participant import Participant
 from LinearAlgebra.Spherical import Spherical
-from LinearAlgebra.Quaternion import Quaternion
+from LinearAlgebra.SwingTwist import SwingTwist
 
 from typing import Optional
 import time
@@ -43,7 +43,9 @@ class Thing:
         @param parent The parent thing (will override owner if set)
         """
         ## The participant owning this thing
-        self.owner: Optional['Participant'] = None
+        #self.owner: Optional['Participant'] = None
+
+
         ## The ID of the thing
         self.id: int = 0
         ## The type of the thing
@@ -53,14 +55,6 @@ class Thing:
 
         ## The parent of this thing
         self.parent: Optional[Thing] = None
-        if parent is not None:
-            self.owner = parent.owner
-            self.SetParent(parent)
-        elif owner == None:
-            from RoboidControl.Participants.ParticipantUDP import ParticipantUDP            
-            self.owner = ParticipantUDP.Isolated()
-        else:
-            self.owner = owner
 
         ## The children of this thing
         self.children: list[Thing] = list()
@@ -70,13 +64,30 @@ class Thing:
         ##  An URL pointing to the location where a model of the thing can be found
         self.model_url: Optional[str] = None
 
+        if owner is not None:
+            # When owner is set, this will be a root thing
+            self.type = Thing.Type.Root
+            self.name = "Root"
+
+            ## The participant owning this thing
+            self.owner = owner
+            # self.owner.Add(self) missing????
+        else:
+            if parent is None:
+                self.SetParent(Thing.LocalRoot())
+            else:
+                self.SetParent(parent)
+            
+            ## The participant owning this thing
+            self.owner = self.parent.owner
+
         ## The position of the thing in local space, in meters
         self.position: Spherical = Spherical.zero
         ## Boolean indicating that the thing has an updated position
         self.position_updated: bool = False
 
         ## The orientation of the thing in local space
-        self.orientation: Quaternion = Quaternion.identity
+        self.orientation: SwingTwist = SwingTwist.identity
         ## Boolean indicating the thing has an updated orientation
         self.orientation_updated: bool = False
 
@@ -96,8 +107,12 @@ class Thing:
             self.owner.Add(self)
 
     @staticmethod
-    def CreateRoot(owner):
+    def CreateRoot(owner: 'Participant'):
-        return Thing(owner = owner)
+        owner.root = Thing(owner = owner)
+    
+    def LocalRoot() -> 'Thing':
+        participant = participant.localParticipant
+        return participant.root
 
 # endregion Init
 
@@ -174,7 +189,7 @@ class Thing:
         self.position = position
         self.position_updated = True
     
-    def SetOrientation(self, orientation: Quaternion) -> None:
+    def SetOrientation(self, orientation: SwingTwist) -> None:
         """! Set the orientation of the thing
         @param orientation The new orientation in local space
         """
@@ -210,7 +225,7 @@ class Thing:
         """
         return int(time.time() * 1000)
 
-    def Update(self, currentTimeMs: int = 0, recurse: bool = False) -> None:
+    def Update(self, recurse: bool = False) -> None:
         """! Update de state of the thing
         @param currentTimeMs The current clock time in milliseconds; If this is zero, the current time is retrieved automatically
         @param recurse When true, this will Update the descendants recursively
@@ -222,7 +237,7 @@ class Thing:
 
         if recurse:
             for child in self.children:
-                child.Update(currentTimeMs, recurse)
+                child.Update(recurse)
 
 # endregion Update
 

test/thing_test.py

@@ -20,7 +20,7 @@ class ThingTest(unittest.TestCase):
         start_time = milliseconds
         while milliseconds < start_time + 5000:
             milliseconds = time.time() * 1000
-            participant.Update(milliseconds)
+            participant.Update()
 
     def test_site_server(self):
         site = SiteServer(port=7681)
@@ -29,7 +29,7 @@ class ThingTest(unittest.TestCase):
         start_time = milliseconds
         while milliseconds < start_time + 5000:
             milliseconds = time.time() * 1000
-            site.Update(milliseconds)
+            site.Update()
 
     def test_site_participant(self):
         site = SiteServer(port=7681)
@@ -39,8 +39,8 @@ class ThingTest(unittest.TestCase):
         start_time = milliseconds
         while milliseconds < start_time + 7000:
             milliseconds = time.time() * 1000
-            site.Update(milliseconds)
+            site.Update()
-            participant.Update(milliseconds)
+            participant.Update()
             time.sleep(0.1)
 
     def test_thing_msg(self):
@@ -55,8 +55,8 @@ class ThingTest(unittest.TestCase):
         start_time = milliseconds
         while milliseconds < start_time + 7000:
             milliseconds = time.time() * 1000
-            site.Update(milliseconds)
+            site.Update()
-            participant.Update(milliseconds)
+            participant.Update()
 
 if __name__ == '__main__':
     unittest.main()