RoboidControl/RoboidControl-python
RoboidControl/RoboidControl-python
1
0
Fork 0
Code Issues 1 Pull Requests Actions Packages Releases Activity

WIP networking

Browse Source
This commit is contained in:
Pascal Serrarens 2025-03-12 17:51:59 +01:00
parent a19e0c3c19
commit a252ff5b33
6 changed files with 103 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
Examples/BB2B_networking.py
View File

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

20
LocalParticipant.py
View File

@ -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):

46
Messages/PoseMsg.py
View File

@ -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]

6
Participant.py
View File

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

58
Thing.py
View File

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

10
Things/DifferentialDrive.py
View File

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