How to print the position with the swarmSequence.py

Firmware/software/electronics/mechanics
Post Reply
gabriel_lasec
Beginner
Posts: 5
Joined: Wed Jan 23, 2019 5:30 pm

How to print the position with the swarmSequence.py

Post by gabriel_lasec »

Hi all

I have successfully completed the task of automatic swarm flight using the swarmSequence.py, but now I want to print the crazyflie's position.

I tried to change the code, but still failed.Here is the code I added

Code: Select all

def position_callback(timestamp, data, logconf):

    x = data['kalman.stateX']
    y = data['kalman.stateY']
    z = data['kalman.stateZ']
    print('pos: ({},{},{})'.format(x, y, z))
    with open('/home/gabriela/Área de Trabalho/Gabriel' +datetime.datetime.now().strftime('%Y-%m-%d-%H_')+'Callback_position.csv', 'a') as cvsfile:
        writer = csv.writer(csvfile,delimiter=',')
        writer.writerow([x, y, z, timestamp])
    csvfile.close()

def start_position_printing(scf):
    log_conf = LogConfig(name='Position', period_in_ms=50)
    log_conf.add_variable('kalman.stateX', 'float')
    log_conf.add_variable('kalman.stateY', 'float')
    log_conf.add_variable('kalman.stateZ', 'float')

    scf.cf.log.add_config(log_conf)
    log_conf.data_received_cb.add_callback(position_callback)
    log_conf.start()
However, i don't know how to to setup a different log callback for each Crazyflies

This is definitely wrong, someone can help me?I think a lot of people need this function.

thanks!
arnaud
Bitcraze
Posts: 2538
Joined: Tue Feb 06, 2007 12:36 pm

Re: How to print the position with the swarmSequence.py

Post by arnaud »

The code you have written so far looks good to start a log block.

If you call

Code: Select all

swarm.parallel(start_position_printing)
in the with swarm block, you will start to log position for all the drones in the swarm.

The scf.cf object contains the URI of the link, this would allow to identify each Crazyflie in "start_position_printing", then you can use a labmbda function as callback to save the position of each drone independently.
gabrielalima
Beginner
Posts: 10
Joined: Thu Jan 24, 2019 12:15 pm

Re: How to print the position with the swarmSequence.py

Post by gabrielalima »

arnaud wrote: Mon Feb 04, 2019 12:47 pm The code you have written so far looks good to start a log block.

If you call

Code: Select all

swarm.parallel(start_position_printing)
in the with swarm block, you will start to log position for all the drones in the swarm.

The scf.cf object contains the URI of the link, this would allow to identify each Crazyflie in "start_position_printing", then you can use a labmbda function as callback to save the position of each drone independently.
Hi Arnaud,

"The scf.cf object contains the URI of the link, this would allow to identify each Crazyflie in "start_position_printing"
Until here, okay.

But I can not understand how to use the lambda function ...

Through the start_position_printing function, could I create two position_callback? position_callback1 and position_calkback2

Thanks
gabrielalima
Beginner
Posts: 10
Joined: Thu Jan 24, 2019 12:15 pm

Re: How to print the position with the swarmSequence.py

Post by gabrielalima »

Hi!

Now, the acquisition is operating well!

I created two different callbacks: position_callback1 e position_callback2

Code: Select all

def start_position_printing(scf):
   
    if scf.cf.link_uri == 'radio://0/80/2M':
           
        log_conf = LogConfig(name='Position', period_in_ms=500)
        log_conf.add_variable('kalman.stateX', 'float')
        log_conf.add_variable('kalman.stateY', 'float')
        log_conf.add_variable('kalman.stateZ', 'float')

        scf.cf.log.add_config(log_conf)
        log_conf.data_received_cb.add_callback(position_callback1)
        log_conf.start()

    if scf.cf.link_uri == 'radio://0/81/2M':
         
        log_conf = LogConfig(name='Position', period_in_ms=500)
        log_conf.add_variable('kalman.stateX', 'float')
        log_conf.add_variable('kalman.stateY', 'float')
        log_conf.add_variable('kalman.stateZ', 'float')

        scf.cf.log.add_config(log_conf)
        log_conf.data_received_cb.add_callback(position_callback2)
        log_conf.start()
I don't know if there is a more appropriate form, but I guarantee separate reading
arnaud
Bitcraze
Posts: 2538
Joined: Tue Feb 06, 2007 12:36 pm

Re: How to print the position with the swarmSequence.py

Post by arnaud »

You can use lambda function, it is exactly what you need to make that cleaner :-).

This is an example of how it works:

Code: Select all

def callback(uri, data):
    print("Uri: {}, Data: {}".format(uri, data))

def setup_and_call(uri):
    # Make a lambda function cb that takes one argument x, and call the callback with the value of uri as well as the argument
    cb = lambda x: callback(uri, x)
    
    # cb is only called with data
    cb("some data")

setup_and_call("uri1")  # Displays Uri: uri1, Data: Some data
setup_and_call("uri2")  # Displays Uri: uri2, Data: Some data
So in you case it could look something like:

Code: Select all

def position_callback(uri, timestamp, data, logconf):

(...)

log_conf.data_received_cb.add_callback(lambda t, d, l: position_callback(scf.cf.link_uri, t, d, l))

Post Reply