From 5afe23ac66632f40400a825f5510acb8383cfc90 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 4 Jun 2025 14:11:21 +0200 Subject: [PATCH] Alignment with C#/C++ --- RoboidControl/Messages/LowLevelMessages.py | 21 +++++++-- RoboidControl/Messages/PoseMsg.py | 4 +- RoboidControl/Participant.py | 44 +++++++++++++++++- RoboidControl/Participants/ParticipantUDP.py | 9 ++-- RoboidControl/Participants/SiteServer.py | 2 +- RoboidControl/Thing.py | 49 +++++++++++++------- test/thing_test.py | 12 ++--- 7 files changed, 104 insertions(+), 37 deletions(-) diff --git a/RoboidControl/Messages/LowLevelMessages.py b/RoboidControl/Messages/LowLevelMessages.py index 0328c93..98cde92 100644 --- a/RoboidControl/Messages/LowLevelMessages.py +++ b/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) - SendAngle8(buffer, ix_ref, vector.direction.horizontal) - SendAngle8(buffer, ix_ref, vector.direction.vertical) + SendFloat16(buffer, ref_ix, vector.distance) + SendAngle8(buffer, ref_ix, vector.direction.horizontal) + 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): diff --git a/RoboidControl/Messages/PoseMsg.py b/RoboidControl/Messages/PoseMsg.py index dba53ac..8d99cf6 100644 --- a/RoboidControl/Messages/PoseMsg.py +++ b/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: diff --git a/RoboidControl/Participant.py b/RoboidControl/Participant.py index 9f1c92e..0899d11 100644 --- a/RoboidControl/Participant.py +++ b/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,8 +32,13 @@ 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() \ No newline at end of file diff --git a/RoboidControl/Participants/ParticipantUDP.py b/RoboidControl/Participants/ParticipantUDP.py index c488d3c..e3ba0bc 100644 --- a/RoboidControl/Participants/ParticipantUDP.py +++ b/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,9 +93,8 @@ class ParticipantUDP(Participant): #region Update - def Update(self, currentTimeMs: Optional[int] = None): - if currentTimeMs is None: - currentTimeMs = int(time.time() * 1000) + def Update(self): + currentTimeMs = int(time.time() * 1000) if self.is_isolated == False: if self.publishInterval > 0 and currentTimeMs > self.nextPublishMe: @@ -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) diff --git a/RoboidControl/Participants/SiteServer.py b/RoboidControl/Participants/SiteServer.py index a362d2d..277cc51 100644 --- a/RoboidControl/Participants/SiteServer.py +++ b/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 diff --git a/RoboidControl/Thing.py b/RoboidControl/Thing.py index 540fe82..c1713eb 100644 --- a/RoboidControl/Thing.py +++ b/RoboidControl/Thing.py @@ -1,6 +1,6 @@ -# from RoboidControl.Participant import Participant +#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,9 +107,13 @@ class Thing: self.owner.Add(self) @staticmethod - def CreateRoot(owner): - return Thing(owner = owner) + def CreateRoot(owner: 'Participant'): + owner.root = Thing(owner = owner) + def LocalRoot() -> 'Thing': + participant = participant.localParticipant + return participant.root + # endregion Init # region Hierarchy @@ -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 diff --git a/test/thing_test.py b/test/thing_test.py index 65866bd..a330164 100644 --- a/test/thing_test.py +++ b/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) - participant.Update(milliseconds) + site.Update() + 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) - participant.Update(milliseconds) + site.Update() + participant.Update() if __name__ == '__main__': unittest.main() \ No newline at end of file