[solved] Logging Data from a Swarm

[sAz]

Hi everyone,

I am running a collision avoidance algorithm on the drones that I have and I would like to plot the flight trajectory of each drone.

To this purpose I am trying to log the position of each drone during the flight.

I am using the following script for flight control:

Code: Select all

    # imports and other data above...
    
 
       

 lg_vars = {
                  'drone.id' : 'int32_t',
                   'drone.posX': 'float',
                   'drone.posY': 'float',
                   'drone.posZ': 'float',
                }

if __name__ == '__main__':
    cflib.crtp.init_drivers(enable_debug_driver=False) # initialize drivers
    factory = CachedCfFactory(rw_cache='./cache')
    
    with Swarm(uris, factory=factory) as swarm: 
    		lgr = l_and_p.LogAndParam('Position', lg_vars, 100, swarm) 
      	        swarm.parallel(lgr.simple_log_async)
      		  
      		  
    # more code below...
          

In the following class the logging functions are defined:

Code: Select all

# more code above...

class LogAndParam:

    def __init__(self, name_conf, vars, prd, swarm = None):
        self.num_of_qds = 5
        self.lg_stab = LogConfig(name=name_conf, period_in_ms=prd)
        self.recordingLstPos = {str(k) : [] for k in range(0, self.num_of_qds)}
        
        for key in vars:
             self.lg_stab.add_variable(key, vars[key])
    
    
    def log_stab_callback(self, timestamp, data ,logconf):
        kys = np.array(list(data.keys()))
        self.recordingLstPos[str(data['drone.id'])].append( [ float(data[ kys[1] ]), float(data[ kys[2] ]), float(data[ kys[3] ]) ] )



    def simple_log_async(self, scf):
        cf = scf.cf
        cf.log.add_config(self.lg_stab)
        self.lg_stab.data_received_cb.add_callback(self.log_stab_callback)
        self.lg_stab.start()
        time.sleep(0.1)
        
# more code below...

The problem is that at the moment I am able to get the position of only one drone, any ideas on how I can get the positions from each drone?

Thanks

[kristoffer]

You can do something like this

Code: Select all

import cflib.crtp
from cflib.crazyflie.log import LogConfig
from cflib.crazyflie.swarm import Swarm
from cflib.crazyflie.swarm import CachedCfFactory
from cflib.crazyflie.syncLogger import SyncLogger


def log_position(scf):
    lg_stab = LogConfig(name='position', period_in_ms=100)
    lg_stab.add_variable('stateEstimate.x', 'float')
    lg_stab.add_variable('stateEstimate.y', 'float')
    lg_stab.add_variable('stateEstimate.z', 'float')

    uri = scf.cf.link_uri
    with SyncLogger(scf, lg_stab) as logger:
        for log_entry in logger:
            data = log_entry[1]

            x = data['stateEstimate.x']
            y = data['stateEstimate.y']
            z = data['stateEstimate.z']

            print(uri, "is at", x, y, z)


if __name__ == '__main__':
    cflib.crtp.init_drivers(enable_debug_driver=False)

    uris = [
        'radio://0/30',
        # 'radio://0/80',
        # ...
    ]

    factory = CachedCfFactory(rw_cache='./cache')
    with Swarm(uris, factory=factory) as swarm:
        swarm.parallel_safe(log_position)

[sAz]

This solution works fine. However my script would look like this:

Code: Select all

             ...

if __name__ == '__main__':
   
    cflib.crtp.init_drivers(enable_debug_driver=False) # initialize drivers
    factory = CachedCfFactory(rw_cache='./cache')

    with Swarm(uris, factory=factory) as swarm:
    		swarm.parallel_safe(log_position)
    		
    		while True:
    			var = input("insert command:\n")
    		
    		...
            

It blocks my program and I can never reach the while loop (or did I miss something?). I needed something that works in the background while I insert commands.

This is why I use the following solution:

Code: Select all

import cflib.crtp
from cflib.crazyflie import Crazyflie
from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
from cflib.crazyflie.log import LogConfig
from cflib.crazyflie.syncLogger import SyncLogger

uris = [ 
   'radio://0/80/2M/E7E7E7E7E0',
   'radio://0/80/2M/E7E7E7E7E1',
    'radio://0/80/2M/E7E7E7E7E2',
    'radio://0/80/2M/E7E7E7E7E3'
	]
	
def log_stab_callback(uri, timestamp, data, log_conf):
        x = float(data['drone.posX'])
        y = float(data['drone.posY'])
        z = float(data['drone.posZ'])
        print(x,y,z)


def simple_log_async(scf):
    lg_vars = {
        'drone.posX': 'float',
        'drone.posY': 'float',
        'drone.posZ': 'float'
    }

    lg_stab = LogConfig(name='Position', period_in_ms=100)
    for key in lg_vars:
        lg_stab.add_variable(key, lg_vars[key])

    cf = scf.cf
    cf.log.add_config(lg_stab)
    lg_stab.data_received_cb.add_callback(lambda t, d, l: log_stab_callback(cf.link_uri, t, d, l))
    lg_stab.start()
    

if __name__ == '__main__':
   
    cflib.crtp.init_drivers(enable_debug_driver=False) # initialize drivers
    factory = CachedCfFactory(rw_cache='./cache')

    with Swarm(uris, factory=factory) as swarm:
    		swarm.parallel_safe(simple_log_async)
    		
    		while True:
    			var = input("insert command:\n")
    		
    		...

I got the idea to use a lambda function from here viewtopic.php?t=3325

[kristoffer]

The call to swarm.parallel_safe() in the main thread returns when the log_position() returns for all members of the swarm.
Since log_position() loops for ever in this case, swarm.parallel_safe() will block forever.