I'm a student and I've been tasked with integrating OptiTrack and Crazyflie 2.1 quadcopters for future classes, pretty much entirely using Python. We have Motive and the NatNet SDK set up to receive the position (xyz) and the quaternion (qx, qy, qz, qw). A special python script takes these values and transmits them via UDP to a specific ip address, where a client python file unpacks the data via socket and places it into a queue:
Code: Select all
def optitrack(queue: Queue, run_process: Value):
logging.info('Beginning socket listener')
with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as s:
s.bind(('0.0.0.0', int(CLIENT_PORT)))
logging.info(f'Starting socket listener')
while run_process.value == 1:
data = s.recvfrom(1024)[0]
if not data:
logging.info(f'No data received')
break
else:
[a, b, c, d, e, f, g, h, i, j, k, l] = struct.unpack('ffffffffffff', data)
x = -a
y = c
z = b
qx = d
qy = e
qz = f
qw = g
roll = -h
yaw = i
pitch = -j
bodyID = k
framecount = l
print(f'x = {x}, y = {y}, z = {z} \n qx = {qx}, qy = {qy}, qz = {qz}, qw = {qw} \n roll = {roll}, yaw = {yaw}, pitch = {pitch} \n bodyID = {bodyID}, framecount = {framecount}')
if queue.empty():
queue.put((x, y, z, qx, qy, qz, qw))
Code: Select all
def send_pose(client, queue: Queue):
logging.info('sending full pose')
while client.is_connected:
x, y, z, qx, qy, qz, qw = queue.get()
client.cf.extpos.send_extpose(x, y, z, qx, qy, qz, qw)
# time.sleep(5)
Thanks for the help!