From e53a01bb508f1ee132eaa6fd0698b15bf97678f8 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Thu, 24 Apr 2025 15:53:21 +0200 Subject: [PATCH] Updates to comply with C++/C# --- Examples/BB2B_networking.py | 7 +-- Messages/BinaryMsg.py | 1 + Messages/ModelUrlMsg.py | 1 + Messages/NameMsg.py | 1 + Messages/NetworkIdMsg.py | 11 +++-- Messages/ParticipantMsg.py | 2 + Messages/PoseMsg.py | 41 ++++++++++------ Messages/ThingMsg.py | 17 ++++--- Participant.py | 3 ++ ParticipantUDP.py | 97 ++++++++++++++++++++++++------------- Thing.py | 9 ++-- 11 files changed, 122 insertions(+), 68 deletions(-) diff --git a/Examples/BB2B_networking.py b/Examples/BB2B_networking.py index c65f749..34221db 100644 --- a/Examples/BB2B_networking.py +++ b/Examples/BB2B_networking.py @@ -11,15 +11,16 @@ from Things.TouchSensor import TouchSensor # Create a local participant for handling communcation # using default settings (UDP communciation over port 7681) participant = ParticipantUDP(port=7681, local_port=7682, ip_address="127.0.0.1") + # The robot's propulsion is a differential drive bb2b = DifferentialDrive(participant) bb2b.name = "BB2B" bb2b.SetDriveDimensions(0.064, 0.128) bb2b.SetOrientation(SwingTwist.Degrees(40, 0, 0)) -bb2b.SetPosition(Spherical(0.15, Direction.up)) +# bb2b.SetPosition(Spherical(0.15, Direction.up)) model = Thing(parent=bb2b) -model.model_url = "https://passer.life/extras/Roller1anim1.png" +model.model_url = "https://passer.life/extras/Roller2anim1.png" model.SetOrientation(SwingTwist.Degrees(90, 0, 0)) # It has a touch sensor at the front left of the roboid @@ -49,4 +50,4 @@ while True: # Update the roboid state participant.Update() # and sleep for 100ms - time.sleep(0.1) + time.sleep(1) diff --git a/Messages/BinaryMsg.py b/Messages/BinaryMsg.py index 1ef0fcb..894f5c5 100644 --- a/Messages/BinaryMsg.py +++ b/Messages/BinaryMsg.py @@ -9,6 +9,7 @@ class BinaryMsg(): self.thing_id = data[2] self.data = data[3:] else: + self.network_id = data self.thing_id = thing.id self.thing = thing diff --git a/Messages/ModelUrlMsg.py b/Messages/ModelUrlMsg.py index e8cb031..bb2ed7e 100644 --- a/Messages/ModelUrlMsg.py +++ b/Messages/ModelUrlMsg.py @@ -14,6 +14,7 @@ class ModelUrlMsg(IMessage): self.url = data[ModelUrlMsg.length:].decode("utf-8") else: if thing is not None: + self.network_id = data self.thing_id = thing.id self.url = thing.model_url diff --git a/Messages/NameMsg.py b/Messages/NameMsg.py index cc6b9d2..bf410cd 100644 --- a/Messages/NameMsg.py +++ b/Messages/NameMsg.py @@ -14,6 +14,7 @@ class NameMsg(IMessage): self.name = data[NameMsg.length:].decode("utf-8") else: if thing is not None: + self.network_id = data self.thing_id = thing.id self.name = thing.name diff --git a/Messages/NetworkIdMsg.py b/Messages/NetworkIdMsg.py index 6e30d93..0b23213 100644 --- a/Messages/NetworkIdMsg.py +++ b/Messages/NetworkIdMsg.py @@ -6,11 +6,14 @@ from Messages.Messages import IMessage class NetworkIdMsg(IMessage): id = 0xA1 length = 2 - ## Create a network id message - def __init__(self, data = None): - pass + def __init__(self, arg1 = None): + if isinstance(arg1, bytes): + buffer = arg1 + self.network_id = buffer[1] + else: + self.network_id = arg1 ## Serialize the message into the given buffer # @@ -20,7 +23,7 @@ class NetworkIdMsg(IMessage): buffer: bytearray = buffer_ref[0] buffer[0:NetworkIdMsg.length] = [ NetworkIdMsg.id, - 0 + self.network_id ] return NetworkIdMsg.length diff --git a/Messages/ParticipantMsg.py b/Messages/ParticipantMsg.py index 29f4368..12dbc05 100644 --- a/Messages/ParticipantMsg.py +++ b/Messages/ParticipantMsg.py @@ -24,6 +24,8 @@ class ParticipantMsg(IMessage): if buffer_ref is None: return 0 + print(f'Send ParticipantMsg [0]') + buffer: bytearray = buffer_ref[0] buffer[0:ParticipantMsg.length] = [ ParticipantMsg.id, diff --git a/Messages/PoseMsg.py b/Messages/PoseMsg.py index 86cd4f3..ac0ba78 100644 --- a/Messages/PoseMsg.py +++ b/Messages/PoseMsg.py @@ -10,30 +10,39 @@ class PoseMsg(): LinearVelocity = 0x04 AngularVelocity = 0x08 - def __init__(self, thing, force: bool = False): - self.thing = thing - self.pose_type = 0 - if thing.position_updated or force: - self.pose_type |= PoseMsg.Position - thing.position_updated = False - if thing.orientation_updated or force: - self.pose_type |= PoseMsg.Orientation - thing.orientation_updated = False - if thing.linear_velocity_updated: - self.pose_type |= PoseMsg.LinearVelocity - thing.linear_velocity_updated = False - if thing.angular_velocity_updated: - self.pose_type |= PoseMsg.AngularVelocity - thing.angular_velocity_updated = False + def __init__(self, arg1, thing, force: bool = False): + if isinstance(arg1, bytes): + self.thing_id = arg1[2] + self.data = arg1[3:] + else: + self.network_id = arg1 + self.thing = thing + self.network_id = 0 + + self.pose_type = 0 + if thing.position_updated or force: + self.pose_type |= PoseMsg.Position + thing.position_updated = False + if thing.orientation_updated or force: + self.pose_type |= PoseMsg.Orientation + thing.orientation_updated = False + if thing.linear_velocity_updated: + self.pose_type |= PoseMsg.LinearVelocity + thing.linear_velocity_updated = False + if thing.angular_velocity_updated: + self.pose_type |= PoseMsg.AngularVelocity + thing.angular_velocity_updated = False def Serialize(self, buffer_ref): if self.thing is None or self.pose_type == 0: return 0 + print(f'Send PoseMsg [{self.network_id}/{self.thing.id}] {self.pose_type}') + buffer: bytearray = buffer_ref[0] buffer[0:PoseMsg.length] = [ PoseMsg.id, - 0, # network_id, + self.network_id, self.thing.id, self.pose_type ] diff --git a/Messages/ThingMsg.py b/Messages/ThingMsg.py index 0222521..5338c5f 100644 --- a/Messages/ThingMsg.py +++ b/Messages/ThingMsg.py @@ -8,13 +8,16 @@ class ThingMsg(IMessage): thing_type = None parent_id = None - def __init__(self, data, thing=None): - if isinstance(data, bytes): - self.thing_id = data[2] - self.thing_type = data[3] - self.parent_id = data[4] + def __init__(self, arg1, thing=None): + if isinstance(arg1, bytes): + buffer = arg1 + self.network_id = buffer[1] + self.thing_id = buffer[2] + self.thing_type = buffer[3] + self.parent_id = buffer[4] else: if thing is not None: + self.network_id = arg1 self.thing_id = thing.id self.thing_type = thing.type if thing.parent is not None: @@ -26,10 +29,12 @@ class ThingMsg(IMessage): if self.thing_id is None: return 0 + print(f'Send ThingMsg [{self.network_id}/{self.thing_id}] {self.thing_type} {self.parent_id}') + buffer: bytearray = buffer_ref[0] buffer[0:ThingMsg.length] = [ ThingMsg.id, - 0, # network_id, + self.network_id, self.thing_id, self.thing_type, self.parent_id diff --git a/Participant.py b/Participant.py index be34f40..9943b4d 100644 --- a/Participant.py +++ b/Participant.py @@ -15,6 +15,9 @@ class Participant: ## The port number for UDP communication with the participant. This is 0 for isolated participants. self.port = port + ## The network ID of the participant + self.network_id =0 + ## The things managed by this participant self.things = set() diff --git a/ParticipantUDP.py b/ParticipantUDP.py index 00bce1f..f0c07bd 100644 --- a/ParticipantUDP.py +++ b/ParticipantUDP.py @@ -42,19 +42,19 @@ class ParticipantUDP(Participant): ## True if the participant is running isolated. # Isolated participants do not communicate with other participants - self.isolated = True + self.is_isolated = True + self.remote_site = None ## The other participants communicating with this participant self.others = [] if self.port != 0: - self.isolated = False + self.is_isolated = False if ip_address is not None: - self.others.append(Participant(ip_address, port)) + self.remote_site = Participant(ip_address, port) + self.others.append(self.remote_site) self.buffer = bytearray(256) - # self.thing_msg_processors = {} - # self.new_thing_handlers = [] self.publishInterval = 3000 # 3 seconds self.udp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) @@ -92,33 +92,55 @@ class ParticipantUDP(Participant): if currentTimeMs is None: currentTimeMs = time.time() * 1000 - if self.isolated == False: + if self.is_isolated == False: if self.publishInterval > 0 and currentTimeMs > self.nextPublishMe: - msg = ParticipantMsg() - if self.others.count == 0: + msg = ParticipantMsg(self.network_id) + if self.remote_site == None: self.Publish(msg) else: - for other_participant in self.others: - self.Send(other_participant, msg) + self.Send(self.remote_site, msg) - print(f'Publish ParticipantMsg') self.nextPublishMe = currentTimeMs + self.publishInterval - for thing in self.things: - thing.Update(currentTimeMs) + self.UpdateMyThings(currentTimeMs) + # for thing in self.things: + # thing.Update(currentTimeMs) - super().Update(currentTimeMs) + # super().Update(currentTimeMs) + + def UpdateMyThings(self, currentTimeMs): + for thing in self.things: + if thing is None: + continue + + # if thing.hierarchyChanged and not (self.isIsolated or self.network_id == ): + # thingMsg = ThingMsg(self.network_id, thing) + # self.Send(self.remote_site, thingMsg) + + thing.Update(currentTimeMs, False) + if not(self.is_isolated or self.network_id == 0): + # if thing.terminate: + # destroyMsg = DestroyMsg(self.network_id, thing) + # self.Send(self.remote_site, destroyMsg) + # else: + # Send to remote site + poseMsg = PoseMsg(thing.owner.network_id, thing) + self.Send(self.remote_site, poseMsg) + binaryMsg = BinaryMsg(thing.owner.network_id, thing) + self.Send(self.remote_site, binaryMsg) + # if thing.terminate: + # self.Remove(thing) #endregion #region Send def SendThingInfo(self, owner, thing, recursively: bool = False): - self.Send(owner, ThingMsg(thing)) - self.Send(owner, NameMsg(thing)) - self.Send(owner, ModelUrlMsg(thing)) - self.Send(owner, PoseMsg(thing, True)) - self.Send(owner, BinaryMsg(thing)) + self.Send(owner, ThingMsg(self.network_id, thing)) + self.Send(owner, NameMsg(self.network_id, thing)) + self.Send(owner, ModelUrlMsg(self.network_id, thing)) + #self.Send(owner, PoseMsg(self.network_id, thing, True)) + self.Send(owner, BinaryMsg(self.network_id, thing)) if recursively: for child in thing.children: @@ -130,7 +152,7 @@ class ParticipantUDP(Participant): if buffer_size <= 0: return True - print(f'{self.name} send {self.buffer[0]} to {owner.ip_address} {owner.port}') + # print(f'{self.name} send {self.buffer[0]} to {owner.ip_address} {owner.port}') self.udp_socket.sendto(self.buffer[:buffer_size], (owner.ip_address, owner.port)) return True @@ -149,15 +171,19 @@ class ParticipantUDP(Participant): def Receiver(self): while True: - data, addr = self.udp_socket.recvfrom(1024) - remote_ip_address = addr[0] - remote_port = addr[1] - # print(f'msg received from {remote_ip_address}:{remote_port}') - remote_participant = self.GetParticipant(remote_ip_address, remote_port) - if remote_participant is None: - # print(f'new participant') - remote_participant = self.AddParticipant(remote_ip_address, remote_port) - self.ReceiveData(data, remote_participant) + try: + data, addr = self.udp_socket.recvfrom(1024) + remote_ip_address = addr[0] + remote_port = addr[1] + # print(f'msg received from {remote_ip_address}:{remote_port}') + remote_participant = self.GetParticipant(remote_ip_address, remote_port) + if remote_participant is None: + # print(f'new participant') + remote_participant = self.AddParticipant(remote_ip_address, remote_port) + self.ReceiveData(data, remote_participant) + except ConnectionError: + self.network_id = 0 + pass def ReceiveData(self, data, sender): msgId = data[0] @@ -166,7 +192,7 @@ class ParticipantUDP(Participant): case ParticipantMsg.id: self.ProcessParticipantMsg(sender, ParticipantMsg(data)) case NetworkIdMsg.id: - self.ProcessNetworkIdMsg(sender, NetworkIdMsg(data)) + self.ProcessSiteIdMsg(sender, NetworkIdMsg(data)) # case InvestigateMsg.id: # self.ProcessInvestigateMsg(InvestigateMsg(data)) case ThingMsg.id: @@ -181,11 +207,12 @@ class ParticipantUDP(Participant): def ProcessParticipantMsg(self, sender, msg: ParticipantMsg): pass - def ProcessNetworkIdMsg(self, sender, msg: NetworkIdMsg): - print(f'{self.name} receive network id') - print(f'sending all things {len(self.things)}') - for thing in self.things: - if thing.parent is None: + def ProcessSiteIdMsg(self, sender, msg: NetworkIdMsg): + print(f'{self.name} Process SiteMsg {self.network_id} -> {msg.network_id}') + if self.network_id != msg.network_id: + self.network_id = msg.network_id + for thing in self.things: + #if thing.parent is None: self.SendThingInfo(sender, thing, recursively=True) def ProcessInvestigateMsg(self, data: bytearray): diff --git a/Thing.py b/Thing.py index 802e217..ea5c14a 100644 --- a/Thing.py +++ b/Thing.py @@ -100,10 +100,11 @@ class Thing: self.angular_velocity = angular_velocity self.angular_velocity_updated = True - def Update(self, currentTime): - pose_msg = PoseMsg(self) - for other in self.owner.others: - self.owner.Send(other, pose_msg) + def Update(self, currentTime, recurse = False): + # pose_msg = PoseMsg(self.owner.network_id, self) + # for other in self.owner.others: + # self.owner.Send(other, pose_msg) + pass def SetParent(self, parent): if parent is None: