RoboidControl-python/Messages.py

import Messages.LowLevelMessages as LowLevelMessages
from Thing import Thing

class IMessage:
    id = 0x00

    ## Serialize the message into the given buffer
    #
    ## @returns: the length of the message
    def Serialize(buffer):
        return 0
    
    def SendTo(self, participant):
        buffer_size = self.Serialize([participant.buffer])
        if buffer_size == 0:
            return False
        return participant.SendBuffer(buffer_size)
    
    def Publish(self, participant):
        bufferSize = self.Serialize([participant.buffer])
        if bufferSize == 0:
            return False
        return participant.PublishBuffer(bufferSize)

class ParticipantMsg(IMessage):
    """! A participant messages notifies other participants of its presence.

    When received by another participant, it can be followed by a NetworkIdMsg
    to announce that participant to this client such that it can join privately
    """
    ## The message ID
    id: int = 0xA0
    ## The length of the message
    length: int = 2
    ## The network ID known by the participant. 
    network_id: int = None

    def __init__(self, network_id: int):
        """! Create a new message for sending
        @param network_id The network ID known by the participant. Use 0 if it is unknown
        """
        if isinstance(network_id, int):
            self.network_id = network_id
        elif isinstance(network_id, bytes):
            self.network_id = network_id[1]

    def Serialize(self, buffer_ref):
        """! Serialize the message into a byte array. 
        @param buffer_ref The buffer to serialize into
        @return The length of the message in the buffer
        """
        if buffer_ref is None or self.network_id is None:
            return 0
        
        buffer: bytearray = buffer_ref[0]
        buffer[0:ParticipantMsg.length] = [
            ParticipantMsg.id,
            self.network_id
        ]
        return ParticipantMsg.length

## A network id message invites another participant to a site
#
## This can be sent in response to a ClientMsg
class NetworkIdMsg(IMessage):
    id = 0xA1
    length = 2

    network_id = None
    
    ## Create a network id message
    #
    # @param network_id The network id assigned to the remote participant.
    def __init__(self, network_id):
        self.network_id = None
        if isinstance(network_id, int):
            self.network_id = network_id
        elif isinstance(network_id, bytes):
            self.network_id = network_id[1]

    ## Serialize the message into the given buffer
    #
    ## @param buffer_ref A reference to the buffer to use. This should be a list with the buffer as its first and only element
    ## @returns the length of the message
    def Serialize(self, buffer_ref):
        if self.network_id is None:
            return 0
        
        buffer: bytearray = buffer_ref[0]
        buffer[0:NetworkIdMsg.length] = [
            NetworkIdMsg.id,
            self.network_id
        ]
        return NetworkIdMsg.length

class InvestigateMsg():
    id = 0x81
    length = 3

    def __init__(self, buffer):
        self.network_id = buffer[1]
        self.thing_id = buffer[2]

class PoseMsg(IMessage):
    """! Message to communicate the pose of the thing 

    The pose is in local space relative to the parent. 
    If there is not parent (the thing is a root thing), the pose will be in world space.
    """
    ## The message ID
    id = 0x10
    ## The length of the message
    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:
            Messages.LowLevelMessages.SendSpherical(buffer, ix, self.thing.position)
        if self.thing.pose_updated & Thing.Orientation:
            Messages.LowLevelMessages.SendQuat32(buffer, ix, self.thing.orientation)
        if self.thing.pose_updated & Thing.LinearVelocity:
            Messages.LowLevelMessages.SendSpherical(buffer, ix, self.thing.linearVelocity)
        if self.thing.pose_updated & Thing.AngularVelocity:
            Messages.LowLevelMessages.SendSpherical(buffer, ix, self.thing.angularVelocity)
        return ix[0]

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