Updates to comply with C++/C#
This commit is contained in:
parent
fe09a5695d
commit
e53a01bb50
@ -11,15 +11,16 @@ from Things.TouchSensor import TouchSensor
|
||||
# Create a local participant for handling communcation
|
||||
# using default settings (UDP communciation over port 7681)
|
||||
participant = ParticipantUDP(port=7681, local_port=7682, ip_address="127.0.0.1")
|
||||
|
||||
# The robot's propulsion is a differential drive
|
||||
bb2b = DifferentialDrive(participant)
|
||||
bb2b.name = "BB2B"
|
||||
bb2b.SetDriveDimensions(0.064, 0.128)
|
||||
bb2b.SetOrientation(SwingTwist.Degrees(40, 0, 0))
|
||||
bb2b.SetPosition(Spherical(0.15, Direction.up))
|
||||
# bb2b.SetPosition(Spherical(0.15, Direction.up))
|
||||
|
||||
model = Thing(parent=bb2b)
|
||||
model.model_url = "https://passer.life/extras/Roller1anim1.png"
|
||||
model.model_url = "https://passer.life/extras/Roller2anim1.png"
|
||||
model.SetOrientation(SwingTwist.Degrees(90, 0, 0))
|
||||
|
||||
# It has a touch sensor at the front left of the roboid
|
||||
@ -49,4 +50,4 @@ while True:
|
||||
# Update the roboid state
|
||||
participant.Update()
|
||||
# and sleep for 100ms
|
||||
time.sleep(0.1)
|
||||
time.sleep(1)
|
||||
|
||||
@ -9,6 +9,7 @@ class BinaryMsg():
|
||||
self.thing_id = data[2]
|
||||
self.data = data[3:]
|
||||
else:
|
||||
self.network_id = data
|
||||
self.thing_id = thing.id
|
||||
self.thing = thing
|
||||
|
||||
|
||||
@ -14,6 +14,7 @@ class ModelUrlMsg(IMessage):
|
||||
self.url = data[ModelUrlMsg.length:].decode("utf-8")
|
||||
else:
|
||||
if thing is not None:
|
||||
self.network_id = data
|
||||
self.thing_id = thing.id
|
||||
self.url = thing.model_url
|
||||
|
||||
|
||||
@ -14,6 +14,7 @@ class NameMsg(IMessage):
|
||||
self.name = data[NameMsg.length:].decode("utf-8")
|
||||
else:
|
||||
if thing is not None:
|
||||
self.network_id = data
|
||||
self.thing_id = thing.id
|
||||
self.name = thing.name
|
||||
|
||||
|
||||
@ -6,11 +6,14 @@ from Messages.Messages import IMessage
|
||||
class NetworkIdMsg(IMessage):
|
||||
id = 0xA1
|
||||
length = 2
|
||||
|
||||
|
||||
## Create a network id message
|
||||
def __init__(self, data = None):
|
||||
pass
|
||||
def __init__(self, arg1 = None):
|
||||
if isinstance(arg1, bytes):
|
||||
buffer = arg1
|
||||
self.network_id = buffer[1]
|
||||
else:
|
||||
self.network_id = arg1
|
||||
|
||||
## Serialize the message into the given buffer
|
||||
#
|
||||
@ -20,7 +23,7 @@ class NetworkIdMsg(IMessage):
|
||||
buffer: bytearray = buffer_ref[0]
|
||||
buffer[0:NetworkIdMsg.length] = [
|
||||
NetworkIdMsg.id,
|
||||
0
|
||||
self.network_id
|
||||
]
|
||||
return NetworkIdMsg.length
|
||||
|
||||
|
||||
@ -24,6 +24,8 @@ class ParticipantMsg(IMessage):
|
||||
if buffer_ref is None:
|
||||
return 0
|
||||
|
||||
print(f'Send ParticipantMsg [0]')
|
||||
|
||||
buffer: bytearray = buffer_ref[0]
|
||||
buffer[0:ParticipantMsg.length] = [
|
||||
ParticipantMsg.id,
|
||||
|
||||
@ -10,30 +10,39 @@ class PoseMsg():
|
||||
LinearVelocity = 0x04
|
||||
AngularVelocity = 0x08
|
||||
|
||||
def __init__(self, thing, force: bool = False):
|
||||
self.thing = thing
|
||||
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:
|
||||
self.pose_type |= PoseMsg.LinearVelocity
|
||||
thing.linear_velocity_updated = False
|
||||
if thing.angular_velocity_updated:
|
||||
self.pose_type |= PoseMsg.AngularVelocity
|
||||
thing.angular_velocity_updated = False
|
||||
def __init__(self, arg1, thing, force: bool = False):
|
||||
if isinstance(arg1, bytes):
|
||||
self.thing_id = arg1[2]
|
||||
self.data = arg1[3:]
|
||||
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:
|
||||
self.pose_type |= PoseMsg.LinearVelocity
|
||||
thing.linear_velocity_updated = False
|
||||
if thing.angular_velocity_updated:
|
||||
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:
|
||||
return 0
|
||||
|
||||
print(f'Send PoseMsg [{self.network_id}/{self.thing.id}] {self.pose_type}')
|
||||
|
||||
buffer: bytearray = buffer_ref[0]
|
||||
buffer[0:PoseMsg.length] = [
|
||||
PoseMsg.id,
|
||||
0, # network_id,
|
||||
self.network_id,
|
||||
self.thing.id,
|
||||
self.pose_type
|
||||
]
|
||||
|
||||
@ -8,13 +8,16 @@ class ThingMsg(IMessage):
|
||||
thing_type = None
|
||||
parent_id = None
|
||||
|
||||
def __init__(self, data, thing=None):
|
||||
if isinstance(data, bytes):
|
||||
self.thing_id = data[2]
|
||||
self.thing_type = data[3]
|
||||
self.parent_id = data[4]
|
||||
def __init__(self, arg1, thing=None):
|
||||
if isinstance(arg1, bytes):
|
||||
buffer = arg1
|
||||
self.network_id = buffer[1]
|
||||
self.thing_id = buffer[2]
|
||||
self.thing_type = buffer[3]
|
||||
self.parent_id = buffer[4]
|
||||
else:
|
||||
if thing is not None:
|
||||
self.network_id = arg1
|
||||
self.thing_id = thing.id
|
||||
self.thing_type = thing.type
|
||||
if thing.parent is not None:
|
||||
@ -26,10 +29,12 @@ class ThingMsg(IMessage):
|
||||
if self.thing_id is None:
|
||||
return 0
|
||||
|
||||
print(f'Send ThingMsg [{self.network_id}/{self.thing_id}] {self.thing_type} {self.parent_id}')
|
||||
|
||||
buffer: bytearray = buffer_ref[0]
|
||||
buffer[0:ThingMsg.length] = [
|
||||
ThingMsg.id,
|
||||
0, # network_id,
|
||||
self.network_id,
|
||||
self.thing_id,
|
||||
self.thing_type,
|
||||
self.parent_id
|
||||
|
||||
@ -15,6 +15,9 @@ class Participant:
|
||||
## The port number for UDP communication with the participant. This is 0 for isolated participants.
|
||||
self.port = port
|
||||
|
||||
## The network ID of the participant
|
||||
self.network_id =0
|
||||
|
||||
## The things managed by this participant
|
||||
self.things = set()
|
||||
|
||||
|
||||
@ -42,19 +42,19 @@ class ParticipantUDP(Participant):
|
||||
|
||||
## True if the participant is running isolated.
|
||||
# Isolated participants do not communicate with other participants
|
||||
self.isolated = True
|
||||
self.is_isolated = True
|
||||
self.remote_site = None
|
||||
|
||||
## The other participants communicating with this participant
|
||||
self.others = []
|
||||
|
||||
if self.port != 0:
|
||||
self.isolated = False
|
||||
self.is_isolated = False
|
||||
if ip_address is not None:
|
||||
self.others.append(Participant(ip_address, port))
|
||||
self.remote_site = Participant(ip_address, port)
|
||||
self.others.append(self.remote_site)
|
||||
|
||||
self.buffer = bytearray(256)
|
||||
# self.thing_msg_processors = {}
|
||||
# self.new_thing_handlers = []
|
||||
self.publishInterval = 3000 # 3 seconds
|
||||
|
||||
self.udp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
||||
@ -92,33 +92,55 @@ class ParticipantUDP(Participant):
|
||||
if currentTimeMs is None:
|
||||
currentTimeMs = time.time() * 1000
|
||||
|
||||
if self.isolated == False:
|
||||
if self.is_isolated == False:
|
||||
if self.publishInterval > 0 and currentTimeMs > self.nextPublishMe:
|
||||
msg = ParticipantMsg()
|
||||
if self.others.count == 0:
|
||||
msg = ParticipantMsg(self.network_id)
|
||||
if self.remote_site == None:
|
||||
self.Publish(msg)
|
||||
else:
|
||||
for other_participant in self.others:
|
||||
self.Send(other_participant, msg)
|
||||
self.Send(self.remote_site, msg)
|
||||
|
||||
print(f'Publish ParticipantMsg')
|
||||
self.nextPublishMe = currentTimeMs + self.publishInterval
|
||||
|
||||
for thing in self.things:
|
||||
thing.Update(currentTimeMs)
|
||||
self.UpdateMyThings(currentTimeMs)
|
||||
# for thing in self.things:
|
||||
# thing.Update(currentTimeMs)
|
||||
|
||||
super().Update(currentTimeMs)
|
||||
# super().Update(currentTimeMs)
|
||||
|
||||
def UpdateMyThings(self, currentTimeMs):
|
||||
for thing in self.things:
|
||||
if thing is None:
|
||||
continue
|
||||
|
||||
# if thing.hierarchyChanged and not (self.isIsolated or self.network_id == ):
|
||||
# thingMsg = ThingMsg(self.network_id, thing)
|
||||
# self.Send(self.remote_site, thingMsg)
|
||||
|
||||
thing.Update(currentTimeMs, False)
|
||||
if not(self.is_isolated or self.network_id == 0):
|
||||
# if thing.terminate:
|
||||
# destroyMsg = DestroyMsg(self.network_id, thing)
|
||||
# self.Send(self.remote_site, destroyMsg)
|
||||
# else:
|
||||
# Send to remote site
|
||||
poseMsg = PoseMsg(thing.owner.network_id, thing)
|
||||
self.Send(self.remote_site, poseMsg)
|
||||
binaryMsg = BinaryMsg(thing.owner.network_id, thing)
|
||||
self.Send(self.remote_site, binaryMsg)
|
||||
# if thing.terminate:
|
||||
# self.Remove(thing)
|
||||
|
||||
#endregion
|
||||
|
||||
#region Send
|
||||
|
||||
def SendThingInfo(self, owner, thing, recursively: bool = False):
|
||||
self.Send(owner, ThingMsg(thing))
|
||||
self.Send(owner, NameMsg(thing))
|
||||
self.Send(owner, ModelUrlMsg(thing))
|
||||
self.Send(owner, PoseMsg(thing, True))
|
||||
self.Send(owner, BinaryMsg(thing))
|
||||
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, BinaryMsg(self.network_id, thing))
|
||||
|
||||
if recursively:
|
||||
for child in thing.children:
|
||||
@ -130,7 +152,7 @@ class ParticipantUDP(Participant):
|
||||
if buffer_size <= 0:
|
||||
return True
|
||||
|
||||
print(f'{self.name} send {self.buffer[0]} to {owner.ip_address} {owner.port}')
|
||||
# print(f'{self.name} send {self.buffer[0]} to {owner.ip_address} {owner.port}')
|
||||
self.udp_socket.sendto(self.buffer[:buffer_size], (owner.ip_address, owner.port))
|
||||
return True
|
||||
|
||||
@ -149,15 +171,19 @@ class ParticipantUDP(Participant):
|
||||
|
||||
def Receiver(self):
|
||||
while True:
|
||||
data, addr = self.udp_socket.recvfrom(1024)
|
||||
remote_ip_address = addr[0]
|
||||
remote_port = addr[1]
|
||||
# print(f'msg received from {remote_ip_address}:{remote_port}')
|
||||
remote_participant = self.GetParticipant(remote_ip_address, remote_port)
|
||||
if remote_participant is None:
|
||||
# print(f'new participant')
|
||||
remote_participant = self.AddParticipant(remote_ip_address, remote_port)
|
||||
self.ReceiveData(data, remote_participant)
|
||||
try:
|
||||
data, addr = self.udp_socket.recvfrom(1024)
|
||||
remote_ip_address = addr[0]
|
||||
remote_port = addr[1]
|
||||
# print(f'msg received from {remote_ip_address}:{remote_port}')
|
||||
remote_participant = self.GetParticipant(remote_ip_address, remote_port)
|
||||
if remote_participant is None:
|
||||
# print(f'new participant')
|
||||
remote_participant = self.AddParticipant(remote_ip_address, remote_port)
|
||||
self.ReceiveData(data, remote_participant)
|
||||
except ConnectionError:
|
||||
self.network_id = 0
|
||||
pass
|
||||
|
||||
def ReceiveData(self, data, sender):
|
||||
msgId = data[0]
|
||||
@ -166,7 +192,7 @@ class ParticipantUDP(Participant):
|
||||
case ParticipantMsg.id:
|
||||
self.ProcessParticipantMsg(sender, ParticipantMsg(data))
|
||||
case NetworkIdMsg.id:
|
||||
self.ProcessNetworkIdMsg(sender, NetworkIdMsg(data))
|
||||
self.ProcessSiteIdMsg(sender, NetworkIdMsg(data))
|
||||
# case InvestigateMsg.id:
|
||||
# self.ProcessInvestigateMsg(InvestigateMsg(data))
|
||||
case ThingMsg.id:
|
||||
@ -181,11 +207,12 @@ class ParticipantUDP(Participant):
|
||||
def ProcessParticipantMsg(self, sender, msg: ParticipantMsg):
|
||||
pass
|
||||
|
||||
def ProcessNetworkIdMsg(self, sender, msg: NetworkIdMsg):
|
||||
print(f'{self.name} receive network id')
|
||||
print(f'sending all things {len(self.things)}')
|
||||
for thing in self.things:
|
||||
if thing.parent is None:
|
||||
def ProcessSiteIdMsg(self, sender, msg: NetworkIdMsg):
|
||||
print(f'{self.name} Process SiteMsg {self.network_id} -> {msg.network_id}')
|
||||
if self.network_id != msg.network_id:
|
||||
self.network_id = msg.network_id
|
||||
for thing in self.things:
|
||||
#if thing.parent is None:
|
||||
self.SendThingInfo(sender, thing, recursively=True)
|
||||
|
||||
def ProcessInvestigateMsg(self, data: bytearray):
|
||||
|
||||
9
Thing.py
9
Thing.py
@ -100,10 +100,11 @@ class Thing:
|
||||
self.angular_velocity = angular_velocity
|
||||
self.angular_velocity_updated = True
|
||||
|
||||
def Update(self, currentTime):
|
||||
pose_msg = PoseMsg(self)
|
||||
for other in self.owner.others:
|
||||
self.owner.Send(other, pose_msg)
|
||||
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
|
||||
|
||||
def SetParent(self, parent):
|
||||
if parent is None:
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user