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

Ant is working with collision avoidance

Browse Source
This commit is contained in:
Pascal Serrarens 2024-12-22 13:51:05 +01:00
parent 134bb7c332
commit d23ff21aa9
8 changed files with 169 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
LowLevelMessages.py
View File

@ -1,4 +1,5 @@
import numpy as np
from .SwingTwist import SwingTwist
def SendAngle8(buffer, ix_ref, angle):
# Normalize angle
@ -21,12 +22,22 @@ def SendFloat16(buffer, ix_ref, value):
]
ix_ref[0] += 2
def ReceiveFloat16(buffer, ix_ref) -> float:
ix = ix_ref[0]
binary = (buffer[ix] << 8) + buffer[ix+1]
value16 = np.uint16(binary).view(np.float16)
ix_ref[0] += 2
return float(value16)
def SendSpherical(buffer, ix_ref, vector):
SendFloat16(buffer, ix_ref, vector.distance)
SendAngle8(buffer, ix_ref, vector.direction.horizontal)
SendAngle8(buffer, ix_ref, vector.direction.vertical)
def SendQuat32(buffer, ix_ref, q):
if isinstance(q, SwingTwist):
q = q.ToQuaternion()
ix = ix_ref[0]
qx = (int)(q.x * 127 + 128)
qy = (int)(q.y * 127 + 128)

35
Messages.py
View File

@ -63,6 +63,14 @@ class NetworkIdMsg(IMessage):
]
return NetworkIdMsg.length
class InvestigateMsg():
id = 0x81
length = 3
def __init__(self, buffer):
self.network_id = buffer[1]
self.thing_id = buffer[2]
class ThingMsg(IMessage):
id = 0x80
length = 5
@ -151,7 +159,7 @@ class PoseMsg(IMessage):
id = 0x10
length = 4
def __init__(self, network_id, thing, poseType):
def __init__(self, network_id, thing):
self.network_id = network_id
self.thing = thing
@ -176,3 +184,28 @@ class PoseMsg(IMessage):
if self.thing.pose_updated & Thing.AngularVelocity:
LowLevelMessages.SendSpherical(buffer, ix, self.thing.angularVelocity)
return ix[0]
class BinaryMsg():
id = 0xB1
def __init__(self, buffer):
self.network_id = buffer[1]
self.thing_id = buffer[2]
self.thing: Thing = Thing.Get(self.network_id, self.thing_id)
self.data = buffer[3:]
def SendTo(participant, thing, data: bytearray):
length = 3
if thing.network_id is None or thing is None or data is None:
return False
participant.buffer[0:length] = [
id,
participant.network_id,
thing.id
]
full_length = length + len(data)
participant.buffer[length:full_length] = data
participant.SendBuffer(full_length)
return True

9
Participant.py
View File

@ -52,6 +52,8 @@ class Participant:
pass
def ProcessNetworkIdMsg(self, data: bytearray):
pass
def ProcessInvestigateMsg(self, data: bytearray):
pass
def ReceiveData(self, data):
msgId = data[0]
@ -62,4 +64,9 @@ class Participant:
case Messages.NetworkIdMsg.id:
msg = Messages.NetworkIdMsg(data)
self.ProcessNetworkIdMsg(msg)
case Messages.InvestigateMsg.id:
msg = Messages.InvestigateMsg(data)
self.ProcessInvestigateMsg(msg)
case Messages.BinaryMsg.id:
msg = Messages.BinaryMsg(data)
msg.thing.ProcessBinary(msg.data)

36
Quaternion.py
View File

@ -1,8 +1,44 @@
import math
Deg2Rad = (math.pi * 2) / 360
class Quaternion:
def __init__(self):
self.x = 0
self.y = 0
self.z = 0
self.w = 1
@staticmethod
def Euler(x, y, z):
yaw = x * Deg2Rad
pitch = y * Deg2Rad
roll = z * Deg2Rad
roll_over_2 = roll * 0.5
sin_roll_over_2 = math.sin(roll_over_2)
cos_roll_over_2 = math.cos(roll_over_2)
pitch_over_2 = pitch * 0.5
sin_pitch_over_2 = math.sin(pitch_over_2)
cos_pitch_over_2 = math.cos(pitch_over_2)
yaw_over_2 = yaw * 0.5
sin_yaw_over_2 = math.sin(yaw_over_2)
cos_yaw_over_2 = math.cos(yaw_over_2)
result = Quaternion()
result.w = (cos_yaw_over_2 * cos_pitch_over_2 * cos_roll_over_2 +
sin_yaw_over_2 * sin_pitch_over_2 * sin_roll_over_2)
result.x = (sin_yaw_over_2 * cos_pitch_over_2 * cos_roll_over_2 +
cos_yaw_over_2 * sin_pitch_over_2 * sin_roll_over_2)
result.y = (cos_yaw_over_2 * sin_pitch_over_2 * cos_roll_over_2 -
sin_yaw_over_2 * cos_pitch_over_2 * sin_roll_over_2)
result.z = (cos_yaw_over_2 * cos_pitch_over_2 * sin_roll_over_2 -
sin_yaw_over_2 * sin_pitch_over_2 * cos_roll_over_2)
return result
Quaternion.identity = Quaternion()

32
SiteServer.py
View File

@ -11,18 +11,42 @@ class SiteServer(Participant):
def Update(self, currentTime):
ready_to_read, _, _ = select.select([self.udpSocket], [], [], 0.1) # Timeout of 0.1 seconds
if ready_to_read:
while ready_to_read:
data, addr = self.udpSocket.recvfrom(1024)
self.ReceiveData(data)
ready_to_read, _, _ = select.select([self.udpSocket], [], [], 0.1) # Timeout of 0.1 seconds
return super().Update(currentTime)
def ProcessNetworkIdMsg(self, thing_msg):
self.network_id = thing_msg.network_id
thing = next(iter(Thing.allThings))
def SendThingInfo(self, thing, recurse = False):
if thing is None:
return
thing_msg = Messages.ThingMsg(self.network_id, thing)
thing_msg.SendTo(self)
name_msg = Messages.NameMsg(self.network_id, thing)
name_msg.SendTo(self)
model_msg = Messages.ModelUrlMsg(self.network_id, thing)
model_msg.SendTo(self)
pose_msg = Messages.PoseMsg(self.network_id, thing)
pose_msg.SendTo(self)
if recurse:
for child in thing.children:
self.SendThingInfo(child, True)
def ProcessNetworkIdMsg(self, thing_msg):
self.network_id = thing_msg.network_id
# HACK: send the root things first
for thing in Thing.allThings:
if thing is not None and thing.parent_id == 0:
self.SendThingInfo(thing)
# then sent the rest
for thing in Thing.allThings:
if thing is not None and thing.parent_id != 0:
self.SendThingInfo(thing)
def ProcessInvestigateMsg(self, msg: Messages.InvestigateMsg):
thing = Thing.Get(msg.network_id, msg.thing_id)
if thing is not None:
self.SendThingInfo(thing)

18
SwingTwist.py Normal file
View File

@ -0,0 +1,18 @@
from .Direction import Direction
from .Quaternion import Quaternion
class SwingTwist:
def __init__(self, swing: Direction, twist: float):
if swing.vertical > 90 or swing.vertical < -90:
swing.horizontal += 180
swing.vertical = 180 - swing.vertical
twist += 180
self.swing = swing
self.twist = twist
def ToQuaternion(self) -> Quaternion:
q = Quaternion.Euler(-self.swing.vertical,
self.swing.horizontal,
self.twist)
return q

29
Thing.py
View File

@ -2,23 +2,28 @@ from .Spherical import Spherical
from .Quaternion import Quaternion
class Thing:
allThings = set()
class Type:
Undetermined = 0x00
Switch = 0x01
DistanceSensor = 0x02
DirectionalSensor = 0x03
Animator = 0x40
Position = 0x01
Orientation = 0x02
LinearVelocity = 0x04
AngularVelocity = 0x08
def __init__(self, parent=None):
def __init__(self, type=Type.Undetermined, parent=None, name=None):
self.networkId = 0
self.id = 0
self.type = 0
self.type = type
if parent is None:
self.parent_id = 0
else:
self.parent_id = parent.id
self.name = None
self.name = name
self.model_url = None
self.position = Spherical.zero
@ -33,11 +38,25 @@ class Thing:
def update(self, currentTime):
pass
def ProcessBinary(self, data):
pass
allThings = set({ None })
@staticmethod
def Add(thing):
thing.id = len(Thing.allThings)
Thing.allThings.add(thing)
@staticmethod
def Get(networkId, thingId):
for thing in Thing.allThings:
if thing is not None:
if thing.networkId == networkId and thing.id == thingId:
return thing
return None
def UpdateAll(currentTime):
for thing in Thing.allThings:
thing.update(currentTime)
if thing is not None:
thing.update(currentTime)

9
__init__.py
View File

@ -1,7 +1,14 @@
__all__ = ['Direction', 'Spherical', 'Thing', 'Participant', 'Messages', 'SiteServer']
__all__ = ['Direction',
'Spherical',
'Thing',
'Participant',
'Messages',
'SiteServer',
'SwingTwist']
from .Direction import Direction
from .Participant import Participant
from .Thing import Thing
from .Spherical import Spherical
from .SwingTwist import SwingTwist
from .SiteServer import SiteServer