Ant is working with collision avoidance

LowLevelMessages.py

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

Messages.py

@@ -62,7 +62,15 @@ class NetworkIdMsg(IMessage):
             self.network_id
         ]
         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

Participant.py

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

Quaternion.py

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

SiteServer.py

@@ -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):
+    def SendThingInfo(self, thing, recurse = False):
-        self.network_id = thing_msg.network_id
+        if thing is None:
-        thing = next(iter(Thing.allThings))
+            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)

SwingTwist.py

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

Thing.py

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

__init__.py

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