def test_that_connection_time_scales_with_more_devices_with_cache(self):
# Fixture
self.test_rig_support.restart_devices(self.test_rig_support.all_uris)
# Fill caches first by connecting to all devices
factory = CachedCfFactory(rw_cache='./cache')
with Swarm(self.test_rig_support.all_uris, factory=factory):
pass
EXPECTED_CONNECTION_TIME = 1.5
for nr_of_devices in range(1, len(self.test_rig_support.all_uris)):
# Test
uris = self.test_rig_support.all_uris[:nr_of_devices]
start_time = time.time()
with Swarm(uris, factory=factory):
connected_time = time.time()
actual = connected_time - start_time
max_expected = EXPECTED_CONNECTION_TIME * nr_of_devices
print('Connection time for', nr_of_devices, ':', actual,
', per device:', actual / nr_of_devices)
# Assert
self.assertLess(actual, max_expected)
def test_that_requested_logging_is_received_properly_from_all_cfs(self):
# Fixture
uris = self.test_rig_support.all_uris
self.test_rig_support.restart_devices(uris)
factory = CachedCfFactory(rw_cache='./cache')
# Test and Assert
with Swarm(uris, factory=factory) as swarm:
swarm.parallel_safe(self.assert_add_logging_and_get_non_zero_value)
def test_memory_mapping_with_all_cfs(self):
# Fixture
uris = self.test_rig_support.all_uris
self.test_rig_support.restart_devices(uris)
factory = CachedCfFactory(rw_cache='./cache')
# Test and Assert
with Swarm(uris, factory=factory) as swarm:
swarm.parallel_safe(self.assert_memory_mapping)
def run(args):
cflib.crtp.init_drivers(enable_debug_driver=False)
factory = CachedCfFactory(rw_cache='./cache')
uris = {trajectory_assignment[key] for key in trajectory_assignment.keys()}
print(uris)
with open(args.json, 'r') as f:
data = json.load(f)
swarm_args = {
trajectory_assignment[drone_pos]: [data[str(drone_pos)]]
for drone_pos in trajectory_assignment.keys()
}
qtmWrapper = QtmWrapper(body_names)
qtm_args = {
key: [qtmWrapper, rigid_bodies[key]]
for key in rigid_bodies.keys()
}
with Swarm(uris, factory=factory) as swarm:
def signal_handler(sig, frame):
print('You pressed Ctrl+C!')
swarm.parallel(land_sequence)
sys.exit(0)
signal.signal(signal.SIGINT, signal_handler)
print('Press Ctrl+C to land.')
print('Starting mocap data relay...')
swarm.parallel_safe(send6DOF, qtm_args)
print('Preflight sequence...')
swarm.parallel_safe(preflight_sequence)
print('Takeoff sequence...')
swarm.parallel_safe(takeoff_sequence)
print('Upload sequence...')
trajectory_count = 0
# repeat = int(data['repeat'])
for trajectory_count in range(1):
if trajectory_count == 0:
print('Uploading Trajectory')
swarm.parallel(upload_trajectory, args_dict=swarm_args)
print('Go...')
swarm.parallel_safe(go_sequence, args_dict=swarm_args)
print('Land sequence...')
swarm.parallel(land_sequence)
print('Closing QTM connection...')
qtmWrapper.close()
def swarm_id_update():
cflib.crtp.init_drivers(enable_debug_driver=False)
factory = CachedCfFactory(rw_cache='./cache')
uris = {trajectory_assignment[key] for key in trajectory_assignment.keys()}
id_args = {key: [id_assignment[key]] for key in id_assignment.keys()}
with Swarm(uris, factory=factory) as swarm:
print('Starting ID update...')
swarm.sequential(
id_update, id_args
) # parallel update has some issue with more than 3 drones, using sequential update here.
def run(args):
# logging.basicConfig(level=logging.DEBUG)
cflib.crtp.init_drivers(enable_debug_driver=False)
factory = CachedCfFactory(rw_cache='./cache')
uris = {trajectory_assignment[key] for key in trajectory_assignment.keys()}
with open(args.json, 'r') as f:
data = json.load(f)
swarm_args = {
trajectory_assignment[drone_pos]: [data[str(drone_pos)]]
for drone_pos in trajectory_assignment.keys()
}
with Swarm(uris, factory=factory) as swarm:
def signal_handler(sig, frame):
print('You pressed Ctrl+C!')
swarm.parallel(land_sequence)
sys.exit(0)
signal.signal(signal.SIGINT, signal_handler)
print('Press Ctrl+C to land.')
print('Preflight sequence...')
swarm.parallel_safe(preflight_sequence)
#print('Takeoff sequence...')
swarm.parallel_safe(takeoff_sequence)
print('Upload sequence...')
trajectory_count = 0
repeat = int(data['repeat'])
for trajectory_count in range(repeat):
if trajectory_count == 0:
print('Uploading Trajectory')
swarm.parallel(upload_trajectory, args_dict=swarm_args)
print('Go...')
swarm.parallel_safe(go_sequence, args_dict=swarm_args)
print('Land sequence...')
swarm.parallel(land_sequence)
def swarm_id_update(args):
cflib.crtp.init_drivers(enable_debug_driver=False)
data = assignments(int(args.count))
trajectory_assignment = data['trajectory_assignment']
body_names = data['body_names']
rigid_bodies = data['rigid_bodies']
id_assignment = data['id_assignment']
factory = CachedCfFactory(rw_cache='./cache')
uris = {trajectory_assignment[key] for key in trajectory_assignment.keys()}
id_args = {key: [id_assignment[key]] for key in id_assignment.keys()}
with Swarm(uris, factory=factory) as swarm:
print('Starting ID update...')
#swarm.sequential(id_update, id_args) # parallel update has some issue with more than 3 drones, using sequential update here.
swarm.sequential(
id_update, id_args
) # parallel update has some issue with more than 3 drones, using sequential update here.
def hover(args):
cflib.crtp.init_drivers(enable_debug_driver=False)
data = assignments(int(args.count))
trajectory_assignment = data['trajectory_assignment']
body_names = data['body_names']
rigid_bodies = data['rigid_bodies']
factory = CachedCfFactory(rw_cache='./cache')
uris = {trajectory_assignment[key] for key in trajectory_assignment.keys()}
qtmWrapper = QtmWrapper(body_names)
qtm_args = {
key: [qtmWrapper, rigid_bodies[key]]
for key in rigid_bodies.keys()
}
with Swarm(uris, factory=factory) as swarm:
def signal_handler(sig, frame):
print('You pressed Ctrl+C!')
swarm.parallel(land_sequence)
time.sleep(1)
print('Closing QTM link...')
qtmWrapper.close()
sys.exit(0)
signal.signal(signal.SIGINT, signal_handler)
print('Press Ctrl+C to land.')
print('Starting mocap data relay...')
swarm.parallel_safe(send6DOF, qtm_args)
print('Preflight sequence...')
swarm.parallel_safe(preflight_sequence)
print('Takeoff sequence...')
swarm.parallel_safe(takeoff_sequence)
print('Land sequence...')
swarm.parallel(land_sequence)
print('Closing QTM connection...')
qtmWrapper.close()
def __init__(self, distance):
"""
Inicialização dos drivers, parâmetros do controle PID e decolagem do drone.
"""
self.distance = distance
self.pid_foward = PID(distance, 0.01, 0.0001, 0.01, 500, -500, 0.7,
-0.7)
self.pid_yaw = PID(0, 0.33, 0.0, 0.33, 500, -500, 100, -100)
self.pid_angle = PID(0.0, 0.01, 0.0, 0.01, 500, -500, 0.3, -0.3)
self.pid_height = PID(0.0, 0.002, 0.0002, 0.002, 500, -500, 0.3, -0.2)
cflib.crtp.init_drivers(enable_debug_driver=False)
self.factory = CachedCfFactory(rw_cache='./cache')
self.URI4 = 'radio://0/40/2M/E7E7E7E7E4'
self.cf = Crazyflie(rw_cache='./cache')
self.sync = SyncCrazyflie(self.URI4, cf=self.cf)
self.sync.open_link()
self.mc = MotionCommander(self.sync)
self.cont = 0
self.notFoundCount = 0
self.calculator = DistanceCalculator()
self.cap = cv2.VideoCapture(1)
self.mc.take_off()
time.sleep(1)
def run(args):
# logging.basicConfig(level=logging.DEBUG)
cflib.crtp.init_drivers(enable_debug_driver=False)
factory = CachedCfFactory(rw_cache='./cache')
uris = URI
with open(args.json, 'r') as f:
data = json.load(f)
swarm_args = {trajectory_assignment[drone_pos]: [data[str(drone_pos)]]
for drone_pos in trajectory_assignment.keys()}
with Swarm(uris, factory=factory) as swarm:
def signal_handler(sig, frame):
print('You pressed Ctrl+C!')
swarm.parallel(land_sequence)
sys.exit(0)
signal.signal(signal.SIGINT, signal_handler)
print('Press Ctrl+C to land.')
print('Preflight sequence...')
swarm.parallel_safe(preflight_sequence)
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()
if __name__ == '__main__':
# logging.basicConfig(level=logging.DEBUG)
cflib.crtp.init_drivers(enable_debug_driver=False)
factory = CachedCfFactory(rw_cache='./cache')
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)
time.sleep(2.0)
# 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(start_position_printing)
time.sleep(3.0)
def main():
# Initalize node
rospy.init_node('swarmControl')
print('Starting swarm ')
global rate
rate = rospy.Rate(120)
#Initialize crazyflie radios
URI1 = 'radio://0/80/2M/E7E7E7E701'
URI2 = 'radio://0/80/2M/E7E7E7E7E7'
URI3 = 'radio://0/80/2M/E7E7E7E703'
uris = {URI1, URI2, URI3}
#-----------------------------Set up Subscribers---------------------------
rospy.Subscriber("/mocap_node/Crazyflie_1/pose", PoseStamped,
crazyflie1_pose_cb)
rospy.Subscriber("/mocap_node/Crazyflie_2/pose", PoseStamped,
crazyflie2_pose_cb)
rospy.Subscriber("/mocap_node/Crazyflie_3/pose", PoseStamped,
crazyflie3_pose_cb)
#----------------------------Set up Publishers------------------------------------
local_pose_pub = rospy.Publisher('Position', Pose, queue_size=10)
Pose_pub = Pose()
Pose_pub.position.x = 0 #thrust
Pose_pub.position.y = 0 #error
Pose_pub.position.z = 0 #z position
#-------------------------Initialize errors----------------------------------------
yerror1 = 0
xerror1 = 0
zerror1 = 0
yerror2 = 0
xerror2 = 0
zerror2 = 0
yerror3 = 0
xerror3 = 0
zerror3 = 0
var = 1 #Sets desired position to just be above their starting positions
cflib.crtp.init_drivers()
factory = CachedCfFactory(rw_cache='./cache')
with Swarm(uris, factory=factory) as swarm:
print("Preparing to initialize...")
print('Waiting for parameters to be downloaded...')
swarm.parallel(wait_for_param_download)
swarm.parallel(initialize)
#swarm.parallel(take_off)
while (not rospy.is_shutdown()):
print('Running Sequence')
if var == 1:
if crazyflie1_pose.pose.position.x != 0.0 and crazyflie1_pose.pose.position.y != 0.0: #make sure we're actually getting something
xd1 = crazyflie1_pose.pose.position.x
yd1 = crazyflie1_pose.pose.position.y
zd1 = 0.45
if crazyflie2_pose.pose.position.x != 0.0 and crazyflie2_pose.pose.position.y != 0.0:
xd2 = crazyflie2_pose.pose.position.x
yd2 = crazyflie2_pose.pose.position.y
zd2 = 0.45
if crazyflie3_pose.pose.position.x != 0.0 and crazyflie3_pose.pose.position.y != 0.0:
xd3 = crazyflie3_pose.pose.position.x
yd3 = crazyflie3_pose.pose.position.y
zd3 = 0.45
var = 0
x1 = crazyflie1_pose.pose.position.x
y1 = crazyflie1_pose.pose.position.y
z1 = crazyflie1_pose.pose.position.z
x2 = crazyflie2_pose.pose.position.x
y2 = crazyflie2_pose.pose.position.y
z2 = crazyflie2_pose.pose.position.z
x3 = crazyflie3_pose.pose.position.x
y3 = crazyflie3_pose.pose.position.y
z3 = crazyflie3_pose.pose.position.z
roll1, pitch1, thrust1, xerror1, yerror1, zerror1 = controller(
xd1, yd1, zd1, x1, y1, z1, xerror1, yerror1, zerror1)
roll2, pitch2, thrust2, xerror2, yerror2, zerror2 = controller(
xd2, yd2, zd2, x2, y2, z2, xerror2, yerror2, zerror2)
roll3, pitch3, thrust3, xerror3, yerror3, zerror3 = controller(
xd3, yd3, zd3, x3, y3, z3, xerror3, yerror3, zerror3)
print("CF1 Desired:", xd1, yd1, zd1)
print("CF1 Actually:", x1, y1, z1)
print("CF2 Desired:", xd2, yd2, zd2)
print("CF2 Actually:", x2, y2, z2)
print("CF3 Desired:", xd3, yd3, zd3)
print("CF3 Actually:", x3, y3, z3)
print("Roll vals", roll1, roll2, roll3)
print("Pitch vals", pitch1, pitch2, pitch3)
print("Thrust Vals", thrust1, thrust2, thrust3)
values = {
URI1: [(roll1, pitch1, 0, math.floor(thrust1))],
URI2: [(roll2, pitch2, 0, math.floor(thrust2))],
URI3: [(roll3, pitch3, 0, math.floor(thrust3))]
}
swarm.parallel(run_sequence, values)
Pose_pub.position.x = yerror1 #thrust
Pose_pub.position.y = yerror3 #error
Pose_pub.position.z = pitch1 #z position
local_pose_pub.publish(Pose_pub)
rate.sleep()
def main():
#------------------------------ Initalize node----------------------------------------------
rospy.init_node('swarmControl')
print('Starting swarm ')
global rate
rate = rospy.Rate(120)
#-------------------------------Initialize crazyflies--------------------------------------
URI1 = 'radio://0/80/2M/E7E7E7E701'
URI2 ='radio://0/80/2M/E7E7E7E7E7'
URI3 ='radio://0/80/2M/E7E7E7E703'
uris = {URI1,URI2,URI3}
#-------------------------------- Set up subscribers --------------------------------------------
rospy.Subscriber("/mocap_node/Crazyflie_1/pose", PoseStamped, crazyflie1_pose_cb)
rospy.Subscriber("/mocap_node/Crazyflie_2/pose", PoseStamped, crazyflie2_pose_cb)
rospy.Subscriber("/mocap_node/Crazyflie_3/pose", PoseStamped, crazyflie3_pose_cb)
rospy.Subscriber("/CF_1/desiredPos",PoseStamped, desired_pose_cb1)
rospy.Subscriber("/CF_2/desiredPos",PoseStamped, desired_pose_cb2)
rospy.Subscriber("/CF_3/desiredPos",PoseStamped, desired_pose_cb3)
#-------------------------------- Set up publishers --------------------------------------------
local_pose_pub = rospy.Publisher('Position', Pose, queue_size=10)
Pose_pub = Pose()
Pose_pub.position.x = 0 #thrust
Pose_pub.position.y = 0 #error
Pose_pub.position.z = 0 #z position
#------------------------------ Initialize Errors ----------------------------------------
yerror1 = 0
xerror1 = 0
zerror1 = 0
yerror2 = 0
xerror2 = 0
zerror2 = 0
yerror3 = 0
xerror3 = 0
zerror3 = 0
var = 1 # set to 1 if you want the cfs to take off and hover
cflib.crtp.init_drivers()
factory = CachedCfFactory(rw_cache='./cache')
with Swarm(uris, factory=factory) as swarm:
print("Preparing to initialize...")
print('Waiting for parameters to be downloaded...')
swarm.parallel(wait_for_param_download)
swarm.parallel(initialize)
#swarm.parallel(take_off)
print("Pose Flag 1",pose_flag1)
while not pose_flag1 or not pose_flag2 or not pose_flag3:
print("Now waiting for desired positions")
rate.sleep()
while(not rospy.is_shutdown()):
#print('Running Sequence')
# s=1
# while s==1:
#
# print(desired_pose1.pose.position.x)
# print(desired_pose1.pose.position.y)
# print(desired_pose1.pose.position.z)
# print("-------------------------------")
xd1 = desired_pose1.pose.position.x
yd1 = desired_pose1.pose.position.y
zd1 = desired_pose1.pose.position.z
xd2 = desired_pose2.pose.position.x
yd2 = desired_pose2.pose.position.y
zd2 = desired_pose2.pose.position.z
xd3 = desired_pose3.pose.position.x
yd3 = desired_pose3.pose.position.y
zd3 = desired_pose3.pose.position.z
x1=crazyflie1_pose.pose.position.x
y1 =crazyflie1_pose.pose.position.y
z1 = crazyflie1_pose.pose.position.z
x2=crazyflie2_pose.pose.position.x
y2 =crazyflie2_pose.pose.position.y
z2 = crazyflie2_pose.pose.position.z
x3=crazyflie3_pose.pose.position.x
y3 =crazyflie3_pose.pose.position.y
z3 = crazyflie3_pose.pose.position.z
roll1,pitch1,thrust1,xerror1,yerror1,zerror1 = controller(xd1,yd1,zd1,x1,y1,z1,xerror1,yerror1,zerror1)
roll2,pitch2,thrust2,xerror2,yerror2,zerror2 = controller(xd2,yd2,zd2,x2,y2,z2,xerror2,yerror2,zerror2)
roll3,pitch3,thrust3,xerror3,yerror3,zerror3 = controller(xd3,yd3,zd3,x3,y3,z3,xerror3,yerror3,zerror3)
# print("CF1 Desired:", xd1,yd1,zd1)
# print("CF1 Actually:", x1,y1,z1)
# print("CF2 Desired:", xd2,yd2,zd2)
# print("CF2 Actually:", x2,y2,z2)
# print("CF3 Desired:", xd3,yd3,zd3)
# print("CF3 Actually:", x3,y3,z3)
# print("Roll vals",roll1,roll2,roll3)
# print("Pitch vals",pitch1,pitch2,pitch3)
# print("Thrust Vals",thrust1,thrust2,thrust3)
values = {
URI1:[(roll1,pitch1,0,math.floor(thrust1))],
URI2:[(roll2,pitch2,0,math.floor(thrust2))],
URI3:[(roll3,pitch3,0,math.floor(thrust3))]
}
swarm.parallel(run_sequence,values)
Pose_pub.position.x = yerror1 #thrust
Pose_pub.position.y = yerror3 #error
Pose_pub.position.z = pitch1 #z position
local_pose_pub.publish(Pose_pub)
rate.sleep()
def start(self):
cflib.crtp.init_drivers(enable_debug_driver=False)
factory = CachedCfFactory(rw_cache='./cache')
self.selectDrone()
selectedTest = 'start'
while (selectedTest != '99'):
messageTest = ("\nQual teste você deseja fazer?\n"
"(1) Combinacao1\n"
"(2) Combinacao2\n"
"(3) Combinacao3\n"
"(4) Esquadrilha\n"
"(5) Círculos\n"
"(9) bateria\n"
"(0) subida\n"
"(123) selecionar drone\n"
"(99) finalizar teste\n")
selectedTest = input(messageTest)
#Verifica se não houve perda de conexão
for cf in self.cfs:
if (cf.link == None):
print("conexao com o drone perdida")
selectedTest = SELECT
break
if (selectedTest == COMB1):
self.setThreadEmergencial()
combinacao.combinacao1(self.mcs[0])
elif (selectedTest == COMB2):
self.setThreadEmergencial()
combinacao.combinacao2(self.mcs[0], self.mcs[1])
elif (selectedTest == COMB3):
self.setThreadEmergencial()
combinacao.combinacao3(self.mcs[0], self.mcs[1], self.mcs[2])
elif (selectedTest == ESQUADRILHA):
self.setThreadEmergencial()
combinacao.esquadrilha(self.mcs[0], self.mcs[1])
elif (selectedTest == CIRCULOS):
self.setThreadEmergencial()
combinacao.circulo(self.mcs[0], self.mcs[1])
elif (selectedTest == TESTBATTERY):
if (len(self.selected) > 1):
d = int(input("selecione o drone: "))
n = self.selected.index(d)
self.testeBateria(self.cfs[n])
else:
self.testeBateria(self.cfs[0])
time.sleep(0.5)
elif (selectedTest == TESTSUBIDA):
if (len(self.selected) == 1):
self.testeBateria(self.cfs[0])
self.testeSubida(self.mcs[0])
else:
print("bateria drone ", self.selected[0])
self.testeBateria(self.cfs[0])
print("bateria drone ", self.selected[1])
self.testeBateria(self.cfs[1])
self.testeSubida2(self.mcs[0], self.mcs[1])
elif (selectedTest == SELECT):
for sync in self.scfs:
sync.close_link()
self.selectDrone()
#zera o pouso emergencial
for mc in self.mcs:
mc.setStopMotion(False)