def publishMocapData(self, t):
frame = optitrack_frame_t()
desc = optitrack_data_descriptions_t()
bodies = [
SimpleBody("base", [0., 0., 0.], [1., 0, 0, 0]),
SimpleBody("test", [0.7, 0.2, 0.2], [1., 0, 0, 0]),
]
n = len(bodies)
frame.num_marker_sets = 0
frame.num_rigid_bodies = n
desc.num_rigid_bodies = n
for (i, body) in enumerate(bodies):
(body_msg, desc_msg) = body.gen_msg(i)
frame.rigid_bodies.append(body_msg)
desc.rigid_bodies.append(desc_msg)
self.lc.publish('OPTITRACK_FRAMES', frame.encode())
self.lc.publish('OPTITRACK_DATA_DESCRIPTIONS', desc.encode())
from director.shallowCopy import shallowCopy
from director.thirdparty import transformations
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()
def __unpackMocapData( self, data ):
trace( "Begin MoCap Frame\n-----------------\n" )
msg = optitrack_frame_t()
msg.utime = time.time() * 1e6
# TODO(sam.creasey) I don't know if this is worth it...
#data = memoryview( data )
offset = 0
# Frame number (4 bytes)
msg.frame, = Int32Value.unpack(data[offset:offset + 4])
offset += 4
trace( "Frame #:", msg.frame )
# Marker set count (4 bytes)
markerSetCount, = Int32Value.unpack(data[offset:offset + 4])
offset += 4
trace( "Marker Set Count:", markerSetCount )
msg.num_marker_sets = markerSetCount
for i in range( 0, markerSetCount ):
marker_set = optitrack_marker_set_t()
# Model name
#modelName, separator, remainder = bytes(data[offset:]).partition( b'\0' )
modelName = data[offset:].split('\0')[0]
offset += len( modelName ) + 1
trace( "Model Name:", modelName.decode( 'utf-8' ) )
marker_set.name = modelName
# Marker count (4 bytes)
markerCount, = Int32Value.unpack(data[offset:offset + 4])
offset += 4
trace( "Marker Count:", markerCount )
marker_set.num_markers = markerCount
for j in range( 0, markerCount ):
marker_set.xyz.append(Vector3.unpack( data[offset:offset+12] ))
offset += 12
#trace( "\tMarker", j, ":", pos[0],",", pos[1],",", pos[2] )
msg.marker_sets.append(marker_set)
# Unlabeled markers count (4 bytes)
unlabeledMarkersCount, = Int32Value.unpack(data[offset:offset + 4])
offset += 4
trace( "Unlabeled Markers Count:", unlabeledMarkersCount )
msg.num_other_markers = unlabeledMarkersCount
for i in range( 0, unlabeledMarkersCount ):
pos = Vector3.unpack( data[offset:offset+12] )
offset += 12
trace( "\tMarker", i, ":", pos[0],",", pos[1],",", pos[2] )
msg.other_markers.append(pos)
# Rigid body count (4 bytes)
rigidBodyCount, = Int32Value.unpack(data[offset:offset + 4])
offset += 4
trace( "Rigid Body Count:", rigidBodyCount )
msg.num_rigid_bodies = rigidBodyCount
for i in range( 0, rigidBodyCount ):
(extra_offset, rigid_body_msg) = self.__unpackRigidBody( data[offset:] )
offset += extra_offset
msg.rigid_bodies.append(rigid_body_msg)
# Version 2.1 and later
skeletonCount = 0
if( ( self.__natNetStreamVersion[0] == 2 and self.__natNetStreamVersion[1] > 0 ) or self.__natNetStreamVersion[0] > 2 ):
skeletonCount, = Int32Value.unpack(data[offset:offset + 4])
offset += 4
trace( "Skeleton Count:", skeletonCount )
for i in range( 0, skeletonCount ):
offset += self.__unpackSkeleton( data[offset:] )
# Labeled markers (Version 2.3 and later)
labeledMarkerCount = 0
if( ( self.__natNetStreamVersion[0] == 2 and self.__natNetStreamVersion[1] > 3 ) or self.__natNetStreamVersion[0] > 2 ):
labeledMarkerCount, = Int32Value.unpack(data[offset:offset + 4])
offset += 4
trace( "Labeled Marker Count:", labeledMarkerCount )
msg.num_labeled_markers = labeledMarkerCount
for i in range( 0, labeledMarkerCount ):
marker = optitrack_marker_t()
marker.id, = Int32Value.unpack(data[offset:offset + 4])
offset += 4
marker.xyz = Vector3.unpack( data[offset:offset+12] )
offset += 12
marker.size, = FloatValue.unpack( data[offset:offset+4] )
offset += 4
# Version 2.6 and later
if( ( self.__natNetStreamVersion[0] == 2 and self.__natNetStreamVersion[1] >= 6 ) or self.__natNetStreamVersion[0] > 2 or major == 0 ):
param, = struct.unpack( 'h', data[offset:offset+2] )
offset += 2
occluded = ( param & 0x01 ) != 0
pointCloudSolved = ( param & 0x02 ) != 0
modelSolved = ( param & 0x04 ) != 0
marker.params = param
msg.labeled_markers.append(marker)
# Version 3.0 and later
if( ( self.__natNetStreamVersion[0] >= 3 ) or major == 0 ):
residual, = FloatValue.unpack( data[offset:offset+4] )
offset += 4
trace( "Residual:", residual )
# Force Plate data (version 2.9 and later)
if( ( self.__natNetStreamVersion[0] == 2 and self.__natNetStreamVersion[1] >= 9 ) or self.__natNetStreamVersion[0] > 2 ):
forcePlateCount, = Int32Value.unpack(data[offset:offset + 4])
offset += 4
trace( "Force Plate Count:", forcePlateCount )
for i in range( 0, forcePlateCount ):
# ID
forcePlateID, = Int32Value.unpack(data[offset:offset + 4])
offset += 4
trace( "Force Plate", i, ":", forcePlateID )
# Channel Count
forcePlateChannelCount, = Int32Value.unpack(data[offset:offset + 4])
offset += 4
# Channel Data
for j in range( 0, forcePlateChannelCount ):
trace( "\tChannel", j, ":", forcePlateID )
forcePlateChannelFrameCount, = Int32Value.unpack(data[offset:offset + 4])
offset += 4
for k in range( 0, forcePlateChannelFrameCount ):
forcePlateChannelVal, = Int32Value.unpack(data[offset:offset + 4])
offset += 4
trace( "\t\t", forcePlateChannelVal )
# Latency
msg.latency, = FloatValue.unpack( data[offset:offset+4] )
offset += 4
# Timecode
msg.timecode, = Int32Value.unpack(data[offset:offset + 4])
offset += 4
msg.timecode_subframe, = Int32Value.unpack(data[offset:offset + 4])
offset += 4
# Timestamp (increased to double precision in 2.7 and later)
if( ( self.__natNetStreamVersion[0] == 2 and self.__natNetStreamVersion[1] >= 7 ) or self.__natNetStreamVersion[0] > 2 ):
msg.timestamp, = DoubleValue.unpack( data[offset:offset+8] )
offset += 8
else:
msg.timestamp, = FloatValue.unpack( data[offset:offset+4] )
offset += 4
# Frame parameters
param, = struct.unpack( 'h', data[offset:offset+2] )
isRecording = ( param & 0x01 ) != 0
trackedModelsChanged = ( param & 0x02 ) != 0
offset += 2
msg.params = param
self.lc.publish('OPTITRACK_FRAMES', msg.encode())