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