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 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 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 test_that_connection_time_scales_with_more_devices_without_cache(self): # Fixture self.test_rig_support.restart_devices(self.test_rig_support.all_uris) EXPECTED_CONNECTION_TIME = 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): 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 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 setUp(self): self.uris = [self.URI1, self.URI2, self.URI3] self.factory = MockFactory() self.sut = Swarm(self.uris, factory=self.factory)
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()
time.sleep(10) scf.cf.param.set_value('commander.enableAP', '0') scf.cf.param.set_value('avoidance.enTcas2', '0') scf.cf.param.set_value('p2p.p2pEnable', '0') print("Land") commander.land(0.0, 2) time.sleep(4) commander.stop() uris = { 'I': 'radio://0/80/2M/E7E7E7E7E3', # I #'II' : 'radio://0/80/2M/E7E7E7E7E5', # II #'III' : 'radio://0/80/2M/E7E7E7E7E8', # III 'IV': 'radio://0/80/2M/E7E7E7E7E9', # IV #'V' : 'radio://0/80/2M/E7E7E7E7EA', # V #'VI' : 'radio://0/80/2M/E7E7E7E7EB', # VI #'USB' : 'usb://0', # USB } if __name__ == '__main__': cflib.crtp.init_drivers(enable_debug_driver=False) factory = CachedCfFactory(rw_cache='./cache') with Swarm(set(uris.values()), factory=factory) as swarm: swarm.parallel_safe(activate_high_level_commander) swarm.parallel_safe(reset_estimator) swarm.parallel_safe(startLastPrinting) swarm.parallel_safe(run_shared_sequence)
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()
return False else: return range < MIN_DISTANCE if __name__ == '__main__': # Initialize the low-level drivers (don't list the debug drivers) cflib.crtp.init_drivers(enable_debug_driver=True) cf = Crazyflie(rw_cache='./cache') factory = CachedCfFactory(rw_cache='./cache') URI = {trajectory_assignment[key] for key in trajectory_assignment.keys()} #cf = scf.cf #for i in URI: #with SyncCrazyflie(URI, cf=cf) as scf: with Swarm(URI, factory=factory) as scf: with MotionCommander(scf) as motion_commander: with Multiranger(scf) as multi_ranger: keep_flying = True while keep_flying: VELOCITY = 0.5 velocity_x = 0.0 velocity_y = 0.0 if is_close(multi_ranger.front): velocity_x -= VELOCITY if is_close(multi_ranger.back): velocity_x += VELOCITY if is_close(multi_ranger.left):
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) swarm.parallel(run_sequence, args_dict=params)
poshold(cf, 2, z) for _ in range(2): # The time for one revolution circle_time = 8 steps = circle_time * fs for _ in range(steps): cf.commander.send_hover_setpoint(d * comp * math.pi / circle_time, 0, 360.0 / circle_time, z) time.sleep(fsi) poshold(cf, 2, z) for r in range(ramp): cf.commander.send_hover_setpoint(0, 0, 0, base + (ramp - r) * (z - base) / ramp) time.sleep(fsi) poshold(cf, 1, base) cf.commander.send_stop_setpoint() if __name__ == '__main__': cflib.crtp.init_drivers(enable_debug_driver=False) with Swarm(uris) as swarm: swarm.parallel(reset_estimator) swarm.parallel(run_sequence, args_dict=params)