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()