unsure about extpos send mocap data to two crazyflies | drone crashes | unsure about code

[Vrinda73]

Hello!
I am trying to use high level commander, pid controller to control two crazyflies. I was initially facing issue sending mocap data (vicon) to the two crazyflies. I have figured it out but am unsure. Please note I was able to give position setpoints to the crazyflies individually, just unable to run them in parallel fashion. I use cflib python API. I am a beginner apologies for the mistakes I make.
Thank you!
Here is my code:

import sys, traceback
import logging
import time
from threading import Thread

import cflib
from cflib.crazyflie import Crazyflie

from pyvicon.pyvicon import PyVicon
from enum import Enum

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
from cflib.crazyflie.swarm import CachedCfFactory


URI1 = 'radio://0/80/2M/E7E7E7E7E0'
URI2 = 'radio://0/80/2M/E7E7E7E7E2'

# List of URIs, comment the one you do not want to fly

uris = {
    URI1,
    URI2
}

global swarm_helper
swarm_helper = None


###### Mocap data
class StreamMode(Enum):
    ClientPull = 0
    ClientPullPreFetch = 1
    ServerPush = 2

viconClient = PyVicon()
viconClient.connect("192.168.78.11")
viconClient.enable_marker_data()
viconClient.enable_device_data()

r = viconClient.get_frame()
c = viconClient.get_subject_count()

pos1 = viconClient.get_marker_global_translation("crf2","uav1")
print ('pos1: '+ str(pos1))

pos2 = viconClient.get_marker_global_translation("crf2","uav2")
print ('pos2: '+ str(pos2))
   
# mocap

class MocapWrapper(Thread):
    def __init__(self):
        Thread.__init__(self)

        self.on_pose = None
        self._stay_open = True

        self.start()

    def close(self):
        self._stay_open = False

    def run(self):
        while self._stay_open:
            viconClient = PyVicon()
            viconClient.connect("192.168.78.11")
            viconClient.enable_marker_data()
            viconClient.enable_device_data()
            if self.on_pose:
                viconClient.get_frame()
                pos1 = viconClient.get_marker_global_translation("crf2","uav1")
                pos2 = viconClient.get_marker_global_translation("crf2","uav2")
                #print('pos1:' + str(pos1))
                #print('pos2:' + str(pos2))
                self.on_pose([pos1[0]/1000, pos1[1]/1000, pos1[2]/1000, pos2[0]/1000, pos2[1]/1000, pos2[2]/1000])


def send_extpose_yo(x1, y1, z1, x2, y2, z2):

    if(swarm_helper is not None):
        for uri, scf in swarm_helper._cfs.items():
            if uri[-2:] == 'E0':
                scf.cf.extpos.send_extpos(x1, y1, z1)
            else:
                scf.cf.extpos.send_extpos(x2, y2, z2) 
               


    
logging.basicConfig(level=logging.ERROR)
        


def activate_mellinger_controller(scf, use_mellinger):
    controller = 1
    if use_mellinger:
        controller = 2
    scf.cf.param.set_value('stabilizer.controller', controller)


def run_sequence(scf):

    activate_mellinger_controller(scf, False)

    commander = scf.cf.high_level_commander

    commander.takeoff(0.5, 1.0)
    time.sleep(3)

    commander.land(0.0, 1.0)
    time.sleep(2)

    commander.stop()

        
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)
    mocap_wrapper = MocapWrapper()
    factory = CachedCfFactory(rw_cache='./cache')
    #pose = mocap_wrapper.on_pose
    #print(pose)

    with Swarm(uris, factory=factory) as swarm:
        swarm_helper = swarm
        mocap_wrapper.on_pose = lambda pose: send_extpose_yo(pose[0], pose[1], pose[2], pose[3], pose[4], pose[5])
        swarm.reset_estimators()
        swarm.parallel_safe(run_sequence)
        swarm_helper = None

[knmcguire]

Seems like swarm_helper is none so it is not sending any data to the crazyflies yet:
if(swarm_helper is not None):

Before going further, have you tried doing this with just one crazyflie first? The swarm class uses threads which can complicate a lot of things, so it would be nice to know if you can just control one crazyflie first.

Also, have you tried this example as well? https://github.com/bitcraze/crazyflie-lib-python/blob/master/examples/mocap/mocap_hl_commander.py

[Vrinda73]

Thank you Kimberly!
I now am sending attitude setpoints to two crazyflies, but sometimes one turns on other doesn't turn on vice a versa. I had implemented swarmSequence.py without using the cf.commander.send_notify_setpoint_stop() line in the code. Do you think supervisor locked my swarm class ? Note that the drones run perfectly individually.

[krichardsson]

Try to add a short sleep (maybe 0.1 s) after you call cf.commander.send_notify_setpoint_stop() to make sure the radio has time to transmit it, otherwise it might be lost

[Vrinda73]

I tried it Kristoffer! Did not work. I also reflashed the latest firmware and did recovery cold boot with no luck. Note that am using CF 2.0. I think it is in locked state. (Also reinstalled client and cflib)

[krichardsson]

That is easy to check if you are using the latest versions. Connect the Crayzyflie to the client and open the flight tab. At the bottom left corner, to the right of the Arm button you will see an info box, if the Crazyflie is locked the info box should be red and saying "Locked-please reboot".

[Vrinda73]

Thank you Kristoffer and Kimberly for the suggestions. Somehow, it worked today!