def setUp(self):
self.uris = [self.URI1, self.URI2, self.URI3]
self.factory = MockFactory()
self.sut = Swarm(self.uris, factory=self.factory)
class TestSwarm(unittest.TestCase):
URI1 = 'uri1'
URI2 = 'uri2'
URI3 = 'uri3'
def setUp(self):
self.uris = [self.URI1, self.URI2, self.URI3]
self.factory = MockFactory()
self.sut = Swarm(self.uris, factory=self.factory)
def test_that_instances_are_created(self):
# Fixture
# Test
# Assert
actual = len(self.factory.mocks)
expected = len(self.uris)
self.assertEqual(expected, actual)
def test_that_all_links_are_opened(self):
# Fixture
# Test
self.sut.open_links()
# Assert
for uri, mock in self.factory.mocks.items():
mock.open_link.assert_called_once_with()
def test_open_of_already_opened_swarm_raises_exception(self):
# Fixture
self.sut.open_links()
# Test
# Assert
with self.assertRaises(Exception):
self.sut.open_links()
def test_failed_open_of_one_link_closes_all_and_raises_exception(self):
# Fixture
self.factory.mocks[self.URI2].open_link.side_effect = Exception()
# Test
# Assert
with self.assertRaises(Exception):
self.sut.open_links()
for uri, mock in self.factory.mocks.items():
mock.close_link.assert_called_once_with()
def test_that_all_links_are_closed(self):
# Fixture
self.sut.open_links()
# Test
self.sut.close_links()
# Assert
for uri, mock in self.factory.mocks.items():
mock.close_link.assert_called_once_with()
def test_open_with_context_management(self):
# Fixture
# Test
with Swarm(self.uris, factory=self.factory):
pass
# Assert
for uri, mock in self.factory.mocks.items():
mock.open_link.assert_called_once_with()
mock.close_link.assert_called_once_with()
def test_sequential_execution_without_arguments(self):
# Fixture
func = MagicMock()
# Test
self.sut.sequential(func)
# Assert
for uri, mock in self.factory.mocks.items():
func.assert_any_call(mock)
def test_sequential_execution(self):
# Fixture
func = MagicMock()
args_dict = {
self.URI1: ['cf1-arg1'],
self.URI2: ['cf2-arg1'],
self.URI3: ['cf3-arg1'],
}
cf1 = self.factory.mocks[self.URI1]
cf2 = self.factory.mocks[self.URI2]
cf3 = self.factory.mocks[self.URI3]
# Test
self.sut.sequential(func, args_dict=args_dict)
# Assert
func.assert_any_call(cf1, 'cf1-arg1')
func.assert_any_call(cf2, 'cf2-arg1')
func.assert_any_call(cf3, 'cf3-arg1')
def test_parallel_execution_without_arguments(self):
# Fixture
func = MagicMock()
# Test
self.sut.parallel(func)
# Assert
for uri, mock in self.factory.mocks.items():
func.assert_any_call(mock)
def test_parallel_execution(self):
# Fixture
func = MagicMock()
args_dict = {
self.URI1: ['cf1-arg1'],
self.URI2: ['cf2-arg1'],
self.URI3: ['cf3-arg1'],
}
cf1 = self.factory.mocks[self.URI1]
cf2 = self.factory.mocks[self.URI2]
cf3 = self.factory.mocks[self.URI3]
# Test
self.sut.parallel(func, args_dict=args_dict)
# Assert
func.assert_any_call(cf1, 'cf1-arg1')
func.assert_any_call(cf2, 'cf2-arg1')
func.assert_any_call(cf3, 'cf3-arg1')
def test_parallel_execution_with_exception(self):
# Fixture
func_fail = MagicMock()
func_fail.side_effect = Exception()
args_dict = {
self.URI1: ['cf1-arg1'],
self.URI2: ['cf2-arg1'],
self.URI3: ['cf3-arg1'],
}
cf1 = self.factory.mocks[self.URI1]
cf2 = self.factory.mocks[self.URI2]
cf3 = self.factory.mocks[self.URI3]
# Test
self.sut.parallel(func_fail, args_dict=args_dict)
# Assert
func_fail.assert_any_call(cf1, 'cf1-arg1')
func_fail.assert_any_call(cf2, 'cf2-arg1')
func_fail.assert_any_call(cf3, 'cf3-arg1')
def test_parallel_safe_execution_with_exception(self):
# Fixture
func_fail = MagicMock()
func_fail.side_effect = Exception()
args_dict = {
self.URI1: ['cf1-arg1'],
self.URI2: ['cf2-arg1'],
self.URI3: ['cf3-arg1'],
}
# Test
# Assert
with self.assertRaises(Exception):
self.sut.parallel_safe(func_fail, args_dict=args_dict)
log_conf.data_received_cb.add_callback(position_callback)
log_conf.start()
def run_sequence(scf, sequence):
cf = scf.cf
cf.param.set_value('flightmode.posSet', '1')
for position in sequence:
print('Setting position {}'.format(position))
for i in range(50):
cf.commander.send_setpoint(position[1], position[0],
position[3],
int(position[2] * 1000))
time.sleep(0.1)
cf.commander.send_setpoint(0, 0, 0, 0)
# 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)
if __name__ == '__main__':
cflib.crtp.init_drivers(enable_debug_driver=False)
with Swarm(uris) as swarm:
swarm.parallel(reset_estimator)
# swarm.parallel(start_position_printing)
swarm.parallel(run_sequence, args_dict=seq_args)
end_time = time.time() + position[3]
while time.time() < end_time:
cf.commander.send_setpoint(position[1], position[0], 0,
int(position[2] * 1000))
time.sleep(0.1)
land(cf, sequence[-1])
except Exception as e:
print(e)
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)
# 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)
def run(args):
data = assignments(int(args.count))
trajectory_assignment = data['trajectory_assignment']
body_names = data['body_names']
rigid_bodies = data['rigid_bodies']
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)
time.sleep(1)
print('Closing QTM link')
qtmWrapper.close()
sys.exit(0)
signal.signal(signal.SIGINT, signal_handler)
print('Press Ctrl+C to land.')
try:
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)
except Exception as e:
print(e)
print('Aborting go sequence, landing')
swarm.parallel(land_sequence)
print('Closing QTM connection...')
qtmWrapper.close()
