From ccffbb181176c117d3da73f78dc73dbb7e5a3ec6 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Thu, 24 Apr 2025 17:21:32 +0200 Subject: [PATCH] BB2B_networking is working --- Examples/BB2B_networking.py | 4 +--- Messages/BinaryMsg.py | 12 ++++++++---- Messages/ParticipantMsg.py | 6 +++--- Messages/PoseMsg.py | 9 ++------- ParticipantUDP.py | 8 +++----- Thing.py | 28 +++++++++++++++++++--------- Things/TouchSensor.py | 4 +++- 7 files changed, 39 insertions(+), 32 deletions(-) diff --git a/Examples/BB2B_networking.py b/Examples/BB2B_networking.py index 34221db..b0ba69d 100644 --- a/Examples/BB2B_networking.py +++ b/Examples/BB2B_networking.py @@ -44,10 +44,8 @@ while True: # When both sides are touching something, both wheels will turn backward # and the roboid will move backwards bb2b.SetWheelVelocity(wheel_speed_left, wheel_speed_right) - if touch_left.touched_something or touch_right.touched_something: - print(f'{wheel_speed_left} {wheel_speed_right} {bb2b.linear_velocity.distance} {bb2b.angular_velocity.distance}') # Update the roboid state participant.Update() # and sleep for 100ms - time.sleep(1) + time.sleep(0.1) diff --git a/Messages/BinaryMsg.py b/Messages/BinaryMsg.py index 894f5c5..9f45d1c 100644 --- a/Messages/BinaryMsg.py +++ b/Messages/BinaryMsg.py @@ -2,12 +2,13 @@ class BinaryMsg(): id = 0xB1 - length = 3 + length = 4 def __init__(self, data, thing = None): if isinstance(data, bytes): self.thing_id = data[2] - self.data = data[3:] + self.data_length = data[3] + self.data = data[4:] else: self.network_id = data self.thing_id = thing.id @@ -19,11 +20,14 @@ class BinaryMsg(): buffer: bytearray = buffer_ref[0] ix = self.length - self.thing.GenerateBinary(buffer, {ix}) + self.data_length = self.thing.GenerateBinary(buffer, {ix}) if ix <= self.length: return 0 + print(f'Send BinaryMsg [{self.network_id}/{self.thing_id}] {self.thing_type} {self.parent_id}') + buffer[0] = self.id - buffer[1] = 0 # network_id + buffer[1] = self.network_id buffer[2] = self.thing_id + buffer[3] = self.data_length return ix diff --git a/Messages/ParticipantMsg.py b/Messages/ParticipantMsg.py index 12dbc05..b785569 100644 --- a/Messages/ParticipantMsg.py +++ b/Messages/ParticipantMsg.py @@ -14,7 +14,7 @@ class ParticipantMsg(IMessage): def __init__(self, data = None): """! Create a new message for sending """ - pass + self.network_id = data def Serialize(self, buffer_ref): """! Serialize the message into a byte array. @@ -24,11 +24,11 @@ class ParticipantMsg(IMessage): if buffer_ref is None: return 0 - print(f'Send ParticipantMsg [0]') + print(f'Send ParticipantMsg [{self.network_id}]') buffer: bytearray = buffer_ref[0] buffer[0:ParticipantMsg.length] = [ ParticipantMsg.id, - 0, # network_id + self.network_id ] return ParticipantMsg.length diff --git a/Messages/PoseMsg.py b/Messages/PoseMsg.py index ac0ba78..4b64ed5 100644 --- a/Messages/PoseMsg.py +++ b/Messages/PoseMsg.py @@ -17,21 +17,16 @@ class PoseMsg(): 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: + if thing.linear_velocity_updated or force: self.pose_type |= PoseMsg.LinearVelocity - thing.linear_velocity_updated = False - if thing.angular_velocity_updated: + if thing.angular_velocity_updated or force: 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: diff --git a/ParticipantUDP.py b/ParticipantUDP.py index f0c07bd..acb0cf2 100644 --- a/ParticipantUDP.py +++ b/ParticipantUDP.py @@ -103,10 +103,6 @@ class ParticipantUDP(Participant): self.nextPublishMe = currentTimeMs + self.publishInterval self.UpdateMyThings(currentTimeMs) - # for thing in self.things: - # thing.Update(currentTimeMs) - - # super().Update(currentTimeMs) def UpdateMyThings(self, currentTimeMs): for thing in self.things: @@ -116,6 +112,8 @@ class ParticipantUDP(Participant): # if thing.hierarchyChanged and not (self.isIsolated or self.network_id == ): # thingMsg = ThingMsg(self.network_id, thing) # self.Send(self.remote_site, thingMsg) + poseMsg = PoseMsg(thing.owner.network_id, thing) + self.Send(self.remote_site, poseMsg) thing.Update(currentTimeMs, False) if not(self.is_isolated or self.network_id == 0): @@ -139,7 +137,7 @@ class ParticipantUDP(Participant): 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, PoseMsg(self.network_id, thing, True)) self.Send(owner, BinaryMsg(self.network_id, thing)) if recursively: diff --git a/Thing.py b/Thing.py index ea5c14a..63ba405 100644 --- a/Thing.py +++ b/Thing.py @@ -69,7 +69,7 @@ class Thing: self.angular_velocity: Spherical = Spherical.zero self.angular_velocity_updated: bool = False - self.pose_updated = 0x00 # the bits indicate which fields have been updated + #self.pose_updated = 0x00 # the bits indicate which fields have been updated self.owner.Add(self) def SetPosition(self, position): @@ -90,21 +90,31 @@ class Thing: """! Set the linear velocity of the thing @param linearVelocity The new linear velocity in local space, in meters per second """ + if self.linear_velocity != linear_velocity: + self.linear_velocity_updated = True + self.linear_velocity = linear_velocity - self.linear_velocity_updated = True def SetAngularVelocity(self, angular_velocity): """! Set the angular velocity of the thing @param angularVelocity the new angular velocity in local space """ + if self.angular_velocity != angular_velocity: + self.angular_velocity_updated = True + self.angular_velocity = angular_velocity - self.angular_velocity_updated = True 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 + self.position_updated = False + self.orientation_updated = False + self.linear_velocity_updated = False + self.angular_velocity_updated = False + + if recurse: + for child in self.children: + if child is None: + continue + child.Update(currentTime, recurse) def SetParent(self, parent): if parent is None: @@ -125,8 +135,8 @@ class Thing: def RemoveChild(self, child): self.children.remove(child) - def GenerateBinary(self, buffer, ix_ref): - pass + def GenerateBinary(self, buffer, ix_ref) -> int: + return 0 def ProcessBinary(self, data): print('default binary processor') diff --git a/Things/TouchSensor.py b/Things/TouchSensor.py index d612b84..166ca7e 100644 --- a/Things/TouchSensor.py +++ b/Things/TouchSensor.py @@ -16,4 +16,6 @@ class TouchSensor(Thing): """ self.touched_something = bytes[0] == 1 if self.touched_something: - print("touched something!") \ No newline at end of file + print(f"{self.name} touched something!") + else: + print(f"{self.name} touching nothing") \ No newline at end of file