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)