WIP networking

Examples/BB2B_networking.py

@@ -12,12 +12,16 @@ import time
 participant = LocalParticipant(port=7681, local_port=7682)
 # The robot's propulsion is a differential drive
 bb2b = DifferentialDrive(participant)
+bb2b.name = "BB2B"
 bb2b.model_url = "https://passer.life/extras/ant1_transparent.png"
+bb2b.SetDriveDimensions(0.064, 0.128)
 
 # It has a touch sensor at the front left of the roboid
 touch_left = TouchSensor(parent=bb2b)
+touch_left.name = "Touch left"
 # and other one on the right
 touch_right = TouchSensor(parent=bb2b)
+touch_right.name = "Touch right"
 
 # Do forever:
 while True:
@@ -34,4 +38,4 @@ while True:
     # Update the roboid state
     participant.Update()
     # and sleep for 100ms
-    time.sleep(100)
+    time.sleep(1.0)

LocalParticipant.py

@@ -53,7 +53,7 @@ class LocalParticipant(Participant):
             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", self.local_port))
-            self.AddParticipant(self.ip_address, self.port)
+            #self.AddParticipant(self.ip_address, self.port)
 
             self.thread = threading.Thread(target = self.Receiver)
             self.thread.daemon = True
@@ -74,7 +74,7 @@ class LocalParticipant(Participant):
         return participant
 
     def AddParticipant(self, ip_address, port):
-        # print(f'{self.name} Add participant {ip_address} {port}')
+        print(f'{self.name} Add participant {ip_address} {port}')
         remote_participant = Participant(ip_address = ip_address, port = port)
         remote_participant.network_id = len(self.others)
         self.others.append(remote_participant)
@@ -89,17 +89,28 @@ class LocalParticipant(Participant):
             print(f'Publish ClientMsg {self.network_id}')
             self.nextPublishMe = currentTimeMs + self.publishInterval
 
+        for thing in self.things:
+            thing.Update(currentTimeMs)
+            pose_msg = PoseMsg(self.network_id, thing)
+            for other in self.others:
+                self.Send(other, pose_msg)
+
         super().Update(currentTimeMs)
 
 #endregion
 
 #region Send
 
-    def SendThingInfo(self, owner, thing):
+    def SendThingInfo(self, owner, thing, recursively: bool = False):
         self.Send(owner, ThingMsg(self.network_id, thing))
         self.Send(owner, NameMsg(self.network_id, thing))
         self.Send(owner, ModelUrlMsg(self.network_id, thing))
 
+        if recursively:
+            for child in thing.children:
+                self.SendThingInfo(owner, child, recursively)
+
+
     def Send(self, owner, msg):
         buffer_size = msg.Serialize([self.buffer])
         if buffer_size <= 0:
@@ -163,7 +174,8 @@ class LocalParticipant(Participant):
             self.network_id = msg.network_id
             print(f'sending all things {len(self.things)}')
             for thing in self.things:
-                self.SendThingInfo(sender, thing)
+                if thing.parent is None:
+                    self.SendThingInfo(sender, thing, recursively=True)
                 # self.Send(NameMsg(self.network_id, thing))
 
     def ProcessInvestigateMsg(self, data: bytearray):

Messages/PoseMsg.py

@@ -1,14 +1,34 @@
+import Messages.LowLevelMessages
+from Thing import Thing
 
 class PoseMsg():
     id = 0x10
     length = 4
 
-    def __init__(self, network_id, thing):
+    Pose_Position = 0x01
+    Pose_Orientation = 0x02    
+    Pose_LinearVelocity = 0x04
+    Pose_AngularVelocity = 0x08
+
+    def __init__(self, network_id, thing, force: bool = False):
         self.network_id = network_id
-        self.thing = thing
+        self.thing: Thing = thing
+        self.pose_type = 0
+        if thing.position_updated or force:
+            self.pose_type |= Thing.Position
+            thing.position_updated = False
+        if thing.orientation_updated or force:
+            self.pose_type |= Thing.Orientation
+            thing.orientation_updated = False
+        if thing.linear_velocity_updated:
+            self.pose_type |= Thing.LinearVelocity
+            thing.linear_velocity_updated = False
+        if thing.angular_velocity_updated:
+            self.pose_type |= Thing.AngularVelocity
+            thing.angular_velocity_updated = False
 
     def Serialize(self, buffer_ref):
-        if (self.network_id is None) or (self.thing is None):
+        if (self.network_id is None) or (self.thing is None) or (self.pose_type == 0):
             return 0
         
         buffer: bytearray = buffer_ref[0]
@@ -16,15 +36,15 @@ class PoseMsg():
             PoseMsg.id,
             self.network_id,
             self.thing.id,
-            self.thing.pose_updated
+            self.pose_type
         ]
         ix = [4]
-        if self.thing.pose_updated & Thing.Position:
+        if self.pose_type & Thing.Position:
-            LowLevelMessages.SendSpherical(buffer, ix, self.thing.position)
+            Messages.LowLevelMessages.SendSpherical(buffer, ix, self.thing.position)
-        if self.thing.pose_updated & Thing.Orientation:
+        if self.pose_type & Thing.Orientation:
-            LowLevelMessages.SendQuat32(buffer, ix, self.thing.orientation)
+            Messages.LowLevelMessages.SendQuat32(buffer, ix, self.thing.orientation)
-        if self.thing.pose_updated & Thing.LinearVelocity:
+        if self.pose_type & Thing.LinearVelocity:
-            LowLevelMessages.SendSpherical(buffer, ix, self.thing.linearVelocity)
+            Messages.LowLevelMessages.SendSpherical(buffer, ix, self.thing.linear_velocity)
-        if self.thing.pose_updated & Thing.AngularVelocity:
+        if self.pose_type & Thing.AngularVelocity:
-            LowLevelMessages.SendSpherical(buffer, ix, self.thing.angularVelocity)
+            Messages.LowLevelMessages.SendSpherical(buffer, ix, self.thing.angular_velocity)
-        return ix[0]
+        return PoseMsg.length + ix[0]

Participant.py

@@ -19,7 +19,7 @@ class Participant:
         self.networkId = 0
 
         ## The things managed by this participant
-        self.things = set({ None })
+        self.things = set()
 
     def Add(self, thing, check_id = True):
         """! Add a new thing for this participant. 
@@ -27,7 +27,7 @@ class Participant:
         @param check_id When true, the thing ID of the thing is checked. If it is 0, a new thing Id will be assigned.
         """
         if check_id and thing.id == 0:
-            thing.id = len(self.things)
+            thing.id = len(self.things) + 1
             self.things.add(thing)
         else:
             found_thing = self.Get(thing.network_id, thing.id)
@@ -58,4 +58,4 @@ class Participant:
     def Update(self, currentTimeMs):
         for thing in list(self.things):
             if thing is not None:
-                thing.update(currentTimeMs)
+                thing.Update(currentTimeMs)

Thing.py

@@ -26,7 +26,8 @@ class Thing:
         self.parent = None
         if parent is not None:
             owner = parent.owner
-            self.parent = parent
+            self.SetParent(parent)
+        self.children = []
 
         ## The participant owning this thing
         self.owner = owner
@@ -45,41 +46,54 @@ class Thing:
 
         ## The position of the thing in local space, in meters
         self.position: Spherical = Spherical.zero
+        self.position_updated: bool = False
         ## The new orientation in local space
         self.orientation: Quaternion = Quaternion.identity
+        self.orientation_updated: bool = False
         ## The linear velocity of the thing in local space, in meters per second
         self.linear_velocity: Spherical = Spherical.zero
+        self.linear_velocity_updated: bool = False
         ## The angular velocity of the thing in local space, in degrees per second
         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.owner.Add(self)
     
-    def update(self, currentTime):
+    def SetPosition(self, position):
+        self.position = position
+        self.position_updated = True
+
+    def SetLinearVelocity(self, linear_velocity):
+        self.linear_velocity = linear_velocity
+        self.linear_velocity_updated = True
+    
+    def SetAngularVelocity(self, angular_velocity):
+        self.angular_velocity = angular_velocity
+        self.angular_velocity_updated = True
+
+    def Update(self, currentTime):
         pass
 
     def ProcessBinary(self, data):
         print('default binary processor')
         pass
 
-    # allThings = set({ None })
+    def SetParent(self, parent):
+        if parent is None:
+            parentThing = self.parent
+            if parentThing is not None:
+                parentThing.RemoveChild(self)
+            self.parent = None
+        else:
+            parent.AddChild(self)
     
-    # @staticmethod
+    def AddChild(self, child):
-    # def Add(thing):
+        if child in self.children:
-    #     thing.id = len(Thing.allThings)
+            return
-    #     Thing.allThings.add(thing)
         
-    # @staticmethod
+        child.parent = self
-    # def Get(network_id, thing_id):
+        self.children.append(child)
-    #     for thing in Thing.allThings:
-    #         if thing is not None:
-    #             if thing.network_id == network_id and thing.id == thing_id:
-    #                 return thing
-    #     return None
     
-    # ## Update all things
+    def RemoveChild(self, child):
-    # @staticmethod
+        self.children.remove(child)
-    # def UpdateAll(currentTime):
-    #     for thing in list(Thing.allThings):
-    #         if thing is not None:
-    #             thing.update(currentTime)

Things/DifferentialDrive.py

@@ -52,6 +52,11 @@ class DifferentialDrive(Thing):
         if self.wheel_right is not None:
             self.wheel_right.SetAngularVelocity(Spherical(speed_right, Direction.Right))
         
+        speed = self.wheel_radius * (speed_left + speed_right) / 2 * Angle.Deg2Rad
+        self.SetLinearVelocity(Spherical(speed, Direction.forward))
+        steer = self.wheel_radius * (speed_left - speed_right) / self.wheel_separation
+        #self.SetAngularVelocity(Spherical(steer, Direction.up))
+
     def Update(self, currentTimeMs, recursive = True):
         """!
         @copydoc Thing::Update
@@ -65,6 +70,9 @@ class DifferentialDrive(Thing):
         if angular_velocity.direction.horizontal < 0:
             angular_speed = -angular_speed
         
+        if self.wheel_left is None or self.wheel_right is None:
+            return
+        
         speed_left: float = 0 if self.wheel_left is None else \
             (linear_velocity + angular_speed * self.wheel_left.position.distance) / self.wheel_radius * Angle.Rad2Deg
         speed_right: float = 0 if self.wheel_right is None else \
@@ -72,4 +80,6 @@ class DifferentialDrive(Thing):
 
         self.SetWheelVelocity(speed_left, speed_right)
 
+        super().Update(currentTimeMs)
+