Crazyradio for Swarm using MOCAP and only the Lib

Post here to get support
Post Reply
pedro.lucas
Beginner
Posts: 7
Joined: Fri Mar 03, 2017 4:57 pm

Crazyradio for Swarm using MOCAP and only the Lib

Post by pedro.lucas » Wed Apr 05, 2017 3:43 pm

Hi,

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



arnaud
Bitcraze
Posts: 1999
Joined: Tue Feb 06, 2007 12:36 pm

Re: Crazyradio for Swarm using MOCAP and only the Lib

Post by arnaud » Mon Apr 10, 2017 1:25 pm

Hi,

I would also suspect a problem with the radio bandwidth. Something you may try is to put all Crazyflie in the same channel but different address, this there will be a little bit less USB communication required since the channel will not have to be changed for each Crazyflie.

Another easy thing to try is to add another Crazyradio: putting two Crazyflie on one radio and two on another one. From your file I guess you already have a second Crazyradio, did you try this setup already?

pedro.lucas
Beginner
Posts: 7
Joined: Fri Mar 03, 2017 4:57 pm

Re: Crazyradio for Swarm using MOCAP and only the Lib

Post by pedro.lucas » Mon Apr 10, 2017 10:24 pm

Hi Arnaud,

Thanks very much for the advice, I changed the URI to use the same channel and data rate, but with a different address and it works very well, however if I add a fourth CF it moves randomly, I also tested by adding another radio and giving other channel to the CFs for that radio and it works as well. In summary, I think I can connect 3 CFs per radio. Now I would like to know if there is a chance to optimize the communication such way I can connect more CFs to one only radio, maybe 5 CFs per radio. What if I deactivate the ack feature?, If this solution considering the ack is relevant, How can I do this (radio and CF firmware modifications, etc)?, or Are there other solutions?

Thanks very much again.

arnaud
Bitcraze
Posts: 1999
Joined: Tue Feb 06, 2007 12:36 pm

Re: Crazyradio for Swarm using MOCAP and only the Lib

Post by arnaud » Tue Apr 11, 2017 9:10 am

Sounds good!

Your experiment proves that one of the bottleneck is the USB protocol to the Crazyradio. There should be way to enhace that a bit but it is not very easy to work on the Crazyradio Firmware.

Other people, like Wolfgang at USC, have used broadcast message to greatly increase the number of copter they could control for each radio. One typical use-case is to send the Crazyflie position from a mocap. Wolfgang is packing multiple position in the same packet and each crazyflie uses its own position from the packet. Broadcast messages have been merged into the crazyradio, crazyflie and crazyflie2-nrf firmware so this should be possible to re-implement by adding a custom localization packet. We have support for 16bit-float in the Crazyflie so it should be possible to send 4 position in each packet. The solution would involve adding the packet in the firmware and in the crazyflie lib.

Post Reply