237 lines
8.4 KiB
Python
237 lines
8.4 KiB
Python
from Participant import Participant
|
|
from LinearAlgebra.Spherical import Spherical
|
|
from LinearAlgebra.Quaternion import Quaternion
|
|
|
|
from typing import Optional
|
|
import time
|
|
|
|
class Thing:
|
|
"""! A thing is the basic building block"""
|
|
class Type:
|
|
"""! Predefined thing types
|
|
"""
|
|
Undetermined = 0x00
|
|
# Sensor
|
|
Switch = 0x01
|
|
DistanceSensor = 0x02
|
|
DirectionalSensor = 0x03
|
|
TemperatureSensor = 0x04
|
|
TouchSensor = 0x05
|
|
# Motor
|
|
ControlledMotor = 0x06
|
|
UncontrolledMotor = 0x07
|
|
Servo = 0x08
|
|
# Other
|
|
Roboid = 0x09
|
|
Humanoid = 0x0A
|
|
ExternalSensor = 0x0B
|
|
Animator = 0x0C
|
|
DifferentialDrive = 0x0D
|
|
|
|
Position = 0x01
|
|
Orientation = 0x02
|
|
LinearVelocity = 0x04
|
|
AngularVelocity = 0x08
|
|
|
|
# region Init
|
|
|
|
def __init__(self, owner: Optional[Participant] = None, parent: Optional['Thing'] = None, thing_type: int = Type.Undetermined, thing_id: int = 0) -> None:
|
|
"""! Create a new thing
|
|
@param owner The owning participant
|
|
@param parent The parent thing (will override owner if set)
|
|
@param thing_type The type of thing (can use \ref RoboidControl::Thing::Thing::Type "Thing.Type")
|
|
@param thingId The ID of the thing, leave out or set to zero to generate an ID
|
|
"""
|
|
## The participant owning this thing
|
|
self.owner: Optional[Participant] = None
|
|
## The ID of the thing
|
|
self.id: int = thing_id
|
|
## The type of the thing
|
|
#
|
|
## This can be either a \ref RoboidControl::Thing::Thing::Type "Thing.Type" or a byte value for custom types.
|
|
self.type: int = thing_type
|
|
|
|
## The parent of this thing
|
|
self.parent: Optional[Thing] = None
|
|
if parent is not None:
|
|
self.owner = parent.owner
|
|
self.SetParent(parent)
|
|
elif owner == None:
|
|
from RoboidControl.ParticipantUDP import ParticipantUDP
|
|
self.owner = ParticipantUDP.Isolated()
|
|
else:
|
|
self.owner = owner
|
|
|
|
## The children of this thing
|
|
self.children: list[Thing] = list()
|
|
|
|
## The name of the thing
|
|
self.name: Optional[str] = None
|
|
## An URL pointing to the location where a model of the thing can be found
|
|
self.model_url: Optional[str] = None
|
|
|
|
## The position of the thing in local space, in meters
|
|
self.position: Spherical = Spherical.zero
|
|
## Boolean indicating that the thing has an updated position
|
|
self.position_updated: bool = False
|
|
|
|
## The orientation of the thing in local space
|
|
self.orientation: Quaternion = Quaternion.identity
|
|
## Boolean indicating the thing has an updated orientation
|
|
self.orientation_updated: bool = False
|
|
|
|
## The linear velocity of the thing in local space, in meters per second
|
|
self.linear_velocity: Spherical = Spherical.zero
|
|
## Boolean indicating the thing has an updated linear velocity
|
|
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
|
|
## Boolean indicating the thing has an updated angular velocity
|
|
self.angular_velocity_updated: bool = False
|
|
|
|
if self.owner is not None:
|
|
self.owner.Add(self)
|
|
|
|
# endregion Init
|
|
|
|
# region Hierarchy
|
|
|
|
def SetParent(self, parent: Optional['Thing']):
|
|
"""! Sets the parent of this Thing
|
|
@param The Thing which should become the parent
|
|
@note Do not set Thing.parent directly, as that will break the parent-child relation
|
|
"""
|
|
if parent is None:
|
|
parentThing = self.parent
|
|
if parentThing is not None:
|
|
parentThing.RemoveChild(self)
|
|
self.parent = None
|
|
else:
|
|
parent.AddChild(self)
|
|
|
|
def AddChild(self, child: 'Thing'):
|
|
"""! Add a child Thing to this Thing
|
|
@param child The Thing which should become a child
|
|
@remark When the Thing is already a child, it will not be added again
|
|
@note Do not add a thing to Thing.children directly, as that will break the parent-child relation
|
|
"""
|
|
if child in self.children:
|
|
return
|
|
|
|
child.parent = self
|
|
self.children.append(child)
|
|
|
|
def RemoveChild(self, child: 'Thing'):
|
|
"""! Remove the given thing as a child of this thing
|
|
@param child The child to remove
|
|
"""
|
|
self.children.remove(child)
|
|
|
|
def GetChild(self, thing_id: int, recurse: bool = False) -> Optional['Thing']:
|
|
"""! Get a child by thing Id
|
|
@param id The thing ID to find
|
|
@param recurse Look recursively through all descendants
|
|
@returns The found thing of nullptr when nothing is found
|
|
"""
|
|
for child in self.children:
|
|
if child.id == thing_id:
|
|
return child
|
|
if recurse:
|
|
found_child = child.GetChild(thing_id, recurse)
|
|
if found_child is not None:
|
|
return found_child
|
|
return None
|
|
|
|
def FindChild(self, name: str, recurse: bool = True) -> Optional['Thing']:
|
|
"""! Find a thing by name
|
|
@param name The name of the thing
|
|
@return The found thing or nullptr when nothing is found
|
|
@param recurse Look recursively through all descendants
|
|
"""
|
|
for child in self.children:
|
|
if child.name == name:
|
|
return child
|
|
if recurse:
|
|
found_child = child.FindChild(name, recurse)
|
|
if found_child is not None:
|
|
return found_child
|
|
return None
|
|
# endregion Hierarchy
|
|
|
|
# region Pose
|
|
|
|
def SetPosition(self, position: Spherical) -> None:
|
|
"""! Set the position of the thing
|
|
@param position The new position in local space, in meters
|
|
"""
|
|
self.position = position
|
|
self.position_updated = True
|
|
|
|
def SetOrientation(self, orientation: Quaternion) -> None:
|
|
"""! Set the orientation of the thing
|
|
@param orientation The new orientation in local space
|
|
"""
|
|
self.orientation = orientation
|
|
self.orientation_updated = True
|
|
|
|
def SetLinearVelocity(self, linear_velocity: Spherical) -> None:
|
|
"""! Set the linear velocity of the thing
|
|
@param linearVelocity The new linear velocity in local space, in meters per second
|
|
"""
|
|
if self.linear_velocity != linear_velocity:
|
|
self.linear_velocity_updated = True
|
|
|
|
self.linear_velocity = linear_velocity
|
|
|
|
def SetAngularVelocity(self, angular_velocity: Spherical) -> None:
|
|
"""! Set the angular velocity of the thing
|
|
@param angularVelocity the new angular velocity in local space
|
|
"""
|
|
if self.angular_velocity != angular_velocity:
|
|
self.angular_velocity_updated = True
|
|
|
|
self.angular_velocity = angular_velocity
|
|
|
|
# endregion Pose
|
|
|
|
# region Update
|
|
|
|
@staticmethod
|
|
def GetTimeMs() -> int:
|
|
"""! Get the current time in milliseconds
|
|
@return The current time in milliseconds
|
|
"""
|
|
return int(time.time() * 1000)
|
|
|
|
def Update(self, currentTimeMs: int = 0, recurse: bool = False) -> None:
|
|
"""! Update de state of the thing
|
|
@param currentTimeMs The current clock time in milliseconds; If this is zero, the current time is retrieved automatically
|
|
@param recurse When true, this will Update the descendants recursively
|
|
"""
|
|
self.position_updated = False
|
|
self.orientation_updated = False
|
|
self.linear_velocity_updated = False
|
|
self.angular_velocity_updated = False
|
|
|
|
if recurse:
|
|
for child in self.children:
|
|
child.Update(currentTimeMs, recurse)
|
|
|
|
# endregion Update
|
|
|
|
def GenerateBinary(self, bytes: bytearray, ix_ref: set[int]) -> int:
|
|
"""! Function used to generate binary data for this thing
|
|
@param buffer The byte array for thw binary data
|
|
@param ix_ref A single element array with the starting position for writing the binary data
|
|
@returns The size of the binary data
|
|
"""
|
|
return 0
|
|
|
|
def ProcessBinary(self, data: bytes):
|
|
"""! Function used to process binary data received for this thing
|
|
@param bytes The binary data
|
|
"""
|
|
print('default binary processor')
|
|
pass
|