50 lines
1.8 KiB
Python
50 lines
1.8 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, 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 Serialize(self, buffer_ref):
|
|
if self.thing is None or self.pose_type == 0:
|
|
return 0
|
|
|
|
buffer: bytearray = buffer_ref[0]
|
|
buffer[0:PoseMsg.length] = [
|
|
PoseMsg.id,
|
|
0, # 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]
|