[Solved] Commander.py with Swarm

Discussions about autonomous flight in general, regardless of positioning method
Post Reply
youngbin1130
Member
Posts: 46
Joined: Thu Dec 12, 2019 4:28 am

[Solved] Commander.py with Swarm

Post by youngbin1130 »

Hi once again.

I would like to combine the commander.py with a swarm code but I'm not sure how i should go about doing it.

So far I've tried the following

1. in the run_sequence function, i add a URI input parameter adn check if the URI is equal to a certain string. This gave me an exception error in syncCrazyflie.py due to the added input.

2. Add a for-loop to check the array of URIs in __name__, and compare thtat to an URI string. Same error as above.

3. The hard coded method would be to replace the "with Swarm(uris, factory=factory) as swarm" to "with SyncCrazyflie..." and replace the individual URIs with the drones I have.

I'm honestly not so sure what the best approach for this is. I've attached my code as a reference if needed.

Cheers.

Code: Select all

# -*- coding: utf-8 -*-
#
#     ||          ____  _ __
#  +------+      / __ )(_) /_______________ _____  ___
#  | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
#  +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
#   ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
#
#  Copyright (C) 2017-2018 Bitcraze AB
#
#  Crazyflie Nano Quadcopter Client
#
#  This program is free software; you can redistribute it and/or
#  modify it under the terms of the GNU General Public License
#  as published by the Free Software Foundation; either version 2
#  of the License, or (at your option) any later version.
#
#  This program is distributed in the hope that it will be useful,
#  but WITHOUT ANY WARRANTY; without even the implied warranty of
#  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
#  GNU General Public License for more details.
#  You should have received a copy of the GNU General Public License
#  along with this program; if not, write to the Free Software
#  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
#  MA  02110-1301, USA.
"""
Version of the AutonomousSequence.py example connecting to 10 Crazyflies.
The Crazyflies go straight up, hover a while and land but the code is fairly
generic and each Crazyflie has its own sequence of setpoints that it files
to.
The layout of the positions:
    x2      x1      x0
y3  10              4
            ^ Y
            |
y2  9       6       3
            |
            +------> X
y1  8       5       2
y0  7               1
"""
import time
import math
#from vectors import Point, Vector
import vectormath as vmath
import numpy
import cflib.crtp
from cflib.crazyflie import Crazyflie
from cflib.crazyflie.log import LogConfig
from cflib.crazyflie.swarm import CachedCfFactory
from cflib.crazyflie.swarm import Swarm
from cflib.crazyflie.syncLogger import SyncLogger
from cflib.crazyflie.syncCrazyflie import SyncCrazyflie

# Import MotionCommander
from cflib.positioning.motion_commander import MotionCommander
from cflib.crazyflie.commander import Commander
# Bring in CSV
import csv

# Change uris and sequences according to your setup
URI1 = 'radio://0/40/1M'
URI2 = 'radio://0/80/2M'
URI3 = 'radio://0/78/2M'
URI4 = 'radio://0/33/1M'
URI5 = 'radio://0/70/2M/E7E7E7E705'
URI6 = 'radio://0/70/2M/E7E7E7E706'
URI7 = 'radio://0/70/2M/E7E7E7E707'
URI8 = 'radio://0/70/2M/E7E7E7E708'
URI9 = 'radio://0/70/2M/E7E7E7E709'
URI10 = 'radio://0/70/2M/E7E7E7E70A'

#    x   y   z  time
sequence1 = [
    (0.3, 0.3, 0.5, 3.0),
    (0.3, 0.3, 0.5, 5.0),
    (0.3, 0.3, 0.5, 3.0),
]



sequence3 = [
    (0.5, 0.5, 0.5, 3.0),
    (0.5, 0.5, 0.5, 5.0),
    (0.5, 0.5, 0.5, 3.0),
]

sequence2 = [
    (1.0, 1.0, 0.5, 3.0),
    (1.0, 1.0, 0.5, 5.0),
    (1.0, 1.0, 0.5, 3.0),
]

sequence4 = [
    (0.5, 1.5, 0.5, 3.0),
    (0.5, 1.5, 0.5, 3.0),
    (0.5, 1.5, 0.5, 3.0),
]

seq_args = {
    URI1: [sequence1],
    URI2: [sequence2],
    URI3: [sequence3],
    URI4: [sequence4],

}


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

# URI1 is the TARGET

uris = {
#    URI1,
#    URI2,
#    URI3,
    URI4,
#    URI5,
#    URI6,
#    URI7,
#    URI8,
#    URI9,
#    URI10
}


location = {} # Dictionary of URI and Location
rotation = {} # Dictionaty of URI and rotation
g_distance = {} # Nested Dicationary of Drone A to Drone B~X
g_bearing = {} #Nested Dictionary of Drone A to Drone B~X

            # [dist_tgt, bearing_tgt, dist_agt, bearing_agt, orient_agt]

inputLM =   [0,0,0,0,0]

for x in uris:
    g_distance[x]={}

for x in uris:
    g_bearing[x] = {}

def wait_for_position_estimator(scf):
    print('Waiting for estimator to find position...')

    log_config = LogConfig(name='Stabilizer', period_in_ms=100)
    log_config.add_variable('stabilizer.roll', 'float')
    log_config.add_variable('stabilizer.pitch', 'float')
    log_config.add_variable('stabilizer.yaw', '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:
        endTime = time.time() + 10

        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 wait_for_param_download(scf):
    while not scf.cf.param.is_updated:
        time.sleep(1.0)
    print('Parameters downloaded for', scf.cf.link_uri)


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):
    pass


def position_callback(uri, timestamp, data, logconf):
    x = data['kalman.stateX']
    y = data['kalman.stateY']
    z = data['kalman.stateZ']

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

    point = (x,y,z)


    location[uri] = point # {'radio://0/33/1M': (-3.2653462886810303, -1.9112612009048462, 0.020919203758239746), 'radio://0/40/1M': (-4.532137870788574, -0.9649770855903625, 0.047263506799936295)



    for k,v in location.items():

        # Calculate distance for all drones
        _d = calculate_distance(point,v)
        g_distance[uri][k]= _d
        g_distance[k][uri]= _d

    #FormLMArray(uri,g_distance)
def TargetMove(scf):

    for y in range(10):
        cf.commander.send_hover_setpoint(0, 0, 0, y / 25)
        time.sleep(0.1)

    for y in range(10):
        cf.commander.send_hover_setpoint(0, 0, 0, (10 - y) / 25)
        time.sleep(0.1)

    cf.commander.send_stop_setpoint()

def FormLMArray(uri,d_dict):

    # Form list of keys and values
    key_list = list(d_dict[uri].keys())
    val_list = list(d_dict[uri].values())

    # Find Distance to target
    dist_tgt = d_dict[uri]['radio://0/40/1M']

    # Find Bearing to Target
    bearing_tgt = CalculateBearing(uri,location[uri], location[URI1])

    # find shortest agents and its URI
    dist_agt = min(filter(lambda x: x>0.00, val_list)) # Distance to nearest agent
    dist_agt_uri = key_list[val_list.index(dist_agt)]  # nearest agent URI

    # Find bearing for shortest agent
    bearing_agt = CalculateBearing(uri,dist_agt_uri)

    inputLM[0] = dist_tgt
    inputLM[1] = bearing_tgt
    inputLM[2] = dist_agt
    inputLM[3] = bearing_agt
    inputLM[4] = rotation[dist_agt_uri][2] #get YAW value

    return(inputLM)

def stablizer_callback(uri, timestamp, data, logconf):
    roll = data['stabilizer.roll']
    pitch = data['stabilizer.pitch']
    yaw = data['stabilizer.yaw']

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

    stabilizer = (roll, pitch, yaw)

    rotation[uri] = stabilizer
    return(stabilizer)

    #CalculateStabDiff(rotation[URI2],rotation[URI3])

    # Get Position Data
def GetPosition(scf):

    log_conf = LogConfig(name='Position', period_in_ms=100)
    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(lambda timestamp, data, logconf: position_callback(scf.cf.link_uri, timestamp, data, logconf))
    log_conf.start()

    # Get Roll, Pitch, Yaw
def GetStabilizer(scf):
    log_conf = LogConfig(name='Stablizer',period_in_ms=100)
    log_conf.add_variable('stabilizer.roll', 'float')
    log_conf.add_variable('stabilizer.pitch', 'float')
    log_conf.add_variable('stabilizer.yaw', 'float')
    scf.cf.log.add_config(log_conf)

    log_conf.data_received_cb.add_callback(lambda timestamp, data, logconf: stablizer_callback(scf.cf.link_uri, timestamp, data, logconf))
    log_conf.start()


    # Calculate Distance Between Two Agents
def calculate_distance(p1, p2):
    diff = [0,0,0]
    diff[0] = p1[0]-p2[0]
    diff[1] = p1[1]-p2[1]
    diff[2] = p1[2]-p2[2]

    result = (math.sqrt( (diff[0]*diff[0]) + (diff[1]*diff[1]) + (diff[2]*diff[2]) ))
    return(result)

def OrientationDifference(p1,p2):
    print("aa")

    # Calculate Bearing with Drone Data
    # Variables needed D2-D1, Forward Vector of D1

def CalculateBearing(uri, p1,p2):

    nVector1 = vmath.Vector2(0,0)
    YawInRad = numpy.radians(rotation[uri][2])
    nVector1[0] = math.cos(YawInRad)
    nVector1[1] = math.sin(YawInRad)

    # Finding Directional Vector from D1 to D2
    Vector2 = vmath.Vector2(0,0)
    Vector2[0] = p2[0] - p1[0]
    Vector2[1] = p2[1] - p1[1]

    angle = nVector1.angle(Vector2,unit='deg')

    return(angle)


if __name__ == '__main__':
    #logging.basicConfig(level=logging.DEBUG)
    factory = CachedCfFactory(rw_cache='./cache')
    cflib.crtp.init_drivers(enable_debug_driver=False)

    """
    with Swarm(uris,factory=factory) as swarm:

        # If the copters are started in their correct positions this is
        # probably not needed. The Kalman filter will have time to converge
        # any way since it takes a while to start them all up and connect. We
        # keep the code here to illustrate how to do it.
        swarm.parallel(reset_estimator)

        # The current values of all parameters are downloaded as a part of the
        # connections sequence. Since we have 10 copters this is clogging up
        # communication and we have to wait for it to finish before we start
        # flying.

        print('Waiting for parameters to be downloaded...')
        swarm.parallel(wait_for_param_download)
        #swarm.parallel(run_sequence, args_dict=seq_args)
        for uri in uris:
    """
    with SyncCrazyflie(URI4, cf = Crazyflie(rw_cache='./cache')) as scf:
        cf = scf.cf
        if URI4 == 'radio://0/33/1M':
            for y in range(10):
                cf.commander.send_hover_setpoint(0, 0, 0, y / 25)
                time.sleep(0.1)
            for _ in range(50):
                cf.commander.send_hover_setpoint(0.3, 0, 72, 0.4)
                time.sleep(0.1)

            for _ in range(50):
                cf.commander.send_hover_setpoint(0.3, 0, -36 * 2, 0.4)
                time.sleep(0.1)

            for _ in range(20):
                cf.commander.send_hover_setpoint(0, 0, 0, 0.4)
                time.sleep(0.1)

            # Land
            for y in range(10):
                cf.commander.send_hover_setpoint(0, 0, 0, (10 - y) / 25)
                time.sleep(0.1)

            cf.commander.send_stop_setpoint()

Last edited by youngbin1130 on Sat Jul 11, 2020 3:52 pm, edited 1 time in total.
kimberly
Bitcraze
Posts: 1050
Joined: Fri Jul 06, 2018 11:13 am

Re: Commander.py with Swarm

Post by kimberly »

Hi,

So it will still be advised to use the swarm parallel threading as indicated in the original example. If I understand correctly, you want to know in run_sequence with which crazyflie you are dealing with, right?

Your code is just showing the one for option 3, but how did you add the URI parameter in run_sequence in option 1? I will check if I can recreate this and see if I'm able to provide a solution for you.
youngbin1130
Member
Posts: 46
Joined: Thu Dec 12, 2019 4:28 am

Re: Commander.py with Swarm

Post by youngbin1130 »

Hi Kimberly,

Thanks for the reply.

For option 1, this is what I did (or something similar).

Code: Select all

# -*- coding: utf-8 -*-
#
#     ||          ____  _ __
#  +------+      / __ )(_) /_______________ _____  ___
#  | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
#  +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
#   ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
#
#  Copyright (C) 2017-2018 Bitcraze AB
#
#  Crazyflie Nano Quadcopter Client
#
#  This program is free software; you can redistribute it and/or
#  modify it under the terms of the GNU General Public License
#  as published by the Free Software Foundation; either version 2
#  of the License, or (at your option) any later version.
#
#  This program is distributed in the hope that it will be useful,
#  but WITHOUT ANY WARRANTY; without even the implied warranty of
#  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
#  GNU General Public License for more details.
#  You should have received a copy of the GNU General Public License
#  along with this program; if not, write to the Free Software
#  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
#  MA  02110-1301, USA.
"""
Version of the AutonomousSequence.py example connecting to 10 Crazyflies.
The Crazyflies go straight up, hover a while and land but the code is fairly
generic and each Crazyflie has its own sequence of setpoints that it files
to.
The layout of the positions:
    x2      x1      x0
y3  10              4
            ^ Y
            |
y2  9       6       3
            |
            +------> X
y1  8       5       2
y0  7               1
"""
import time
import math
#from vectors import Point, Vector
import vectormath as vmath
import numpy
import cflib.crtp
from cflib.crazyflie import Crazyflie
from cflib.crazyflie.log import LogConfig
from cflib.crazyflie.swarm import CachedCfFactory
from cflib.crazyflie.swarm import Swarm
from cflib.crazyflie.syncLogger import SyncLogger
from cflib.crazyflie.syncCrazyflie import SyncCrazyflie

# Import MotionCommander
from cflib.positioning.motion_commander import MotionCommander
from cflib.crazyflie.commander import Commander
# Bring in CSV
import csv

# Change uris and sequences according to your setup
URI1 = 'radio://0/40/1M'
URI2 = 'radio://0/80/2M'
URI3 = 'radio://0/78/2M'
URI4 = 'radio://0/33/1M'
URI5 = 'radio://0/70/2M/E7E7E7E705'
URI6 = 'radio://0/70/2M/E7E7E7E706'
URI7 = 'radio://0/70/2M/E7E7E7E707'
URI8 = 'radio://0/70/2M/E7E7E7E708'
URI9 = 'radio://0/70/2M/E7E7E7E709'
URI10 = 'radio://0/70/2M/E7E7E7E70A'

#    x   y   z  time
sequence1 = [
    (0.3, 0.3, 0.5, 3.0),
    (0.3, 0.3, 0.5, 5.0),
    (0.3, 0.3, 0.5, 3.0),
]



sequence3 = [
    (0.5, 0.5, 0.5, 3.0),
    (0.5, 0.5, 0.5, 5.0),
    (0.5, 0.5, 0.5, 3.0),
]

sequence2 = [
    (1.0, 1.0, 0.5, 3.0),
    (1.0, 1.0, 0.5, 5.0),
    (1.0, 1.0, 0.5, 3.0),
]

sequence4 = [
    (0.5, 1.5, 0.5, 3.0),
    (0.5, 1.5, 0.5, 3.0),
    (0.5, 1.5, 0.5, 3.0),
]

seq_args = {
    URI1: [sequence1],
    URI2: [sequence2],
    URI3: [sequence3],
    URI4: [sequence4],

}


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

# URI1 is the TARGET

uris = {
#    URI1,
#    URI2,
#    URI3,
    URI4,
#    URI5,
#    URI6,
#    URI7,
#    URI8,
#    URI9,
#    URI10
}


location = {} # Dictionary of URI and Location
rotation = {} # Dictionaty of URI and rotation
g_distance = {} # Nested Dicationary of Drone A to Drone B~X
g_bearing = {} #Nested Dictionary of Drone A to Drone B~X

            # [dist_tgt, bearing_tgt, dist_agt, bearing_agt, orient_agt]

inputLM =   [0,0,0,0,0]

for x in uris:
    g_distance[x]={}

for x in uris:
    g_bearing[x] = {}

def wait_for_position_estimator(scf):
    print('Waiting for estimator to find position...')

    log_config = LogConfig(name='Stabilizer', period_in_ms=100)
    log_config.add_variable('stabilizer.roll', 'float')
    log_config.add_variable('stabilizer.pitch', 'float')
    log_config.add_variable('stabilizer.yaw', '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:
        endTime = time.time() + 10

        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 wait_for_param_download(scf):
    while not scf.cf.param.is_updated:
        time.sleep(1.0)
    print('Parameters downloaded for', scf.cf.link_uri)


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, uri):
    if (uri)  == URI2:
    	# do something
    if (uri) == URI3:
    	#do another thing
    	
def position_callback(uri, timestamp, data, logconf):
    x = data['kalman.stateX']
    y = data['kalman.stateY']
    z = data['kalman.stateZ']

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

    point = (x,y,z)


    location[uri] = point # {'radio://0/33/1M': (-3.2653462886810303, -1.9112612009048462, 0.020919203758239746), 'radio://0/40/1M': (-4.532137870788574, -0.9649770855903625, 0.047263506799936295)



    for k,v in location.items():

        # Calculate distance for all drones
        _d = calculate_distance(point,v)
        g_distance[uri][k]= _d
        g_distance[k][uri]= _d

    #FormLMArray(uri,g_distance)
def TargetMove(scf):

    for y in range(10):
        cf.commander.send_hover_setpoint(0, 0, 0, y / 25)
        time.sleep(0.1)

    for y in range(10):
        cf.commander.send_hover_setpoint(0, 0, 0, (10 - y) / 25)
        time.sleep(0.1)

    cf.commander.send_stop_setpoint()

def FormLMArray(uri,d_dict):

    # Form list of keys and values
    key_list = list(d_dict[uri].keys())
    val_list = list(d_dict[uri].values())

    # Find Distance to target
    dist_tgt = d_dict[uri]['radio://0/40/1M']

    # Find Bearing to Target
    bearing_tgt = CalculateBearing(uri,location[uri], location[URI1])

    # find shortest agents and its URI
    dist_agt = min(filter(lambda x: x>0.00, val_list)) # Distance to nearest agent
    dist_agt_uri = key_list[val_list.index(dist_agt)]  # nearest agent URI

    # Find bearing for shortest agent
    bearing_agt = CalculateBearing(uri,dist_agt_uri)

    inputLM[0] = dist_tgt
    inputLM[1] = bearing_tgt
    inputLM[2] = dist_agt
    inputLM[3] = bearing_agt
    inputLM[4] = rotation[dist_agt_uri][2] #get YAW value

    return(inputLM)

def stablizer_callback(uri, timestamp, data, logconf):
    roll = data['stabilizer.roll']
    pitch = data['stabilizer.pitch']
    yaw = data['stabilizer.yaw']

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

    stabilizer = (roll, pitch, yaw)

    rotation[uri] = stabilizer
    return(stabilizer)

    #CalculateStabDiff(rotation[URI2],rotation[URI3])

    # Get Position Data
def GetPosition(scf):

    log_conf = LogConfig(name='Position', period_in_ms=100)
    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(lambda timestamp, data, logconf: position_callback(scf.cf.link_uri, timestamp, data, logconf))
    log_conf.start()

    # Get Roll, Pitch, Yaw
def GetStabilizer(scf):
    log_conf = LogConfig(name='Stablizer',period_in_ms=100)
    log_conf.add_variable('stabilizer.roll', 'float')
    log_conf.add_variable('stabilizer.pitch', 'float')
    log_conf.add_variable('stabilizer.yaw', 'float')
    scf.cf.log.add_config(log_conf)

    log_conf.data_received_cb.add_callback(lambda timestamp, data, logconf: stablizer_callback(scf.cf.link_uri, timestamp, data, logconf))
    log_conf.start()


    # Calculate Distance Between Two Agents
def calculate_distance(p1, p2):
    diff = [0,0,0]
    diff[0] = p1[0]-p2[0]
    diff[1] = p1[1]-p2[1]
    diff[2] = p1[2]-p2[2]

    result = (math.sqrt( (diff[0]*diff[0]) + (diff[1]*diff[1]) + (diff[2]*diff[2]) ))
    return(result)

def OrientationDifference(p1,p2):
    print("aa")

    # Calculate Bearing with Drone Data
    # Variables needed D2-D1, Forward Vector of D1

def CalculateBearing(uri, p1,p2):

    nVector1 = vmath.Vector2(0,0)
    YawInRad = numpy.radians(rotation[uri][2])
    nVector1[0] = math.cos(YawInRad)
    nVector1[1] = math.sin(YawInRad)

    # Finding Directional Vector from D1 to D2
    Vector2 = vmath.Vector2(0,0)
    Vector2[0] = p2[0] - p1[0]
    Vector2[1] = p2[1] - p1[1]

    angle = nVector1.angle(Vector2,unit='deg')

    return(angle)


if __name__ == '__main__':
    #logging.basicConfig(level=logging.DEBUG)
    factory = CachedCfFactory(rw_cache='./cache')
    cflib.crtp.init_drivers(enable_debug_driver=False)

    """
    with Swarm(uris,factory=factory) as swarm:

        # If the copters are started in their correct positions this is
        # probably not needed. The Kalman filter will have time to converge
        # any way since it takes a while to start them all up and connect. We
        # keep the code here to illustrate how to do it.
        swarm.parallel(reset_estimator)

        # The current values of all parameters are downloaded as a part of the
        # connections sequence. Since we have 10 copters this is clogging up
        # communication and we have to wait for it to finish before we start
        # flying.

        print('Waiting for parameters to be downloaded...')
        swarm.parallel(wait_for_param_download)
        #swarm.parallel(run_sequence, args_dict=seq_args)
        for uri in uris:
    """
    with SyncCrazyflie(URI4, cf = Crazyflie(rw_cache='./cache')) as scf:
        cf = scf.cf
        if URI4 == 'radio://0/33/1M':
            for y in range(10):
                cf.commander.send_hover_setpoint(0, 0, 0, y / 25)
                time.sleep(0.1)
            for _ in range(50):
                cf.commander.send_hover_setpoint(0.3, 0, 72, 0.4)
                time.sleep(0.1)

            for _ in range(50):
                cf.commander.send_hover_setpoint(0.3, 0, -36 * 2, 0.4)
                time.sleep(0.1)

            for _ in range(20):
                cf.commander.send_hover_setpoint(0, 0, 0, 0.4)
                time.sleep(0.1)

            # Land
            for y in range(10):
                cf.commander.send_hover_setpoint(0, 0, 0, (10 - y) / 25)
                time.sleep(0.1)

            cf.commander.send_stop_setpoint()
In the run_seuqence, I wasn't sure how I should get the URI into the function.

I've tried studying the example/swarmSqeuence.py from the Git and for some reason, the drones in the outer circle doesn't in a full circle but rather into one direction and just drops. I'll attach a video for your reference. (https://youtu.be/FBU4Q17jSmc)
kimberly
Bitcraze
Posts: 1050
Joined: Fri Jul 06, 2018 11:13 am

Re: Commander.py with Swarm

Post by kimberly »

So I investigated it a little bit, and you are able to get the URI in run_sequence as follows (here as a print):

Code: Select all

 print(scf.cf.link_uri)  
This was given in this forum thread: viewtopic.php?f=19&t=4006&p=18229&hilit ... arm#p18229

This will solve your uri problem but if you would like to add more arguments to run sequence, you will need to add more arguments in a python dict structure, of which swarmSequenceCircle.py is a nice example of.


What is going on with the swarmSequence.py I don't know exactly... actually they are only supposed to take off and land again. SwarmSequenceCircle.py is the one that makes them go in a circle. But it is indeed odd that it would even move forward in that case... you have tried to original code unmodified from the repository?
youngbin1130
Member
Posts: 46
Joined: Thu Dec 12, 2019 4:28 am

Re: Commander.py with Swarm

Post by youngbin1130 »

Thank you for your reply once again.

The goal of my code is to move the drones according to the velocity output of the machine learning model using distance between drones and orienation. I've attached the much updated version here if you are interested in seeing it (and hopefully give tips about about a better way of doing certain things).

Code: Select all

# -*- coding: utf-8 -*-
#
#     ||          ____  _ __
#  +------+      / __ )(_) /_______________ _____  ___
#  | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
#  +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
#   ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
#
#  Copyright (C) 2017-2018 Bitcraze AB
#
#  Crazyflie Nano Quadcopter Client
#
#  This program is free software; you can redistribute it and/or
#  modify it under the terms of the GNU General Public License
#  as published by the Free Software Foundation; either version 2
#  of the License, or (at your option) any later version.
#
#  This program is distributed in the hope that it will be useful,
#  but WITHOUT ANY WARRANTY; without even the implied warranty of
#  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
#  GNU General Public License for more details.
#  You should have received a copy of the GNU General Public License
#  along with this program; if not, write to the Free Software
#  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
#  MA  02110-1301, USA.
"""
Version of the AutonomousSequence.py example connecting to 10 Crazyflies.
The Crazyflies go straight up, hover a while and land but the code is fairly
generic and each Crazyflie has its own sequence of setpoints that it files
to.
The layout of the positions:
    x2      x1      x0
y3  10              4
            ^ Y
            |
y2  9       6       3
            |
            +------> X
y1  8       5       2
y0  7               1
"""
import time
import math
#from vectors import Point, Vector
import vectormath as vmath
import numpy
import cflib.crtp
from cflib.crazyflie import Crazyflie
from cflib.crazyflie.log import LogConfig
from cflib.crazyflie.swarm import CachedCfFactory
from cflib.crazyflie.swarm import Swarm
from cflib.crazyflie.syncLogger import SyncLogger
from cflib.crazyflie.syncCrazyflie import SyncCrazyflie

# Import MotionCommander
from cflib.positioning.motion_commander import MotionCommander
# Bring in CSV
import csv


####################### start of changes by darrence ##################################
import importlib
mod = importlib.import_module('DSOInferenceModule-v1')
no_of_agents = 3
path_pb = "D:\DSO-AISwarm\crazyflie-lib-python\examples\swarm\DSOfrozen_graph_def.pb"
infer = mod.InferenceModule(path_pb)
observations = []
furtherest_allowable_distance = 0.7  #this is dm in slide 7
####################### end of changes ##################################
# Change uris and sequences according to your setup
URI1 = 'radio://0/40/1M'
URI2 = 'radio://0/33/1M/E7E7E7E7E6'
URI3 = 'radio://0/78/2M'
URI4 = 'radio://0/33/1M'
URI5 = 'radio://0/70/2M/E7E7E7E705'
URI6 = 'radio://0/70/2M/E7E7E7E706'
URI7 = 'radio://0/70/2M/E7E7E7E707'
URI8 = 'radio://0/70/2M/E7E7E7E708'
URI9 = 'radio://0/70/2M/E7E7E7E709'
URI10 = 'radio://0/70/2M/E7E7E7E70A'

#    x   y   z  time
sequence1 = [
    (0.3, 0.3, 0.5, 3.0),
    (0.3, 0.3, 0.5, 5.0),
    (0.3, 0.3, 0.5, 3.0),
]



sequence3 = [
    (0.5, 0.5, 0.5, 3.0),
    (0.5, 0.5, 0.5, 5.0),
    (0.5, 0.5, 0.5, 3.0),
]

sequence2 = [
    (1.0, 1.0, 0.5, 3.0),
    (1.0, 1.0, 0.5, 5.0),
    (1.0, 1.0, 0.5, 3.0),
]

sequence4 = [
    (0.5, 1.5, 0.5, 3.0),
    (0.5, 1.5, 0.5, 3.0),
    (0.5, 1.5, 0.5, 3.0),
]
"""
sequence5 = [
    (1.0, 1.5, 0.5, 3.0),
    (1.0, 1.5, 0.5, 3.0),
    (1.0, 1.5, 0.5, 3.0),
]

sequence6 = [
    (x1, y2, z0, 3.0),
    (x1, y2, z, 30.0),
    (x1, y2, z0, 3.0),
]

sequence7 = [
    (x2, y0, z0, 3.0),
    (x2, y0, z, 30.0),
    (x2, y0, z0, 3.0),
]

sequence8 = [
    (x2, y1, z0, 3.0),
    (x2, y1, z, 30.0),
    (x2, y1, z0, 3.0),
]

sequence9 = [
    (x2, y2, z0, 3.0),
    (x2, y2, z, 30.0),
    (x2, y2, z0, 3.0),
]

sequence10 = [
    (x2, y3, z0, 3.0),
    (x2, y3, z, 30.0),
    (x2, y3, z0, 3.0),
]

"""
seq_args = {
    URI1: [sequence1],
    URI2: [sequence2],
    URI3: [sequence3],
    URI4: [sequence4],

}


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

# URI1 is the TARGET

uris = {
    URI1,
    URI2,
#    URI3,
#    URI4,
#    URI5,
#    URI6,
#    URI7,
#    URI8,
#    URI9,
#    URI10
}


location = {} # Dictionary of URI and Location
rotation = {} # Dictionaty of URI and rotation
g_distance = {} # Nested Dicationary of Drone A to Drone B~X
g_bearing = {} #Nested Dictionary of Drone A to Drone B~X

            # [dist_tgt, bearing_tgt, dist_agt, bearing_agt, orient_agt]

inputLM =   [0,0,0,0,0]

for x in uris:
    g_distance[x]={}

for x in uris:
    g_bearing[x] = {}

def wait_for_position_estimator(scf):
    print('Waiting for estimator to find position...')

    log_config = LogConfig(name='Stabilizer', period_in_ms=100)
    log_config.add_variable('stabilizer.roll', 'float')
    log_config.add_variable('stabilizer.pitch', 'float')
    log_config.add_variable('stabilizer.yaw', '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:
        endTime = time.time() + 10

        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 wait_for_param_download(scf):
    while not scf.cf.param.is_updated:
        time.sleep(1.0)
    print('Parameters downloaded for', scf.cf.link_uri)


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 take_off(cf, position):
    take_off_time = 1.0
    sleep_time = 0.1
    steps = int(take_off_time / sleep_time)
    vz = position[2] / take_off_time

    print(vz)

    for i in range(steps):
        cf.commander.send_velocity_world_setpoint(0, 0, vz, 0)
        time.sleep(sleep_time)


def land(cf, position):
    landing_time = 1.0
    sleep_time = 0.1
    steps = int(landing_time / sleep_time)
    vz = -position[2] / landing_time

    print(vz)

    for _ in range(steps):
        cf.commander.send_velocity_world_setpoint(0, 0, vz, 0)
        time.sleep(sleep_time)

    cf.commander.send_stop_setpoint()
    # 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)



def position_callback(uri, timestamp, data, logconf):
    x = data['kalman.stateX']
    y = data['kalman.stateY']
    z = data['kalman.stateZ']

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

    point = (x,y,z) # (-39.528446197509766, -8.916569709777832, -0.5112752318382263)\


    location[uri] = point # {'radio://0/33/1M': (-3.2653462886810303, -1.9112612009048462, 0.020919203758239746), 'radio://0/40/1M': (-4.532137870788574, -0.9649770855903625, 0.047263506799936295)
    #rotation[uri] =

    for k,v in location.items():

        # Calculate distance for all drones
        _d = calculate_distance(point,v)
        g_distance[uri][k]= _d
        g_distance[k][uri]= _d

    FormLMArray(uri,g_distance)

    #print(location[uri])
    #CalculateBearing(location[URI2],location[URI3])
    #return point

def FormLMArray(uri,d_dict):

    # Form list of keys and values
    key_list = list(d_dict[uri].keys())
    val_list = list(d_dict[uri].values())

    # Find Distance to target
    dist_tgt = d_dict[uri]['radio://0/40/1M']

    # Find Bearing to Target
    bearing_tgt = CalculateBearing(uri,location[uri], location[URI1])

    # find shortest agents and its URI
    dist_agt = min(filter(lambda x: x>0.00, val_list)) # Distance to nearest agent
    dist_agt_uri = key_list[val_list.index(dist_agt)]  # nearest agent URI

    # Find bearing for shortest agent
####################### start of changes by Darrence ##################################
    bearing_agt = CalculateBearing(uri,location[uri],location[dist_agt_uri])
####################### end of changes ##################################

    inputLM[0] = distance_transformation(dist_tgt)
    inputLM[1] = bearing_tgt
    inputLM[2] = distance_transformation(dist_agt)
    inputLM[3] = bearing_agt
    inputLM[4] = rotation[dist_agt_uri][2] #get YAW value
####################### start of changes by Darrence ##################################
    observations.extend(inputLM)
####################### end of changes ##################################
    return(inputLM)

####################### start of changes by Darrence ##################################

def distance_transformation(act_distance):
    dm=0.35 #0.35~0.7
    dt=0.07
    Dt=0.25 #0.25~0.5
    return(min(dt*(act_distance/Dt),dm))

####################### end of changes ##################################

def stablizer_callback(uri, timestamp, data, logconf):
    roll = data['stabilizer.roll']
    pitch = data['stabilizer.pitch']
    yaw = data['stabilizer.yaw']

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

    stabilizer = (roll, pitch, yaw)

    rotation[uri] = stabilizer
    return(stabilizer)

    #CalculateStabDiff(rotation[URI2],rotation[URI3])

    # Get Position Data
def GetPosition(scf):

    log_conf = LogConfig(name='Position', period_in_ms=100)
    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(lambda timestamp, data, logconf: position_callback(scf.cf.link_uri, timestamp, data, logconf))
    log_conf.start()

    # Get Roll, Pitch, Yaw
def GetStabilizer(scf):
    log_conf = LogConfig(name='Stablizer',period_in_ms=100)
    log_conf.add_variable('stabilizer.roll', 'float')
    log_conf.add_variable('stabilizer.pitch', 'float')
    log_conf.add_variable('stabilizer.yaw', 'float')
    scf.cf.log.add_config(log_conf)

    log_conf.data_received_cb.add_callback(lambda timestamp, data, logconf: stablizer_callback(scf.cf.link_uri, timestamp, data, logconf))
    log_conf.start()


    # Calculate Distance Between Two Agents
def calculate_distance(p1, p2):
    diff = [0,0,0]
    diff[0] = p1[0]-p2[0]
    diff[1] = p1[1]-p2[1]
    diff[2] = p1[2]-p2[2]

    result = (math.sqrt( (diff[0]*diff[0]) + (diff[1]*diff[1]) + (diff[2]*diff[2])))

    return(result)



def OrientationDifference(p1,p2):
    print("aa")

    # Calculate Bearing with Drone Data
    # Variables needed D2-D1, Forward Vector of D1

def CalculateBearing(uri, p1,p2):

    # Finding Forward Vector of D1
    #p1_l = location[p1]
    #p2_l = location[p2]

    nVector1 = vmath.Vector2(0,0)
    YawInRad = numpy.radians(rotation[uri][2])
    nVector1[0] = math.cos(YawInRad)
    nVector1[1] = math.sin(YawInRad)

    # Finding Directional Vector from D1 to D2
    Vector2 = vmath.Vector2(0,0)
    Vector2[0] = p2[0] - p1[0]
    Vector2[1] = p2[1] - p1[1]

    #angle = nVector1.angle(Vector2,unit='deg')
    signed_angle = np.arctan2(Vector2[0],Vector2[1]) - np.arctan2(nVector1[0],nnVector1[1])
####################### start of changes by darrence ##################################
    return(np.degrees(signed_angle)/180)
####################### end of changes ##################################


####################### start of changes by darrence ##################################
def run_sequence(scf):
    #time.sleep(10)
    actions = infer.inferenceMod(observations)
    velocity_v = float(actions[0])*0.3
    rotation = float(actions[1])*75
    scf.cf.commander.send_hover_setpoint(vx=velocity_v,vy=0,yawrate=rotation, zdistance=0.4)
    observations.pop()
####################### end of changes ##################################

if __name__ == '__main__':
    #logging.basicConfig(level=logging.DEBUG)
    factory = CachedCfFactory(rw_cache='./cache')
    cflib.crtp.init_drivers(enable_debug_driver=False)


    with Swarm(uris,factory=factory) as swarm:

        # If the copters are started in their correct positions this is
        # probably not needed. The Kalman filter will have time to converge
        # any way since it takes a while to start them all up and connect. We
        # keep the code here to illustrate how to do it.
        swarm.parallel(reset_estimator)

        # The current values of all parameters are downloaded as a part of the
        # connections sequence. Since we have 10 copters this is clogging up
        # communication and we have to wait for it to finish before we start
        # flying.

        print('Waiting for parameters to be downloaded...')

####################### start of changes by darrence ##################################
        swarm.parallel_safe(wait_for_param_download)
        swarm.parallel_safe(GetStabilizer) #brough this line above of swarm.parallel_safe(GetPosition) because rotation{} needs to be populated first
        #swarm.parallel_safe(GetPosition)
        #swarm.parallel_safe(calculate_distance)
        #swarm.parallel(CalculateBearing)
        swarm.parallel(run_sequence)
####################### end of changes ##################################


So they initially take off, four of the drones will follow one of the drones using the model and rotate around it. I hope this makes sense for you.While testing this code out, not matter which commander function i put there, it seems to not work on the crazyflie (won't fly at all).

For the SwarmSequenceCircle.py, I believe I'm currently using a unmodified version of the code so I thought it was strange that it didn't work.
kimberly
Bitcraze
Posts: 1050
Joined: Fri Jul 06, 2018 11:13 am

Re: Commander.py with Swarm

Post by kimberly »

What I can see in your example, is that you only send the setpoint command just once. If you look in the original swarmSequence example, is that the setpoints are sent within a while loop continuesly. These setpoints have to be send all the time the crazyflie will land if it hasn't received anything for a short while. It is weird that the crazyflie does not even slightly take off, but that might be that the connection is closed before it has a chance to react to the setpoint given by your script. Also see this documentation: https://www.bitcraze.io/documentation/r ... setpoints/

So what I would advice you to do in general, is to make minimal example scripts, where you test certain elements ones at the time. Then you can more clearly separate what is working and what is not. Also such scripts are a bit easier for us to encode the code for you ;)
youngbin1130
Member
Posts: 46
Joined: Thu Dec 12, 2019 4:28 am

Re: Commander.py with Swarm

Post by youngbin1130 »

I had fixed the issue! It was just carelessness from my side when I was testing out certain codes.

However, I do have two more questions regarding the orientation of the Crazyflie while using the LPS nodes. From what I understand is that one should make sure that the Crazyflie faces the +x direction before running the program/codes as stated in multiple forum posts and guides. Is there a way to make sure that the Crazyflie's stabilizer value is (0,0,0)? Here are some things that might help you understand the problem.

The YAW value stabilizer graph from the Crazyflie client for some reason increases/decreases after turning the drone manually. This is apparent in whatever drone I use. I've uploaded two images of the graph I took from two different drones. Additionally, if I take a look at the "Flight Control" tab from the client, I can see that the drone's stabilizer is moving around when nothing is actually moving it.

Another question is that I've seen some people have issues with the Flow Deck v2 and LPS node together (namely this post at GitHub https://github.com/bitcraze/crazyflie-f ... issues/368). I should update the Crazyflie with the latest firmware, is that correct?
Attachments
fgggg.PNG
asdsa.PNG
asd.PNG
kimberly
Bitcraze
Posts: 1050
Joined: Fri Jul 06, 2018 11:13 am

Re: Commander.py with Swarm

Post by kimberly »

Ah good you fixed it!

Also about the next questions, these are not really related to this forum thread, so before I help you with it could you please start a new forum thread with exactly these questions and the printscreens ?

Sorry to be strict on this, but this is mostly to keep the forum readable and solutions & support searchable for other users, so we aim to have one forum thread per issue ;) You can start the new one in the Loco positioning system group!
youngbin1130
Member
Posts: 46
Joined: Thu Dec 12, 2019 4:28 am

Re: Commander.py with Swarm

Post by youngbin1130 »

Will do!
Post Reply