OptiTrack and Crazyflie Python no ROS
Posted: Tue Feb 15, 2022 11:00 pm
Hi all,
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:
This part works perfectly, I am able to receive the full pose and run this function in its own process to avoid any interference with the radio communication. I then have to send this information to the quadcopter:
I run this function in its own thread to receive the queues and push the information to the quadcopter constantly. There is a mocap deck attached with 4 markers placed in a unique configuration. No other decks are attached. Running a simple hover test, the quadcopter crashes pretty much instantly. Some other things to note: I am using the PID controller via ('stabilizer.controller', 1) and the EKF via ('stabilizer.estimator', 2). My question is, in theory, is this the right way to approach it? My worry is that currently there is custom firmware installed for the class. Should I upload either the default firmware or even the crazyswarm firmware? I am for now only using one quadcopter, although it is possible more will be added in the future.
Thanks for the help!
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!