diff --git a/.gitattributes b/.gitattributes new file mode 100644 index 0000000..2125666 --- /dev/null +++ b/.gitattributes @@ -0,0 +1 @@ +* text=auto \ No newline at end of file diff --git a/.gitignore b/.gitignore index e5ddc82..d31799e 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,5 @@ __pycache__/* -test/__pycache__/* \ No newline at end of file +test/__pycache__/* +.vscode/* +DoxyGen/DoxyWarnLogfile.txt +**/__pycache__ \ No newline at end of file diff --git a/BinaryMsg.py b/BinaryMsg.py new file mode 100644 index 0000000..ad9248d --- /dev/null +++ b/BinaryMsg.py @@ -0,0 +1,27 @@ +from .Messages import IMessage +from .Thing import Thing + +class BinaryMsg(IMessage): + 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 \ No newline at end of file diff --git a/ClientMsg.py b/ClientMsg.py index a431436..0b51580 100644 --- a/ClientMsg.py +++ b/ClientMsg.py @@ -1,4 +1,4 @@ -from Messages import IMessage +from .Messages import IMessage ## A client message announces the presence of a participant # diff --git a/LowLevelMessages.py b/LowLevelMessages.py index 5545141..114107d 100644 --- a/LowLevelMessages.py +++ b/LowLevelMessages.py @@ -1,5 +1,5 @@ import numpy as np -from SwingTwist import SwingTwist +from .SwingTwist import SwingTwist def SendAngle8(buffer, ix_ref, angle): # Normalize angle @@ -24,7 +24,10 @@ def SendFloat16(buffer, ix_ref, value): def ReceiveFloat16(buffer, ix_ref) -> float: ix = ix_ref[0] + # if ix < len(buffer) - 1: binary = (buffer[ix] << 8) + buffer[ix+1] + # else: + # binary = 0 value16 = np.uint16(binary).view(np.float16) ix_ref[0] += 2 return float(value16) diff --git a/Messages.py b/Messages.py index 4aff96f..5206a60 100644 --- a/Messages.py +++ b/Messages.py @@ -1,5 +1,5 @@ -import LowLevelMessages -from Thing import Thing +from . import LowLevelMessages +from .Thing import Thing class IMessage: id = 0x00 @@ -59,28 +59,3 @@ class PoseMsg(IMessage): if self.thing.pose_updated & Thing.AngularVelocity: 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 \ No newline at end of file diff --git a/ModelUrlMsg.py b/ModelUrlMsg.py index bd670e2..349729a 100644 --- a/ModelUrlMsg.py +++ b/ModelUrlMsg.py @@ -1,4 +1,4 @@ -from Messages import IMessage +from .Messages import IMessage class ModelUrlMsg(IMessage): id = 0x90 diff --git a/NameMsg.py b/NameMsg.py index 5acb8bd..4908748 100644 --- a/NameMsg.py +++ b/NameMsg.py @@ -1,4 +1,4 @@ -from Messages import IMessage +from .Messages import IMessage class NameMsg(IMessage): id = 0x91 diff --git a/NetworkIdMsg.py b/NetworkIdMsg.py index 280fae5..524baf6 100644 --- a/NetworkIdMsg.py +++ b/NetworkIdMsg.py @@ -1,4 +1,4 @@ -from Messages import IMessage +from .Messages import IMessage ## A network id message invites another participant to a site # diff --git a/Participant.py b/Participant.py index 9d1fc3a..5261c7d 100644 --- a/Participant.py +++ b/Participant.py @@ -1,12 +1,14 @@ import socket import threading +import time -from ClientMsg import ClientMsg -from NetworkIdMsg import NetworkIdMsg -from ThingMsg import ThingMsg -from NameMsg import NameMsg -from ModelUrlMsg import ModelUrlMsg -from Thing import Thing +from .ClientMsg import ClientMsg +from .NetworkIdMsg import NetworkIdMsg +from .ThingMsg import ThingMsg +from .NameMsg import NameMsg +from .ModelUrlMsg import ModelUrlMsg +from .BinaryMsg import BinaryMsg +from .Thing import Thing ## A participant is device which can communicate with other participants # @@ -27,11 +29,13 @@ class Participant: self.others = [] self.network_id = 0 self.buffer = bytearray(256) + self.thing_msg_processors = {} + self.new_thing_handlers = [] if remote == False: 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", 0)) + self.udp_socket.bind(("0.0.0.0", port)) self.AddParticipant(self.ip_address, self.port) self.thread = threading.Thread(target = self.Receiver) @@ -59,7 +63,10 @@ class Participant: self.others.append(remote_participant) return remote_participant - def Update(self, currentTimeMs): + def Update(self, currentTimeMs = None): + if currentTimeMs is None: + currentTimeMs = time.time() * 1000 + if self.publishInterval > 0 and currentTimeMs > self.nextPublishMe: self.Publish(ClientMsg(self.network_id)) print(f'Publish ClientMsg {self.network_id}') @@ -126,9 +133,11 @@ class Participant: self.ProcessNameMsg(NameMsg(data)) case ModelUrlMsg.id: self.ProcessModelUrlMsg(ModelUrlMsg(data)) - # case Messages.BinaryMsg.id: - # msg = Messages.BinaryMsg(data) - # msg.thing.ProcessBinary(msg.data) + case BinaryMsg.id: + self.ProcessBinary(BinaryMsg(data)) + # msg = BinaryMsg(data) + # if msg.thing != None: + # msg.thing.ProcessBinary(msg.data) def ProcessClientMsg(self, sender, msg: ClientMsg): pass @@ -146,7 +155,16 @@ class Participant: pass def ProcessThingMsg(self, msg: ThingMsg): - print(f'received thing {msg.thing_id}') + print(f'received thing {msg.network_id} {msg.thing_id}') + if msg.thing_type in self.thing_msg_processors: + constructor = self.thing_msg_processors[msg.thing_type] + constructor(msg.network_id, msg.thing_id) + + # not really 'new' thing, but it is a start + thing = Thing.Get(msg.network_id, msg.thing_id) + if thing is not None: + for handler in self.new_thing_handlers: + handler(thing) def ProcessNameMsg(self, msg: NameMsg): print(f'received name {msg.name}') @@ -154,4 +172,22 @@ class Participant: def ProcessModelUrlMsg(self, msg: ModelUrlMsg): print(f'received model url: {msg.url}') + def ProcessBinary(self, msg: BinaryMsg): + print('received binary data') + if msg.thing != None: + msg.thing.ProcessBinary(msg.data) + + + def Register(self, constructor, thing_type): + self.thing_msg_processors[thing_type] = constructor + + def OnNewThing(self, event_handler): + self.new_thing_handlers.append(event_handler) + + def OnNewThingType(self, thing_type, event_handler): + def ConditionalHandler(thing): + if thing.type == thing_type: + event_handler(thing) + self.new_thing_handlers.append(ConditionalHandler) + #endregion \ No newline at end of file diff --git a/Sensors/TemperatureSensor.py b/Sensors/TemperatureSensor.py new file mode 100644 index 0000000..ef34910 --- /dev/null +++ b/Sensors/TemperatureSensor.py @@ -0,0 +1,19 @@ +from ..Thing import Thing +from .. import LowLevelMessages + +class TemperatureSensor(Thing): + def __init__(self, network_id, thing_id): + super().__init__(network_id, thing_id, type = Thing.Type.TemperatureSensor) + self.temp = 0 + self._watchers = [] + + def OnUpdate(self, handler): + self._watchers.append(handler) + def CancelOnUpdate(self, handler): + self._watchers.remove(handler) + + def ProcessBinary(self, data): + ix = 0 + self.temp = LowLevelMessages.ReceiveFloat16(data, [ix]) + for watcher in self._watchers: + watcher(self.temp) diff --git a/SiteServer.py b/SiteServer.py index 3635347..861ee95 100644 --- a/SiteServer.py +++ b/SiteServer.py @@ -1,6 +1,6 @@ -from Participant import Participant -from ClientMsg import ClientMsg -from NetworkIdMsg import NetworkIdMsg +from .Participant import Participant +from .ClientMsg import ClientMsg +from .NetworkIdMsg import NetworkIdMsg import socket import threading @@ -19,7 +19,7 @@ class SiteServer(Participant): if remote == False: 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", 7681)) + self.udp_socket.bind(("0.0.0.0", port)) self.AddParticipant(self.ip_address, self.port) self.thread = threading.Thread(target = self.Receiver) diff --git a/Spherical.py b/Spherical.py index 75fe54a..6701afd 100644 --- a/Spherical.py +++ b/Spherical.py @@ -1,5 +1,5 @@ import math -from Direction import Direction +from .Direction import Direction class Spherical: def __init__(self, distance, direction): diff --git a/SwingTwist.py b/SwingTwist.py index 5e0c893..e312b15 100644 --- a/SwingTwist.py +++ b/SwingTwist.py @@ -1,5 +1,5 @@ -from Direction import Direction -from Quaternion import Quaternion +from .Direction import Direction +from .Quaternion import Quaternion class SwingTwist: def __init__(self, swing: Direction, twist: float): diff --git a/Thing.py b/Thing.py index 69a0a51..a01b6ce 100644 --- a/Thing.py +++ b/Thing.py @@ -1,5 +1,5 @@ -from Spherical import Spherical -from Quaternion import Quaternion +from .Spherical import Spherical +from .Quaternion import Quaternion ## A thing is the basic building block # @@ -10,6 +10,7 @@ class Thing: Switch = 0x01 DistanceSensor = 0x02 DirectionalSensor = 0x03 + TemperatureSensor = 0x04 Animator = 0x40 Position = 0x01 @@ -17,9 +18,9 @@ class Thing: LinearVelocity = 0x04 AngularVelocity = 0x08 - def __init__(self, type=Type.Undetermined, parent=None, name=None): - self.networkId = 0 - self.id = 0 + def __init__(self, network_id = 0, thing_id = 0, type=Type.Undetermined, parent=None, name=None): + self.network_id = network_id + self.id = thing_id self.type = type if parent is None: self.parent_id = 0 @@ -42,6 +43,7 @@ class Thing: pass def ProcessBinary(self, data): + print('default binary processor') pass allThings = set({ None }) @@ -52,16 +54,16 @@ class Thing: Thing.allThings.add(thing) @staticmethod - def Get(networkId, thingId): + def Get(network_id, thing_id): for thing in Thing.allThings: if thing is not None: - if thing.networkId == networkId and thing.id == thingId: + if thing.network_id == network_id and thing.id == thing_id: return thing return None ## Update all things @staticmethod def UpdateAll(currentTime): - for thing in Thing.allThings: + for thing in list(Thing.allThings): if thing is not None: thing.update(currentTime) diff --git a/ThingMsg.py b/ThingMsg.py index b97f597..eb72008 100644 --- a/ThingMsg.py +++ b/ThingMsg.py @@ -1,4 +1,4 @@ -from Messages import IMessage +from .Messages import IMessage class ThingMsg(IMessage): id = 0x80