def gen_msg(self, id, parent_id=0):
body = setattrs(
optitrack_rigid_body_t(),
id=id,
xyz=self.xyz,
quat=self.quat,
params=0x01, # Tracking Value: True
)
desc = setattrs(
optitrack_rigid_body_description_t(),
id=id,
parent_id=parent_id,
name=self.name,
)
return (body, desc)
from director import vtkAll as vtk
from optitrack import optitrack_data_descriptions_t
from optitrack import optitrack_frame_t
import numpy as np
# Test code below
from optitrack import optitrack_marker_set_t
from optitrack import optitrack_marker_t
from optitrack import optitrack_rigid_body_t
from optitrack import optitrack_rigid_body_description_t
test_message = optitrack_frame_t()
body = optitrack_rigid_body_t()
body.id = 100
body.xyz = [-0.1, 0.3, 0.6]
body.quat = [1., 0., 0., 0.]
marker_xyz = [[0.1, 0.1, 0.0], [-0.1, 0.1, 0.02], [-0.1, -0.1, -0.0]]
body.marker_xyz = [[
xyz[0] + body.xyz[0], xyz[1] + body.xyz[1], xyz[2] + body.xyz[2]
] for xyz in marker_xyz]
body.marker_ids = [101, 102, 103]
body.num_markers = len(body.marker_ids)
test_message.rigid_bodies.append(body)
test_message.num_rigid_bodies = len(test_message.rigid_bodies)
test_desc_message = optitrack_data_descriptions_t()
body_desc = optitrack_rigid_body_description_t()
body_desc.name = "Foo"
def __unpackRigidBody( self, data ):
offset = 0
msg = optitrack_rigid_body_t()
# ID (4 bytes)
msg.id, = Int32Value.unpack(data[offset:offset + 4])
offset += 4
trace( "ID:", msg.id )
# Position and orientation
pos = Vector3.unpack( data[offset:offset+12] )
offset += 12
trace( "\tPosition:", pos[0],",", pos[1],",", pos[2] )
msg.xyz = pos
rot = Quaternion.unpack( data[offset:offset+16] )
offset += 16
trace( "\tOrientation:", rot[0],",", rot[1],",", rot[2],",", rot[3] )
msg.quat = rot
if( self.__natNetStreamVersion[0] >= 3 ) :
# After version 3.0 marker data moves to the description.
msg.num_markers = 0
else:
# Marker count (4 bytes)
markerCount, = Int32Value.unpack(data[offset:offset + 4])
offset += 4
markerCountRange = range( 0, markerCount )
trace( "\tMarker Count:", markerCount )
msg.num_markers = markerCount
# Marker positions
for i in markerCountRange:
pos = Vector3.unpack( data[offset:offset+12] )
offset += 12
trace( "\tMarker", i, ":", pos[0],",", pos[1],",", pos[2] )
msg.marker_xyz.append(pos)
# Populate marker_ids and marker_sizes in case we
# don't fill them later.
msg.marker_ids.append(i)
msg.marker_sizes.append(0.)
if( self.__natNetStreamVersion[0] >= 2 ):
# Marker ID's
for i in markerCountRange:
msg.marker_ids[i], = Int32Value.unpack(data[offset:offset + 4])
offset += 4
trace( "\tMarker ID", i, ":", msg.marker_ids[i] )
# Marker sizes
for i in markerCountRange:
msg.marker_sizes[i], = FloatValue.unpack( data[offset:offset+4] )
offset += 4
trace( "\tMarker Size", i, ":", msg.marker_sizes[i] )
markerError, = FloatValue.unpack( data[offset:offset+4] )
offset += 4
trace( "\tMarker Error:", markerError )
msg.mean_error = markerError
# Version 2.6 and later
if( ( ( self.__natNetStreamVersion[0] == 2 ) and ( self.__natNetStreamVersion[1] >= 6 ) ) or self.__natNetStreamVersion[0] > 2 or self.__natNetStreamVersion[0] == 0 ):
param, = struct.unpack( 'h', data[offset:offset+2] )
trackingValid = ( param & 0x01 ) != 0
offset += 2
trace( "\tTracking Valid:", 'True' if trackingValid else 'False' )
msg.params = param
return offset, msg