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