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

Added name and model support

Browse Source
This commit is contained in:
Pascal Serrarens 2024-12-22 10:22:32 +01:00
parent 77a7153675
commit 1eb71a21c0
4 changed files with 143 additions and 114 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

209
Messages.py
View File

@ -1,7 +1,6 @@
# from .Participant import Participant # from .Participant import Participant
from . import LowLevelMessages from . import LowLevelMessages
from .Spherical import Spherical from .Thing import Thing
from .Quaternion import Quaternion
class IMessage: class IMessage:
id = 0x00 id = 0x00
@ -10,10 +9,10 @@ class IMessage:
return 0 return 0
def SendTo(self, participant): def SendTo(self, participant):
bufferSize = self.Serialize([participant.buffer]) buffer_size = self.Serialize([participant.buffer])
if bufferSize == 0: if buffer_size == 0:
return False return False
return participant.SendBuffer(bufferSize) return participant.SendBuffer(buffer_size)
def Publish(self, participant): def Publish(self, participant):
bufferSize = self.Serialize([participant.buffer]) bufferSize = self.Serialize([participant.buffer])
@ -25,20 +24,20 @@ class ClientMsg(IMessage):
id = 0xA0 id = 0xA0
length = 2 length = 2
def __init__(self, networkId): def __init__(self, network_id):
if isinstance(networkId, int): if isinstance(network_id, int):
self.networkId = networkId self.network_id = network_id
elif isinstance(networkId, bytes): elif isinstance(network_id, bytes):
self.networkId = networkId[1] self.network_id = network_id[1]
def Serialize(self, buffer_ref): def Serialize(self, buffer_ref):
if self.networkId is None: if self.network_id is None:
return 0 return 0
buffer: bytearray = buffer_ref[0] buffer: bytearray = buffer_ref[0]
buffer[0:2] = [ buffer[0:ClientMsg.length] = [
ClientMsg.id, ClientMsg.id,
self.networkId self.network_id
] ]
return ClientMsg.length return ClientMsg.length
@ -46,21 +45,21 @@ class NetworkIdMsg(IMessage):
id = 0xA1 id = 0xA1
length = 2 length = 2
def __init__(self, networkId): def __init__(self, network_id):
self.networkId = None self.network_id = None
if isinstance(networkId, int): if isinstance(network_id, int):
self.networkId = networkId self.network_id = network_id
elif isinstance(networkId, bytes): elif isinstance(network_id, bytes):
self.networkId = networkId[1] self.network_id = network_id[1]
def Serialize(self, buffer_ref): def Serialize(self, buffer_ref):
if self.networkId is None: if self.network_id is None:
return 0 return 0
buffer: bytearray = buffer_ref[0] buffer: bytearray = buffer_ref[0]
buffer[0:2] = [ buffer[0:NetworkIdMsg.length] = [
NetworkIdMsg.id, NetworkIdMsg.id,
self.networkId self.network_id
] ]
return NetworkIdMsg.length return NetworkIdMsg.length
@ -68,106 +67,112 @@ class ThingMsg(IMessage):
id = 0x80 id = 0x80
length = 5 length = 5
def __init__(self, networkId, thing): def __init__(self, network_id, thing):
self.networkId = networkId self.network_id = network_id
self.thingId = thing.id self.thing = thing
self.thingType = thing.type
self.parentId = None
def Serialize(self, buffer_ref): def Serialize(self, buffer_ref):
if self.networkId is None or self.thingId is None: if self.network_id is None or self.thing is None:
return 0 return 0
buffer: bytearray = buffer_ref[0] buffer: bytearray = buffer_ref[0]
buffer[0:5] = [ buffer[0:ThingMsg.length] = [
ThingMsg.id, ThingMsg.id,
self.networkId, self.network_id,
self.thingId, self.thing.id,
0x00, self.thing.type,
0x00 self.thing.parent_id
] ]
return ThingMsg.length return ThingMsg.length
class NameMsg(IMessage):
id = 0x91
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]
encoded_name = self.thing.name.encode('utf-8')
name_length = len(encoded_name)
full_length = NameMsg.length + name_length
if name_length == 0 or full_length > len(buffer):
return 0
buffer[0:NameMsg.length] = [
NameMsg.id,
self.network_id,
self.thing.id,
name_length
]
# Append the name string
buffer[NameMsg.length:full_length] = encoded_name
return full_length
class ModelUrlMsg(IMessage): class ModelUrlMsg(IMessage):
id = 0x90 id = 0x90
length = 6 length = 6
def __init__(self, networkId, thingId, url): def __init__(self, network_id, thing):
self.networkId = networkId self.network_id = network_id
self.thingId = thingId self.thing = thing
self.url = url
def Serialize(self, buffer):
buffer[0:ModelUrlMsg.length] = [
ModelUrlMsg.id,
self.networkId,
self.thingId,
0x3D, # Dummy float16 value 1
0x00,
len(self.url)
]
# Append the url string
fullLength = ModelUrlMsg.length + len(self.url)
buffer[ModelUrlMsg.length:fullLength] = \
[ord(c) for c in self.url]
return fullLength
# ix = 0
# buffer[ix] = ModelUrlMsg.id
# ix+=1
# buffer[ix] = self.networkId
# ix+=1
# buffer[ix] = self.thingId
# ix+=1
# buffer[ix] = 0x3D # Dummy float16 value 1
# ix+=1
# buffer[ix] = 0x00
# ix+=1
# buffer[ix] = len(self.url)
# ix+=1
# for c in self.url:
# buffer[ix] = ord(c)
# ix+=1
# return ix
class PoseMsg(IMessage):
id = 0x10
length = 4 + 4 + 4
Position = 0x01
Orientation = 0x02
LinearVelocity = 0x04
AngularVelocity = 0x08
def __init__(self, networkId, thing, poseType):
self.networkId = networkId
self.thingId = thing.id
poseType = 0x0F # unity server currently requires position and orientation
self.poseType = poseType
self.position = Spherical.zero
self.orientation = Quaternion.identity
self.linearVelocity = thing.linearVelocity
self.angularVelocity = thing.angularVelocity
def Serialize(self, buffer_ref): def Serialize(self, buffer_ref):
if (self.networkId is None) or (self.thingId is None): if self.network_id is None or self.thing is None:
return 0 return 0
buffer: bytearray = buffer_ref[0] buffer: bytearray = buffer_ref[0]
buffer[0:4] = [
encoded_url = self.thing.model_url.encode('utf-8')
url_length = len(encoded_url)
full_length = ModelUrlMsg.length + url_length
if url_length == 0 or full_length > len(buffer):
return 0
buffer[0:ModelUrlMsg.length] = [
ModelUrlMsg.id,
self.network_id,
self.thing.id,
0x3D, # Dummy float16 value 1
0x00, # this field will disappear
url_length
]
# Append the url string
buffer[ModelUrlMsg.length:full_length] = encoded_url
return full_length
class PoseMsg(IMessage):
id = 0x10
length = 4
def __init__(self, network_id, thing, poseType):
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, PoseMsg.id,
self.networkId, self.network_id,
self.thingId, self.thing.id,
self.poseType self.thing.pose_updated
] ]
ix = [4] ix = [4]
if self.poseType & PoseMsg.Position: if self.thing.pose_updated & Thing.Position:
LowLevelMessages.SendSpherical(buffer, ix, self.position) LowLevelMessages.SendSpherical(buffer, ix, self.thing.position)
if self.poseType & PoseMsg.Orientation: if self.thing.pose_updated & Thing.Orientation:
LowLevelMessages.SendQuat32(buffer, ix, self.orientation) LowLevelMessages.SendQuat32(buffer, ix, self.thing.orientation)
if self.poseType & PoseMsg.LinearVelocity: if self.thing.pose_updated & Thing.LinearVelocity:
LowLevelMessages.SendSpherical(buffer, ix, self.linearVelocity) LowLevelMessages.SendSpherical(buffer, ix, self.thing.linearVelocity)
if self.poseType & PoseMsg.AngularVelocity: if self.thing.pose_updated & Thing.AngularVelocity:
LowLevelMessages.SendSpherical(buffer, ix, self.angularVelocity) LowLevelMessages.SendSpherical(buffer, ix, self.thing.angularVelocity)
return ix[0] return ix[0]

7
Participant.py
View File

@ -11,9 +11,10 @@ class Participant:
self.port = port self.port = port
self.udpSocket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) self.udpSocket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
self.udpSocket.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1) self.udpSocket.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)
self.networkId = 0 # self.udpSocket.bind(("0.0.0.0", 7681))
#self.udpSocket.bind(("0.0.0.0", 7681))
self.buffer = bytearray(256) self.buffer = bytearray(256)
self.network_id = 0
self.nextPublishMe = 0 self.nextPublishMe = 0
def SendBuffer(self, bufferSize): def SendBuffer(self, bufferSize):
@ -37,7 +38,7 @@ class Participant:
return True return True
def PublishMe(self): def PublishMe(self):
msg = Messages.ClientMsg(self.networkId) msg = Messages.ClientMsg(self.network_id)
msg.Publish(self) msg.Publish(self)
def Update(self, currentTime): def Update(self, currentTime):

13
SiteServer.py
View File

@ -17,7 +17,12 @@ class SiteServer(Participant):
return super().Update(currentTime) return super().Update(currentTime)
def ProcessNetworkIdMsg(self, msg): def ProcessNetworkIdMsg(self, thing_msg):
self.networkId = msg.networkId self.network_id = thing_msg.network_id
msg = Messages.ThingMsg(self.networkId, next(iter(Thing.allThings))) thing = next(iter(Thing.allThings))
msg.SendTo(self) thing_msg = Messages.ThingMsg(self.network_id, thing)
thing_msg.SendTo(self)
name_msg = Messages.NameMsg(self.network_id, thing)
name_msg.SendTo(self)
model_msg = Messages.ModelUrlMsg(self.network_id, thing)
model_msg.SendTo(self)

28
Thing.py
View File

@ -1,12 +1,30 @@
from .Spherical import Spherical
from .Quaternion import Quaternion
class Thing: class Thing:
allThings = set() allThings = set()
def __init__(self): Position = 0x01
self.networkId = None Orientation = 0x02
self.id = None LinearVelocity = 0x04
self.type = None AngularVelocity = 0x08
def __init__(self):
self.networkId = 0
self.id = 0
self.type = 0
self.parent_id = 0
self.name = None
self.model_url = None
self.position = Spherical.zero
self.orientation = Quaternion.identity
self.linearVelocity = Spherical.zero
self.angularVelocity = Spherical.zero
self.pose_updated = 0x00 # the bits indicate which fields have been updated
self.modelUrl = None
Thing.Add(self) Thing.Add(self)
def update(self, currentTime): def update(self, currentTime):