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