RoboidControl/RoboidControl-python
RoboidControl/RoboidControl-python
1
0
Fork 0
Code Issues 1 Pull Requests Actions Packages Releases Activity

Alignment with C#/C++

Browse Source
This commit is contained in:
Pascal Serrarens 2025-06-04 14:11:21 +02:00
parent f10babb247
commit 5afe23ac66
7 changed files with 104 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
RoboidControl/Messages/LowLevelMessages.py
View File

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

4
RoboidControl/Messages/PoseMsg.py
View File

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

44
RoboidControl/Participant.py
View File

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

9
RoboidControl/Participants/ParticipantUDP.py
View File

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

2
RoboidControl/Participants/SiteServer.py
View File

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

49
RoboidControl/Thing.py
View File

@ -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,8 +107,12 @@ 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
@ -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

12
test/thing_test.py
View File

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