BB2B_networking is working

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)

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

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

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:

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:

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')

Things/TouchSensor.py

@@ -16,4 +16,6 @@ class TouchSensor(Thing):
         """
         self.touched_something = bytes[0] == 1
         if self.touched_something:
-            print("touched something!")
+            print(f"{self.name} touched something!")
+        else:
+            print(f"{self.name} touching nothing")