def __unpackDataDescriptions( self, data ):
offset = 0
datasetCount, = Int32Value.unpack(data[offset:offset + 4])
trace( "Dataset count:", datasetCount )
offset += 4
data_descriptions = optitrack_data_descriptions_t()
for i in range( 0, datasetCount ):
type, = Int32Value.unpack(data[offset:offset + 4])
offset += 4
trace( "Data type:", type )
if( type == 0 ):
count, description = self.__unpackMarkerSetDescription( data[offset:] )
offset += count
data_descriptions.marker_sets.append(description)
elif( type == 1 ):
count, description = self.__unpackRigidBodyDescription( data[offset:] )
offset += count
data_descriptions.rigid_bodies.append(description)
elif( type == 2 ):
offset += self.__unpackSkeletonDescription( data[offset:] )
data_descriptions.num_marker_sets = len(data_descriptions.marker_sets)
data_descriptions.num_rigid_bodies = len(data_descriptions.rigid_bodies)
self.lc.publish('OPTITRACK_DATA_DESCRIPTIONS', data_descriptions.encode())
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())
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"
body_desc.id = 100
test_desc_message.rigid_bodies.append(body_desc)
test_desc_message.num_rigid_bodies = len(test_desc_message.rigid_bodies)
class BigOptitrackVisualizer(object):
'''
Usage:
optitrackVis = OptitrackVisualizer('OPTITRACK_CHANNEL_NAME')
# You can enable visualization of edges between optitrack markers,