I'm trying to fly a swarm of CFs using a MOCAP system and only the crazyflie-lib-python, everything on Windows. I changed the mode for the CFs to Kalman and I modified the autonomousSequence.py script to support the communication between the CFs and the mocap by using the cf.extpos.send_extpos function. It works very well with one CF, even two; but, when I add a third one, the behaviour is erratic and the CFs move randomly, even sometimes they go up higher than the setpoint that is sent. Also, I have tested considering that two of them fly to some point and other is just quiet with a setpoint with zero thrust and the same happens. The three CFs works well individually, that it is why I think this is a problem from radio communication. Is the ack strategy something that could affect significantly ?, It is a problem that everything is running under windows ?, or Is there any consideration for communicating several CFs with this setup ?. I attach the code that I'm using. Thanks very much.
Code: Select all
import sys, traceback
import logging
import time
from threading import Thread
import cflib
from cflib.crazyflie import Crazyflie
from NatNetClient import NatNetClient
import numpy as np
import cflib.crtp
from cflib.crazyflie.log import LogConfig
from cflib.crazyflie.swarm import Swarm
from cflib.crazyflie.syncLogger import SyncLogger
#Identifiers
URI0 = "radio://0/070/2M/E7E7E7E7E7"
URI1 = "radio://0/080/2M/E7E7E7E7E8"
URI2 = "radio://1/060/2M/E7E7E7E7E6"
URI3 = "radio://0/100/2M"
#uris = {URI0, URI1, URI2, URI3}
uris = {URI0,URI2,URI3}
mocap_identifiers = {
1: URI0,
2: URI1,
3: URI2,
4: URI3,
}
global swarm_helper
swarm_helper = None
# x y z YAW (mocap coord system)
sequence0 = [(0, 0.3, 0, 0),
(0, 0.03, 0, 0)]
sequence1 = [(0, 0.3, 0.5, 0),
(0, 0.03, 0.5, 0)]
sequence2 = [(0, 0, 1, 0),
(0, 0, 1, 0)]
sequence3 = [(0, 0.3, 1.25, 0),
(0, 0.03, 1.25, 0)]
seq_args = {
URI0: [sequence0],
URI1: [sequence1],
URI2: [sequence2],
URI3: [sequence3],
}
# This is a callback function that gets connected to the NatNet client and called once per mocap frame.
def receiveNewFrame( frameNumber, markerSetCount, unlabeledMarkersCount, rigidBodyCount, skeletonCount,
labeledMarkerCount, latency, timecode, timecodeSub, timestamp, isRecording, trackedModelsChanged ):
#print( "Received frame", frameNumber )
pass
# This is a callback function that gets connected to the NatNet client. It is called once per rigid body per frame
def receiveRigidBodyFrame( id, position, rotation ):
if(swarm_helper is not None):
for uri, scf in swarm_helper._cfs.items():
if(mocap_identifiers[id] == uri):
x= position[0]
y= position[1]
z= position[2]
scf.cf.extpos.send_extpos(z, x, y)
#print (position)
break
# This will create a new NatNet client
streamingClient = NatNetClient()
# Configure the streaming client to call our rigid body handler on the emulator to send data out.
streamingClient.newFrameListener = receiveNewFrame
streamingClient.rigidBodyListener = receiveRigidBodyFrame
# Start up the streaming client now that the callbacks are set up.
# This will run perpetually, and operate on a separate thread.
#streamingClient.run()
logging.basicConfig(level=logging.ERROR)
def wait_for_position_estimator(scf):
print('Waiting for estimator to find position...')
log_config = LogConfig(name='Kalman Variance', period_in_ms=500)
log_config.add_variable('kalman.varPX', 'float')
log_config.add_variable('kalman.varPY', 'float')
log_config.add_variable('kalman.varPZ', 'float')
var_y_history = [1000] * 10
var_x_history = [1000] * 10
var_z_history = [1000] * 10
threshold = 0.001
with SyncLogger(scf, log_config) as logger:
for log_entry in logger:
data = log_entry[1]
var_x_history.append(data['kalman.varPX'])
var_x_history.pop(0)
var_y_history.append(data['kalman.varPY'])
var_y_history.pop(0)
var_z_history.append(data['kalman.varPZ'])
var_z_history.pop(0)
min_x = min(var_x_history)
max_x = max(var_x_history)
min_y = min(var_y_history)
max_y = max(var_y_history)
min_z = min(var_z_history)
max_z = max(var_z_history)
# print("{} {} {}".
# format(max_x - min_x, max_y - min_y, max_z - min_z))
if (max_x - min_x) < threshold and (
max_y - min_y) < threshold and (
max_z - min_z) < threshold:
break
def reset_estimator(scf):
cf = scf.cf
cf.param.set_value('kalman.resetEstimation', '1')
time.sleep(0.1)
cf.param.set_value('kalman.resetEstimation', '0')
wait_for_position_estimator(cf)
def run_sequence(scf, sequence):
cf = scf.cf
cf.param.set_value('flightmode.posSet', '1')
time.sleep(0.1)
for position in sequence:
print('Setting position {}'.format(position))
for i in range(50):
cf.commander.send_setpoint(position[0], position[2],
position[3],
int(position[1] * 1000))
time.sleep(0.1)
cf.commander.send_setpoint(0, 0, 0, 0)
# Make sure that the last packet leaves before the link is closed
# since the message queue is not flushed before closing
time.sleep(0.1)
print("Finish")
if __name__ == '__main__':
# Initialize the low-level drivers (don't list the debug drivers)
cflib.crtp.init_drivers(enable_debug_driver=False)
time.sleep(1)
streamingClient.run()
time.sleep(0.5)
with Swarm(uris) as swarm:
swarm_helper = swarm
swarm.parallel(reset_estimator)
swarm.parallel(run_sequence, args_dict=seq_args)
swarm_helper = None