54 lines
1.9 KiB
Python
54 lines
1.9 KiB
Python
import Messages.LowLevelMessages
|
|
#from Thing import Thing
|
|
|
|
class PoseMsg():
|
|
id = 0x10
|
|
length = 4
|
|
|
|
Position = 0x01
|
|
Orientation = 0x02
|
|
LinearVelocity = 0x04
|
|
AngularVelocity = 0x08
|
|
|
|
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.pose_type = 0
|
|
if thing.position_updated or force:
|
|
self.pose_type |= PoseMsg.Position
|
|
if thing.orientation_updated or force:
|
|
self.pose_type |= PoseMsg.Orientation
|
|
if thing.linear_velocity_updated or force:
|
|
self.pose_type |= PoseMsg.LinearVelocity
|
|
if thing.angular_velocity_updated or force:
|
|
self.pose_type |= PoseMsg.AngularVelocity
|
|
|
|
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,
|
|
self.network_id,
|
|
self.thing.id,
|
|
self.pose_type
|
|
]
|
|
ix = [4]
|
|
if self.pose_type & PoseMsg.Position:
|
|
Messages.LowLevelMessages.SendSpherical(buffer, ix, self.thing.position)
|
|
if self.pose_type & PoseMsg.Orientation:
|
|
Messages.LowLevelMessages.SendQuat32(buffer, ix, self.thing.orientation)
|
|
if self.pose_type & PoseMsg.LinearVelocity:
|
|
Messages.LowLevelMessages.SendSpherical(buffer, ix, self.thing.linear_velocity)
|
|
if self.pose_type & PoseMsg.AngularVelocity:
|
|
Messages.LowLevelMessages.SendSpherical(buffer, ix, self.thing.angular_velocity)
|
|
return PoseMsg.length + ix[0]
|