diff --git a/Examples/BB2B_networking.py b/Examples/BB2B_networking.py index c384a40..e36fba5 100644 --- a/Examples/BB2B_networking.py +++ b/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) diff --git a/LocalParticipant.py b/LocalParticipant.py index 0a0c1e4..7a90c13 100644 --- a/LocalParticipant.py +++ b/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): diff --git a/Messages/PoseMsg.py b/Messages/PoseMsg.py index a23ec51..95a13d4 100644 --- a/Messages/PoseMsg.py +++ b/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: - LowLevelMessages.SendSpherical(buffer, ix, self.thing.position) - if self.thing.pose_updated & Thing.Orientation: - LowLevelMessages.SendQuat32(buffer, ix, self.thing.orientation) - if self.thing.pose_updated & Thing.LinearVelocity: - LowLevelMessages.SendSpherical(buffer, ix, self.thing.linearVelocity) - if self.thing.pose_updated & Thing.AngularVelocity: - LowLevelMessages.SendSpherical(buffer, ix, self.thing.angularVelocity) - return ix[0] + if self.pose_type & Thing.Position: + Messages.LowLevelMessages.SendSpherical(buffer, ix, self.thing.position) + if self.pose_type & Thing.Orientation: + Messages.LowLevelMessages.SendQuat32(buffer, ix, self.thing.orientation) + if self.pose_type & Thing.LinearVelocity: + Messages.LowLevelMessages.SendSpherical(buffer, ix, self.thing.linear_velocity) + if self.pose_type & Thing.AngularVelocity: + Messages.LowLevelMessages.SendSpherical(buffer, ix, self.thing.angular_velocity) + return PoseMsg.length + ix[0] diff --git a/Participant.py b/Participant.py index 8e361c3..bae7f2f 100644 --- a/Participant.py +++ b/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) diff --git a/Thing.py b/Thing.py index e095fd8..fd3564a 100644 --- a/Thing.py +++ b/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 }) - - # @staticmethod - # def Add(thing): - # thing.id = len(Thing.allThings) - # Thing.allThings.add(thing) + 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 Get(network_id, thing_id): - # 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 - # @staticmethod - # def UpdateAll(currentTime): - # for thing in list(Thing.allThings): - # if thing is not None: - # thing.update(currentTime) + def AddChild(self, child): + if child in self.children: + return + + child.parent = self + self.children.append(child) + + def RemoveChild(self, child): + self.children.remove(child) \ No newline at end of file diff --git a/Things/DifferentialDrive.py b/Things/DifferentialDrive.py index dc74f1d..585c5b0 100644 --- a/Things/DifferentialDrive.py +++ b/Things/DifferentialDrive.py @@ -51,6 +51,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): """! @@ -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) +