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

More compatiblity with C#

Browse Source
This commit is contained in:
Pascal Serrarens 2025-03-12 15:49:59 +01:00
parent 26678c4ab1
commit a19e0c3c19
6 changed files with 48 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
Examples/BB2B_networking.py
View File

@ -1,3 +1,7 @@
import sys
from os import path
sys.path.append(path.abspath(path.join(path.dirname(__file__), '..')))
from LocalParticipant import LocalParticipant from LocalParticipant import LocalParticipant
from Things.DifferentialDrive import DifferentialDrive from Things.DifferentialDrive import DifferentialDrive
from Things.TouchSensor import TouchSensor from Things.TouchSensor import TouchSensor
@ -5,13 +9,15 @@ import time
# Create a local participant for handling communcation # Create a local participant for handling communcation
# using default settings (UDP communciation over port 7681) # using default settings (UDP communciation over port 7681)
participant = LocalParticipant() participant = LocalParticipant(port=7681, local_port=7682)
# The robot's propulsion is a differential drive # The robot's propulsion is a differential drive
bb2b = DifferentialDrive(participant) bb2b = DifferentialDrive(participant)
bb2b.model_url = "https://passer.life/extras/ant1_transparent.png"
# It has a touch sensor at the front left of the roboid # It has a touch sensor at the front left of the roboid
touch_left = TouchSensor(bb2b) touch_left = TouchSensor(parent=bb2b)
# and other one on the right # and other one on the right
touch_right = TouchSensor(bb2b) touch_right = TouchSensor(parent=bb2b)
# Do forever: # Do forever:
while True: while True:
@ -26,6 +32,6 @@ while True:
bb2b.SetWheelVelocity(wheel_speed_left, wheel_speed_right) bb2b.SetWheelVelocity(wheel_speed_left, wheel_speed_right)
# Update the roboid state # Update the roboid state
bb2b.Update(True) participant.Update()
# and sleep for 100ms # and sleep for 100ms
time.sleep(100) time.sleep(100)

43
LocalParticipant.py
View File

@ -1,5 +1,6 @@
import socket import socket
import threading import threading
import time
from Participant import Participant from Participant import Participant
from Thing import Thing from Thing import Thing
@ -24,7 +25,6 @@ class LocalParticipant(Participant):
Currently, only UDP communication is supported Currently, only UDP communication is supported
""" """
publishInterval = 3000 # 3 seconds
buffer = None buffer = None
## The network ID of the participant ## The network ID of the participant
@ -34,20 +34,25 @@ class LocalParticipant(Participant):
thread = None thread = None
name = "Participant" name = "Participant"
def __init__(self, port=7681, ip_address = "0.0.0.0", remote=False, udp_socket = None): def __init__(self, port=7681, ip_address="0.0.0.0", local_port=0, remote=False, udp_socket = None):
super().__init__(ip_address, port) super().__init__(ip_address = ip_address, port = port)
self.ip_address = ip_address # self.ip_address = ip_address
self.port = port # self.port = port
if local_port == 0:
local_port = port
self.local_port = local_port
self.others = [] self.others = []
self.network_id = 0 self.network_id = 0
self.buffer = bytearray(256) self.buffer = bytearray(256)
self.thing_msg_processors = {} self.thing_msg_processors = {}
self.new_thing_handlers = [] self.new_thing_handlers = []
self.publishInterval = 3000 # 3 seconds
if remote == False: if remote == False:
self.udp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) 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.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)
self.udp_socket.bind(("0.0.0.0", port)) 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 = threading.Thread(target = self.Receiver)
@ -70,7 +75,7 @@ class LocalParticipant(Participant):
def AddParticipant(self, ip_address, port): 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 = LocalParticipant(ip_address, port, remote=True, udp_socket=self.udp_socket) remote_participant = Participant(ip_address = ip_address, port = port)
remote_participant.network_id = len(self.others) remote_participant.network_id = len(self.others)
self.others.append(remote_participant) self.others.append(remote_participant)
return remote_participant return remote_participant
@ -90,18 +95,18 @@ class LocalParticipant(Participant):
#region Send #region Send
def SendThingInfo(self, thing): def SendThingInfo(self, owner, thing):
self.Send(ThingMsg(self.network_id, thing)) self.Send(owner, ThingMsg(self.network_id, thing))
self.Send(NameMsg(self.network_id, thing)) self.Send(owner, NameMsg(self.network_id, thing))
self.Send(ModelUrlMsg(self.network_id, thing)) self.Send(owner, ModelUrlMsg(self.network_id, thing))
def Send(self, msg): def Send(self, owner, msg):
buffer_size = msg.Serialize([self.buffer]) buffer_size = msg.Serialize([self.buffer])
if buffer_size <= 0: if buffer_size <= 0:
return True return True
# print(f'{self.name} send {self.buffer[0]} to {self.ip_address} {self.port}') print(f'{self.name} send {self.buffer[0]} to {owner.ip_address} {owner.port}')
self.udp_socket.sendto(self.buffer[:buffer_size], (self.ip_address, self.port)) self.udp_socket.sendto(self.buffer[:buffer_size], (owner.ip_address, owner.port))
return True return True
def Publish(self, msg): def Publish(self, msg):
@ -129,14 +134,14 @@ class LocalParticipant(Participant):
remote_participant = self.AddParticipant(remote_ip_address, remote_port) remote_participant = self.AddParticipant(remote_ip_address, remote_port)
self.ReceiveData(data, remote_participant) self.ReceiveData(data, remote_participant)
def ReceiveData(self, data, remote_participant): def ReceiveData(self, data, sender):
msgId = data[0] msgId = data[0]
# print(f'msg {msgId} ') # print(f'msg {msgId} ')
match msgId: match msgId:
case ParticipantMsg.id: case ParticipantMsg.id:
self.ProcessClientMsg(remote_participant, ParticipantMsg(data)) self.ProcessClientMsg(sender, ParticipantMsg(data))
case NetworkIdMsg.id: case NetworkIdMsg.id:
self.ProcessNetworkIdMsg(remote_participant, NetworkIdMsg(data)) self.ProcessNetworkIdMsg(sender, NetworkIdMsg(data))
# case InvestigateMsg.id: # case InvestigateMsg.id:
# self.ProcessInvestigateMsg(InvestigateMsg(data)) # self.ProcessInvestigateMsg(InvestigateMsg(data))
case ThingMsg.id: case ThingMsg.id:
@ -156,9 +161,9 @@ class LocalParticipant(Participant):
print(f'{self.name} receive network id {self.network_id} -> {msg.network_id}') print(f'{self.name} receive network id {self.network_id} -> {msg.network_id}')
if self.network_id != msg.network_id: if self.network_id != msg.network_id:
self.network_id = msg.network_id self.network_id = msg.network_id
# print(f'sending all things {len(Thing.allThings)}') print(f'sending all things {len(self.things)}')
for thing in self.things: for thing in self.things:
sender.SendThingInfo(thing) self.SendThingInfo(sender, thing)
# self.Send(NameMsg(self.network_id, thing)) # self.Send(NameMsg(self.network_id, thing))
def ProcessInvestigateMsg(self, data: bytearray): def ProcessInvestigateMsg(self, data: bytearray):

5
Messages/ThingMsg.py
View File

@ -20,7 +20,10 @@ class ThingMsg(IMessage):
if thing is not None: if thing is not None:
self.thing_id = thing.id self.thing_id = thing.id
self.thing_type = thing.type self.thing_type = thing.type
self.parent_id = thing.parent_id if thing.parent is not None:
self.parent_id = thing.parent.id
else:
self.parent_id = 0
def Serialize(self, buffer_ref): def Serialize(self, buffer_ref):
if self.network_id is None or self.thing_id is None: if self.network_id is None or self.thing_id is None:

4
Participant.py
View File

@ -11,7 +11,7 @@ class Participant:
""" """
## The Ip Address of a participant. When the participant is local, this contains 0.0.0.0 ## The Ip Address of a participant. When the participant is local, this contains 0.0.0.0
self.ip_addres = ip_address self.ip_address = ip_address
## The port number for UDP communication with the participant. This is 0 for isolated participants. ## The port number for UDP communication with the participant. This is 0 for isolated participants.
self.port = port self.port = port
## The network Id to identify the participant. ## The network Id to identify the participant.
@ -21,7 +21,7 @@ class Participant:
## The things managed by this participant ## The things managed by this participant
self.things = set({ None }) self.things = set({ None })
def Add(self, thing, check_id): def Add(self, thing, check_id = True):
"""! Add a new thing for this participant. """! Add a new thing for this participant.
@param thing The thing to add @param thing The thing to add
@param check_id When true, the thing ID of the thing is checked. If it is 0, a new thing Id will be assigned. @param check_id When true, the thing ID of the thing is checked. If it is 0, a new thing Id will be assigned.

13
Thing.py
View File

@ -22,6 +22,12 @@ class Thing:
def __init__(self, owner = None, network_id = 0, thing_id = 0, type = Type.Undetermined, parent = None, name = None): def __init__(self, owner = None, network_id = 0, thing_id = 0, type = Type.Undetermined, parent = None, name = None):
"""! Create a new thing """ """! Create a new thing """
## The parent thing
self.parent = None
if parent is not None:
owner = parent.owner
self.parent = parent
## The participant owning this thing ## The participant owning this thing
self.owner = owner self.owner = owner
## The network ID of this thing ## The network ID of this thing
@ -32,11 +38,6 @@ class Thing:
## The type of the thing ## The type of the thing
self.type = type self.type = type
## The parent thing
self.parent = 0
if parent is not None:
self.parent_id = parent.id
## The name of the thing ## The name of the thing
self.name = name self.name = name
## An URL pointing to the location where a model of the thing can be found ## An URL pointing to the location where a model of the thing can be found
@ -52,7 +53,7 @@ class Thing:
self.angular_velocity: Spherical = Spherical.zero self.angular_velocity: Spherical = Spherical.zero
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 update(self, currentTime): def update(self, currentTime):
pass pass

2
Things/TouchSensor.py
View File

@ -6,7 +6,7 @@ class TouchSensor(Thing):
def __init__(self, owner = None, parent = None): def __init__(self, owner = None, parent = None):
"""! Create a touch sensor """! Create a touch sensor
""" """
super().__init__(owner, parent) super().__init__(owner = owner, parent = parent)
## Value which is true when the sensor is touching something, false otherwise ## Value which is true when the sensor is touching something, false otherwise
self.touched_somthing = False self.touched_somthing = False