class DroneApp(App): def build(self): return MainLayout() def on_start(self): Clock.schedule_once(self.init, 0) def on_pause(self): return True def on_resume(self): if not self.drone.state: self.discover_drone() @run_on_ui_thread def init(self, *args, **kwargs): self.drone = Drone() self.discover_drone() def on_activity_result(self, request_code, result_code, intent): fmt = "on_activity_result request_code={}, result_code={}" Logger.debug(fmt.format(request_code, result_code)) @run_on_ui_thread def discover_drone(self): self.drone.discover()
def __init__(self, host="", port=9000): threading.Thread.__init__(self) self.host = host self.port = port self.drone = Drone() self.socket = None self.start()
class DroneApp(App): def build(self): return MainLayout() def on_start(self): Clock.schedule_once(self.init, 0) def on_pause(self): return True def on_resume(self): if not self.drone.state: self.disvover_drone() @run_on_ui_thread def init(self, *args, **kwargs): self.drone = Drone() self.discover_drone() def on_activity_result(self, request_code, result_code, intent): fmt = "on_activity_result request_code={}, result_code={}" Logger.debug(fmt.format(request_code, result_code)) @run_on_ui_thread def discover_drone(self): self.drone.discover()
def main(): config = get_config("settings.cfg") host = config.get('settings', 'host') drone_port = int(config.get('settings', 'port_seed')) server_port = int(config.get('settings', 'server_port')) s = create_socket() while True: if bind_socket(s, host, drone_port): break else: drone_port += 1 config.set('settings', 'port_seed', str(drone_port + 1)) with open("settings.cfg", 'w') as configfile: config.write(configfile) drone = Drone() drone.port = drone_port begin_streaming(s, host, server_port, drone) s.close()
def connect(self): print('[+] Connection') self.s.settimeout(3) self.s.connect((self.host, self.port)) next_cmd = 'waiting_welcome' for line in self.recv_line(): print(' |-- %s' % line) if next_cmd == 'waiting_welcome': if line == 'BIENVENUE': self.send_msg(self.team) next_cmd = 'waiting_nbconnect' continue raise Exception('It seems we are not welcome... :(') if next_cmd == 'waiting_nbconnect': if line != 'ko': self.nbconnect = int(line) next_cmd = 'waiting_mapinfo' continue raise Exception('Invalid team') if next_cmd == 'waiting_mapinfo': (self.map_x, self.map_y) = [int(i) for i in line.split()] self.drone = Drone(self, self.map_x, self.map_y, self.team) break assert (self.drone) self.s.settimeout(None) print(' `-- Connection OK\n')
def __init__(self, stopped, min_speed=DEF_MIN_SPEED, max_speed=DEF_MAX_SPEED, verbosity=Drone.QUIET, fpc=DEF_FPC, initial_d=DEF_INITIAL_D): """ [stopped] -> cf Drone. [min_speed] et [max_speed] -> Les vitesses min et max que l'on peut avoir en donnant des ordres au drone (selon le drone). [verbosity] -> cf Drone. [fpc] -> Le nombre d'images que l'on attend avant d'envoyer une commande (règle la précision et la réactivité). [initial_d] -> La distance initiale entre la cible et le drone lors de sa sélection (pour se déplacer en avant et en arrière). [mode] -> Selon si l'on choisit nous-même la zone d'image à suivre ou si le drone le fait automatiquement. """ Drone.__init__(self, stopped, max_speed, verbosity) Thread.__init__(self) self.min_speed = min_speed self.max_speed = max_speed self.tracking = False self.queue = Queue(self, fpc) self.frontCam() self.initial_d = initial_d self._autoMove_last_t = datetime.now() # Contrôleur PID self.pid_x = PID(kp=AutoDrone.KP_X, ki=AutoDrone.KI_X, kd=AutoDrone.KD_X) self.pid_y = PID(kp=AutoDrone.KP_Y, ki=AutoDrone.KI_Y, kd=AutoDrone.KD_Y) self.pid_z = PID(kp=AutoDrone.KP_Z, ki=AutoDrone.KI_Z, kd=AutoDrone.KD_Z)
def generateDrone(self): while True: x,y=randint(0,self.GRIDS_X-1),randint(0,self.GRIDS_Y-1) if self.grid[y][x] == 0: self.drone = Drone(x,y) self.grid[y][x] = self.drone break
def main(): drone1 = Drone() user_input = 5 while user_input != 0: user_input = int( input("Enter 1 for accelerate, 2 for decelerate, " "3 for ascend, 4 for descend, 0 for exit: ")) if user_input == 1: drone1.accelerate() print("Speed ", drone1.speed, "Height", drone1.height) elif user_input == 2: drone1.decelerate() print("Speed ", drone1.speed, "Height", drone1.height) elif user_input == 3: drone1.ascend() print("Speed ", drone1.speed, "Height", drone1.height) elif user_input == 4: drone1.descend() print("Speed ", drone1.speed, "Height", drone1.height) else: print("Error please input 1, 2, 3, 4, or 0") user_input = int( input( "Enter 1 for accelerate, 2 for decelerate, 3 for ascend, 4 for descend, 0 for exit: " )) print("Speed ", drone1.speed, "Height", drone1.height)
def main(): drone1 = Drone() oper = 1 while oper != 0: oper = int( input('Enter 1 for accelerate, 2 for decelerate, ' '3 for ascend, 4 for descend, 0 for exit: ')) if oper == 1: drone1.accelerate() print('Speed:', drone1.speed, 'Height:', drone1.height) print() elif oper == 2: drone1.decelerate() print('Speed:', drone1.speed, 'Height:', drone1.height) print() elif oper == 3: drone1.ascend() print('Speed:', drone1.speed, 'Height:', drone1.height) print() elif oper == 4: drone1.descend() print('Speed:', drone1.speed, 'Height:', drone1.height) print()
def __init__(self): # Configure the four correctors for each coordinate self._pid_x = SpeedCorrector() self._pid_y = SpeedCorrector() self._pid_z = SpeedCorrector() self._pid_yaw = SpeedCorrector() # kalman filter is used for the drone state estimation self._estimate = StateEstimate() # Used to process images and backproject them # this._camera = new Camera(); TODO # Ensure that we don't enter the processing loop twice self._busy = False self._goal = None self.inFlight = None # The last known state self._state = {"x": 0, "y": 0, "z": 0, "yaw": 0} # The last time we have reached the goal (all control commands = 0) self._last_ok = 0 # Register the listener on navdata for our control loop def navdataListener(navdata): #print("seconds:", navdata.header.stamp.secs) self._processNavdata(navdata) self._control(navdata) # drone to manipulate self._drone = Drone(navdataListener)
def test_drone_update(self): drone = Drone() drone.pos = vec2d(100, 100) target = vec2d(200, 200) drone.target.append(target) drone.update() self.assertEqual(drone.pos, (103, 103)) self.assertEqual(drone.power, 255-0.1)
def buy_drone(self): if self.inventory[1][1] > 10000: self.inventory[1][1] -= 10000 tempdrone = Drone() tempdrone.pos = self.pos target = self.pos tempdrone.target.append(target) print("New drone is waiting for orders!") return tempdrone else: print("You need 10 000 ore to buy this!")
def spawn_drone(self): if self.counter % (60 - self.level*2) == 0: drone = Drone() max_x = self.width/2 + self.height/2 min_x = self.width/2 - self.height/2 drone.x = randint(min_x, max_x) drone.y = self.height drone.dy = randint(-5, -3) drone.dx = randint(-3, 3) self.add_widget(drone) self.drones.append(drone)
def test_mine(self): p = TestMain() drone = Drone() drone.pos = vec2d(100, 100) target = vec2d(100, 100) drone.target.append(target) ore = Ore() ore.pos = vec2d(100, 100) ore.mine(p, drone) self.assertEqual(drone.inventory[1][1], 1) self.assertEqual(ore.quantity, 799999)
def init_drone(self) -> Drone: """Creates a Drone object initialised with a deterministic set of target coordinates. Returns: Drone: An initial drone object with some programmed target coordinates. """ drone = Drone() drone.add_target_coordinate((0.35, 0.3)) drone.add_target_coordinate((-0.35, 0.4)) drone.add_target_coordinate((0.5, -0.4)) drone.add_target_coordinate((-0.35, 0)) return drone
def make_3d_lattice(side_num, sep_len): # create a 3d lattice (cube) of drones # with side_num drones per edge, sepearted by a distance of sep_len # Means you will have side_num^3 many drones. ds = [] for i in range(side_num): for j in range(side_num): for k in range(side_num): d = Drone() d.position = np.array([i * sep_len, j * sep_len, k * sep_len]) ds.append(d) return ds
def _create_drones(num_of_clients, drone_capacity): drones_needed = num_of_clients // drone_capacity if num_of_clients % drone_capacity == 0: drones = [ Drone(i + 1, drone_capacity) for i in range(drones_needed) ] else: drones_needed += 1 drones = [ Drone(i + 1, drone_capacity) for i in range(drones_needed) ] return drones, drones_needed
def __init__(self, w=WIDTH, h=HEIGHT): self.w = w self.h = h self.display = pygame.display.set_mode((self.w, self.h)) pygame.display.set_caption('BLOCKS_AI') self.drone = Drone(20, RED) self.man = Man(20, BLUE1) self.reset() self.clock = pygame.time.Clock() self.time = 0
def __init__(self, grid): self.grid = grid # all of these lists will be for instances of the classes self.correct = [] # which z=0 positions are in the correct place self.stacks = [] #dict.fromkeys(['x', 'y', 'z'], 0) self.fullStacks = [] self.emptySpaces = [] # empty space available. self.unknown = [ ] # list of 2d visualization with which ones are stacks we don't know below self.toStack = [] self.colourStacks = [] self.drone = Drone(grid, [0, 0])
def reset(self): """ Reset the Level """ self.completionRating = CompletionRating(self) self.moveRating = MoveRating(self) self.powerRating = PowerRating(self.getPowerRating(), self) self.minefield = Minefield(self.rows, self.columns) self.drone = Drone(self.minefield, self.moveRating, self.powerRating) self.defenseItems = [] for defenseClass in self.defenses: for i in range(self.defenses[defenseClass]): self.addDefense(defenseClass)
def mov(self, x_1, y_1, z_1): dro = Drone(self) sets = 10 zaux = [] xaux = [] yaux = [] ''' for i in range(self): dro.x[i] = uniform(0.0, (x_1)) dro.y[i] = uniform(0.0, (y_1)) dro.z[i] = uniform(0.0, (z_1)) ''' partes = self/sets for j in range(sets): par = randint(1, 4) theta = np.linspace((-1*par) * np.pi, par * np.pi, partes) z = uniform(np.linspace(randint(0, 2), randint(0, 2), partes), randint(-1, int(z_1))) r = (z**2 + 1)*x_1*y_1*z_1 x = uniform(r * np.sin(theta), randint(-1, int(x_1))) y = uniform(r * np.cos(theta), randint(-1, int(y_1))) """ print("X[",j,"]:",x) print("Y[",j,"]:",y) print("Z[",j,"]:",z) """ for i in range(int(partes)): xaux.append(x[i]) yaux.append(y[i]) zaux.append(z[i]) """ for i in range(int(partes)): dro.x[j*i] = x[i] #* randint(100,120)/100 dro.y[j*i] = y[i] * randint(100, 120)/100 dro.z[j*i] = z[i] # * randint(100, 120)/100 """ dro.x = xaux dro.y = yaux dro.z = zaux #x_1, y_1, z_1 = dro.x[i], dro.y[i], dro.z[i] return dro.x, dro.y, dro.z
def update(self, delta_time): Drone.update(self) self.period -= delta_time if self.period < 0: self.choose_direction() if self.x + self.rng > self.field_width: self.x_direction = -1 elif self.x - self.rng < 0: self.x_direction = 1 if self.y + self.rng > self.field_height: self.y_direction = -1 elif self.y - self.rng < 0: self.y_direction = 1
def __init__(self): self.x_max = 50 self.y_max = 50 self.world = World(self.x_max, self.y_max) #self.world.random() #self.world.generate_with_roads(resource_points=True, drop_points = True, recharge_points=True) self.world.generate_city(resource_points=True, drop_points=True, recharge_points=True) # drones may have different charaterisics, battery size, max lift, max weight carrying capacity, turning abilities? some might make only right turns? self.drones = [] self.drone_world_plots = [] self.message_dispatcher = MessageDispatcher() self.drone_alpha = Drone('URBAN0X1', self.world, self.message_dispatcher, cooperate=True) self.drones.append(self.drone_alpha) self.drone_beta = Drone('URBAN0X2', self.world, self.message_dispatcher, cooperate=True) self.drones.append(self.drone_beta) #self.drone_gamma = Drone('URBAN0X3', self.world,self.message_dispatcher, cooperate=False) #self.drones.append(self.drone_gamma) #self.fig = plt.figure(figsize=(1, 2))#, dpi=80, facecolor='w', edgecolor='k') self.drop_point_marker = 'o' #self.world.drop_point_locations = [random.sample(range(self.world.x_max), 10), random.sample(range(self.world.y_max), 10)] self.drop_point_plot = None #reacharge point self.recharge_point_marker = 's' #square self.recharge_point_plot = None #resources #self.fig = plt.figure(figsize=(1, 2))#, dpi=80, facecolor='w', edgecolor='k') #self.world.resource_locations = [[5,10,20,40,45],[5,10,20,40,45]] self.resource_plot = None self.resource_marker = 'v' self.markers = ['|', '+', 'x', '*'] self.colors = ['r', 'k', 'g', 'c', 'm', 'b'] self.init_figures()
def main(): drone1 = Drone() drone_speed = int(drone1.speed) drone_height = int(drone1.height) usr_input = input( 'Enter 1 for accelerate, 2 for decelerate, 3 for ascend, 4 for descend, 0 for exit: \n' ) usr_input = error_check(usr_input) while usr_input != 0: usr_input = int(usr_input) if usr_input == 1: drone1.accelerate() drone_speed = int(drone1.speed) print('Speed:', drone_speed, 'Height:', drone_height, '\n') usr_input = input( 'Enter 1 for accelerate, 2 for decelerate, 3 for ascend, 4 for descend, 0 for exit: \n' ) usr_input = error_check(usr_input) elif usr_input == 2: drone1.decelerate() drone_speed = int(drone1.speed) print('Speed:', drone_speed, 'Height:', drone_height, '\n') usr_input = input( 'Enter 1 for accelerate, 2 for decelerate, 3 for ascend, 4 for descend, 0 for exit: \n' ) usr_input = error_check(usr_input) elif usr_input == 3: drone1.ascend() drone_height = int(drone1.height) print('Speed:', drone_speed, 'Height:', drone_height, '\n') usr_input = input( 'Enter 1 for accelerate, 2 for decelerate, 3 for ascend, 4 for descend, 0 for exit: \n' ) usr_input = error_check(usr_input) elif usr_input == 4: drone1.descend() drone_height = int(drone1.height) print('Speed:', drone_speed, 'Height:', drone_height, '\n') usr_input = input( 'Enter 1 for accelerate, 2 for decelerate, 3 for ascend, 4 for descend, 0 for exit: \n' ) usr_input = error_check(usr_input) elif usr_input == 0: drone_speed = int(drone1.speed) drone_height = int(drone1.height) print('Speed:', drone_speed, 'Height:', drone_height, '\n')
def setEnemy(self, enum, x, y): if (enum == 0): return Drone(x, y) elif (enum == 1): return Fighter(x, y) elif (enum == 2): return Diver(x, y)
def connect(self): print('[+] Connection') self.s.settimeout(3) self.s.connect((self.host, self.port)) next_cmd = 'waiting_welcome' for line in self.recv_line(): print(' |-- %s' % line) if next_cmd == 'waiting_welcome': if line == 'BIENVENUE': self.send_msg(self.team) next_cmd = 'waiting_nbconnect' continue raise Exception('It seems we are not welcome... :(') if next_cmd == 'waiting_nbconnect': if line != 'ko': self.nbconnect = int(line) next_cmd = 'waiting_mapinfo' continue raise Exception('Invalid team') if next_cmd == 'waiting_mapinfo': (self.map_x, self.map_y) = [int(i) for i in line.split()] self.drone = Drone(self, self.map_x, self.map_y, self.team) break assert(self.drone) self.s.settimeout(None) print(' `-- Connection OK\n')
def __init__(self, config_file): with open(config_file, 'r') as f: self.rows, self.cols, self.num_drones, self.num_turns, self.max_payload = get_line(f.readline()) self.num_product_types = get_line_first(f.readline()) weights = get_line(f.readline()) self.products = [] for idx in range(self.num_product_types): self.products.append(Product(idx, weights[idx])) self.num_warehouses = get_line_first(f.readline()) self.warehouses = [] for warehouse_idx in range(self.num_warehouses): loc = tuple(get_line(f.readline())) warehouse = Warehouse(warehouse_idx, loc) for prod_idx, prod_quant in enumerate(get_line(f.readline())): prod = self.products[prod_idx] warehouse.add_product(prod, prod_quant) self.warehouses.append(warehouse) self.num_orders = get_line_first(f.readline()) self.orders = [] for order_idx in range(self.num_orders): loc = tuple(get_line(f.readline())) order = Order(order_idx, loc) f.readline() #just skip cause we know it from the next line for prod_id in get_line(f.readline()): prod = self.products[prod_id] order.add_product(prod) self.orders.append(order) self.drones = [Drone(drone_idx, self.warehouses[0].location, self.max_payload) \ for drone_idx in range(self.num_drones)]
def populate_drones(drones, max_payload, warehouse): id_ = -1 items = [] for i in xrange(drones): id_ += 1 items.append(Drone(id_, max_payload, warehouse.position)) return items
def start(self): factor = len(self.cycle) / (len(self.other_drones) + 1) start_position = self.unique_id * factor same_strategy_list = Drone.same_strategy_list(self) # Denotes mixed strategy if len(same_strategy_list) != len(self.other_drones.keys()): if len(same_strategy_list) != 0: self.type_id = self.unique_id / ((len(self.other_drones.keys()) + 1) / (len(same_strategy_list) + 1)) else: self.type_id = 0 factor = len(self.cycle) / (len(same_strategy_list) + 1) if self.type_id == len(same_strategy_list): # Last drone self.cycle = self.cycle[self.type_id * factor:] else: self.cycle = self.cycle[self.type_id * factor:(self.type_id + 1) * factor] self.index = len(self.cycle) / 2 self.goal = self.cycle[self.index] self.x = self.cycle[self.index][0] self.y = self.cycle[self.index][1] self.x_direction = (self.goal[0] - self.x) / self.velocity self.y_direction = (self.goal[1] - self.y) / self.velocity else: if self.unique_id == len(self.other_drones.keys()): # Last drone self.cycle = self.cycle[start_position:] else: self.cycle = self.cycle[start_position:(self.unique_id + 1) * factor] self.index = 0 self.goal = self.cycle[0] self.x = self.cycle[0][0] self.y = self.cycle[0][1] self.x_direction = (self.goal[0] - self.x) / self.velocity self.y_direction = (self.goal[1] - self.y) / self.velocity
def get_thrusts(self, drone: Drone) -> Tuple[float, float]: """Takes a given drone object, containing information about its current state and calculates a pair of thrust values for the left and right propellers. Args: drone (Drone): The drone object containing the information about the drones state. Returns: Tuple[float, float]: A pair of floating point values which respectively represent the thrust of the left and right propellers, must be between 0 and 1 inclusive. """ target_point = drone.get_next_target() dx = target_point[0] - drone.x dy = target_point[1] - drone.y thrust_adj = np.clip(dy * self.ky, -self.abs_thrust_delta, self.abs_thrust_delta) target_pitch = np.clip(dx * self.kx, -self.abs_pitch_delta, self.abs_pitch_delta) delta_pitch = target_pitch - drone.pitch thrust_left = np.clip(0.5 + thrust_adj + delta_pitch, 0.0, 1.0) thrust_right = np.clip(0.5 + thrust_adj - delta_pitch, 0.0, 1.0) # The default controller sets each propeller to a value of 0.5 0.5 to stay stationary. return (thrust_left, thrust_right)
def __init__(self, path): self.path = path input_data = self._get_input(path) self.deadline = input_data["deadline"] self.rows = input_data["rows"] self.columns = input_data["columns"] self.num_products = input_data["num_products"] self.num_drones = input_data["num_drones"] self.num_orders = input_data["num_orders"] self.num_warehouses = input_data["num_warehouses"] self.max_load = input_data["max_load"] self.products = [ Product(weight) for weight in input_data["product_weights"] ] self.warehouses = [ Warehouse(wh["row"], wh["column"], wh["stock"]) for wh in input_data["warehouses"] ] self.drones = [ Drone(i, self.warehouses, input_data["max_load"]) for i in range(input_data["num_drones"]) ] self.orders = [ Order(i, order["row"], order["column"], order["product_ids"]) for i, order in enumerate(input_data["orders"]) ]
def registerNewCallSign(self, ssid): assert isinstance(ssid, str) i = len(self.managedDrones) + 1 callsign = "%s-%i" % (self.squadronPrefix, i) self.managedDrones[ssid] = Drone(ssid, callsign) self.saveDroneList() return callsign
def main(): drone1 = Drone() choice = -1 while choice != 0: choice = int(input( 'Enter 1 for accelerate, 2 for decelerate,3 for ascend, 4 for decend, 0 for exit: ')) if choice == 1: drone1.accelerate() elif choice == 2: drone1.decelerate() elif choice == 3: drone1.ascend() elif choice == 4: drone1.descend() print(f'Speed: {drone1.speed} Height {drone1.height}')
def do_GET(self): self.send_response(200) self.send_header("Content-type", "text/html") self.end_headers() drone = Drone() message = drone.connect() drone.command(self.path[1:]) print(message) self.wfile.write( bytes("<html><head><title>Title goes here.</title></head>", "utf-8")) self.wfile.write(bytes("<body><p>This is a test.</p>", "utf-8")) self.wfile.write( bytes("<p>Drone is not connected %s</p>" % message, "utf-8")) self.wfile.write(bytes("</body></html>", "utf-8")) return
def deleteDrone(): try: _json = request.json title=_json['title'] return Drone.delete_one(query={'title':'Mavic Air'}) except Exception as e: print(e) return "Nothing Deleted"
def __init__(self): self.config_parser = ConfigObj('../config/config') self.droneNum = int(self.config_parser.get('NUMBER_OF_DRONES')) self.drones = [] for drone_id in range(1, self.droneNum): self.drones.append( Drone(drone_id, self.getWorkerNode('worker-1')['ipv4'][0]))
class Service: def __init__(self): self.__environment = Environment() self.__board = Board() self.__drone = Drone(self.__environment) def getEnvironment(self): return self.__environment def getBoard(self): return self.__board def getPositionStack(self): return self.__drone.getPositionStack() def getVisitedPositions(self): return self.__drone.getVisitedPositions() def getDroneXCoord(self): return self.__drone.getXCoord() def getDroneYCoord(self): return self.__drone.getYCoord() def droneOneStepDFS(self): return self.__drone.moveDFS(self.__board) def markBoardDetectedWalls(self): self.__board.markDetectedWalls(self.__environment, self.__drone.getXCoord(), self.__drone.getYCoord())
def __init__(self): self.w, self.h = 800, 600 PygameHelper.__init__(self, size=(self.w, self.h), fill=((255,255,255))) self.hud = Hud() self.drones = [] for i in range(4): tempagent = Drone() tempagent.pos = vec2d(int(uniform(0, self.w - 20)), int(uniform(0, 400))) target = vec2d(int(uniform(0, self.w - 20)), int(uniform(0, 400))) tempagent.target.append(target) self.drones.append(tempagent) self.selected = self.drones[0] self.builder = self.drones[0] self.project = vec2d(0, 0) self.buildtype = 0 self.buildpos = vec2d(0, 0) self.buildings = [] self.ores = [] count = 0 while count < 15: tempore = Ore() tempore.pos = vec2d(int(uniform(0, 1500)), int(uniform(0, 1500))) if not len(self.ores) == 0: flag = 1 for orecheck in self.ores: dist = orecheck.pos.get_distance(tempore.pos) if dist < 120: flag = 0 if flag: self.ores.append(tempore) count += 1 else: self.ores.append(tempore) count += 1 firstbuild = MainBase() firstbuild.pos = vec2d(300, 300) firstbuild.target = vec2d(firstbuild.pos[0] + 10, firstbuild.pos[1]) self.buildings.append(firstbuild)
def __init__(self, droneCount, capacity, warehouses, orders, productTypeWeights): self.drones = [] for i in range(0, droneCount): d = Drone(i, capacity, warehouses[0]['location']) self.drones.append(d) self.warehouses = warehouses self.orders = orders self.productTypeWeights = productTypeWeights
def findDrones(): #recieve that here first and give to query try: _json = request.json title=_json['title'] except Exception as e: print(e) query = {'title': title} return json_util.dumps(Drone.find_many(query))
def update(self, time): Drone.update(self) if Cyclic_Drone.check_reached_goal(self): if self.backtrack == True: self.backtrack = False self.goal = self.original_position self.on_path = True Cyclic_Drone.set_directions(self) elif self.on_path == True: self.on_path = False self.goal = self.back_goal Cyclic_Drone.set_directions(self) elif random.random() < self.probability: Cyclic_Drone.set_next_goal(self) self.back_goal = self.goal self.original_position = (self.x, self.y) self.backtrack = True self.set_next_goal() else: Cyclic_Drone.set_next_goal(self)
# latitude = float(latitude) # longitude = float(longitude) # altitude = float(altitude) # print "Set waypoint as %f, %f, %f?" % (latitude, longitude, altitude) # waypoint = LocationGlobalRelative(latitude, longitude, altitude) # print "WAYPOINT SET as %f, %f, %f?" % (latitude, longitude, altitude) if sitl: waypoint_location = frame_conversion.get_location_metres(start_location, 122, 0) # waypoint_location = LocationGlobalRelative(41.834575, -87.626842, 10) #simm else: waypoint_location = LocationGlobalRelative(41.837283, -87.624709, 10) #real field test = Drone(connection_string, waypoint_location.lat, waypoint_location.lon) # print "Clearing mission" # test.clear_mission() # test.download_mission() # print "Preparing mission" # test.prepare_mission() # print "Uploading mission" # test.upload_mission() # print "oops" # raw_input("Press enter to begin arming and taking off") # test.arm() # test.takeoff() # test.begin_mission()
class DroneClient(threading.Thread): """ Class representing a client which receive drone commands and send drone odometry. """ running = True def __init__(self, host="", port=9000): threading.Thread.__init__(self) self.host = host self.port = port self.drone = Drone() self.socket = None self.start() def run(self): """ Main loop. """ print "DroneClient: Connecting.." connected = False while not connected: try: self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) self.socket.connect((self.host, self.port)) self.socket.settimeout(2) connected = True except socket.error: continue print "DroneClient: Connected to host." while self.running: try: msg = self.socket.recv(1) if len(msg) == 0: print "Connection to server lost." self.close() elif msg == chr(10): print "Emergency." self.drone.emergency() elif msg == chr(0): print "Initialize." self.drone.initialize() self.socket.send(str(self.drone.getSize())) self.socket.send("\n") elif msg == chr(1): print "Shutdown" self.drone.shutdown() self.close() elif msg == chr(2): self.drone.move(ord(msg)) self.socket.send(self.drone.get_odometry()) self.drone.send("\n") elif msg == chr(3): self.drone.move(ord(msg)) self.socket.send(self.drone.get_odometry()) self.drone.send("\n") elif msg == chr(4): self.drone.move(ord(msg)) self.socket.send(self.drone.get_odometry()) self.drone.send("\n") elif msg == chr(5): self.drone.move(ord(msg)) self.socket.send(self.drone.get_odometry()) self.drone.send("\n") # Testing commands. elif msg == chr(6): self.drone.move(2) elif msg == chr(7): self.drone.move(3) elif msg == chr(8): self.drone.move(4) elif msg == chr(9): self.drone.move(5) elif msg == chr(11): self.drone.land() elif msg == chr(12): self.drone.initialize() elif msg == chr(13): self.socket.send(self.drone.get_odometry()) self.drone.send("\n") except socket.timeout: pass except socket.error, err: print err self.close() except KeyboardInterrupt: print "Interrupted." self.close()
from include_functions import * from drone import Drone from item import Item if __name__ == "__main__": read_input_file("data/ex.in") data = read_input_file("data/busy_day.in") read_input_file("data/mother_of_all_warehouses.in") read_input_file("data/redundancy.in") liste = populate_warehouses(data[4], data[5]) orders, warehouses, drones, items = populate_all("data/busy_day.in") return test_item = Item(123, 100) test_item2 = Item(123, 1) test_drone = Drone(200) print test_drone test_drone.move_to([10, 10]) print test_drone test_drone.add_item(test_item, None) test_drone.add_item(test_item, None) print test_drone test_drone.deliver_item(test_item, None) print test_drone test_drone.add_item(test_item2, None) print test_drone
def update(self, time): Drone.update(self) if self.check_reached_goal(): self.set_next_goal()
class Level: """ Represents a Level """ def __init__(self, init): """ Initialize the Level """ self.init = init self.name = init.name self.rows = init.rows self.columns = init.columns self.defenses = init.defenses self.reset() def reset(self): """ Reset the Level """ self.completionRating = CompletionRating(self) self.moveRating = MoveRating(self) self.powerRating = PowerRating(self.getPowerRating(), self) self.minefield = Minefield(self.rows, self.columns) self.drone = Drone(self.minefield, self.moveRating, self.powerRating) self.defenseItems = [] for defenseClass in self.defenses: for i in range(self.defenses[defenseClass]): self.addDefense(defenseClass) def addDefense(self, defenseClass): """ Add a Defense to the minefield """ defense = defenseClass() defenseAdder = defenseClass.adderClass() self.defenseItems.append(defense) defenseAdder.addDefense(defense, self.minefield) def getPowerRating(self): """ Returns the amount of power the drone should have on the level """ powerRating = self.rows*self.columns*SCAN_POWER for defenseClass in self.defenses: powerRating += self.defenses[defenseClass]*defenseClass.powerRating return powerRating def performGameCycle(self): """ Perform a single Game Cycle """ if not self.finished(): for defense in self.defenseItems: defense.performGameCycle(self.minefield, self.drone) elif self.won(): self.tryToAwardRatings() def lost(self): """ Return if the player has lost the level """ return self.destroyed() or self.noPower() def destroyed(self): """ Return if the player drone has been destroyed """ return self.drone.destroyed def noPower(self): """ Return if the player has no power """ return not self.drone.hasPower() def won(self): """ Return if the player has won the level """ won = True for defense in self.defenseItems: if not defense.isDeactivated(): return False else: return self.drone.powerRating.power >= 0 return won def finished(self): """ Return if the level is finished """ return self.lost() or self.won() def tryToAwardRatings(self): """ Try To award the Ratings """ self.completionRating.checkAwarded() self.moveRating.checkAwarded() self.powerRating.checkAwarded() CURRENT_PROFILE.achievement.award() def getRemainingDefenses(self): """ Return the number of Remaining defenses """ remainingDefenses = {} for defenseClass in self.defenses: remainingDefenses[defenseClass] = 0 for defense in self.defenseItems: if not defense.isDeactivated(): remainingDefenses[defense.__class__] += 1 return remainingDefenses def getID(self): """ Return the Level's ID """ return self.init.id
def add_line(text, offset): cv2.putText( image, text, (10, offset), cv2.FONT_HERSHEY_PLAIN, 0.8, (255, 255, 255) ) window = cv2.namedWindow("win") cv2.moveWindow("win", 800, 30) drone = Drone() ion() # interaction mode needs to be turned off r = 5 x = np.arange(0, 100, 1) # we'll create an x-axis from 0 to 2 pi yaw_error = np.arange(0, 100, 1, dtype="float") yaw_line, = plot(x, x, label="Yaw") yaw_line.axes.set_ylim(-r, r) roll_error = np.arange(0, 100, 1, dtype="float") roll_line, = plot(x, x, label="Roll") roll_line.axes.set_ylim(-r, r) pitch_error = np.arange(0, 100, 1, dtype="float")
def __init__(self, unique_id, radius, field_width, field_height, velocity, rng): Drone.__init__(self, unique_id, radius, field_width, field_height, velocity, rng) self.strategy = 'random-walk' self.x = random.randint(0 + self.rng, self.field_width - self.rng) self.y = random.randint(0 + self.rng, self.field_height - self.rng) self.choose_direction()
#!/usr/bin/python from drone import Drone drone = Drone() drone.mainLoop()
from drone import Drone import time try: drone = Drone() drone.initialize() time.sleep(10) drone.turn(90) x=0 while(x<100): print drone.getOdometry() time.sleep(0.05) x+=1 #drone.turn(-90) drone.shutdown() except KeyboardInterrupt: print('\n\nKeyboard exception received. Emergency shutdown.') drone.shutdown()
class Protocol: _ACTIONS = \ { # id | cmd | description # time 'go_ahead': ('avance', 'going ahead', 7), 'turn_right': ('droite', 'turning right', 7), 'turn_left': ('gauche', 'turning left', 7), 'get_inventory': ('inventaire', 'inventory ?', 1), 'see': ('voir', 'opening eyes', 7), 'grab_object': ('prend %s', 'grabbing %s', 7), 'drop_object': ('pose %s', 'dropping %s', 7), 'kick': ('expulse', 'kicking', 7), 'cry': ('broadcast %s', 'crying "%s"', 7), 'spell': ('incantation', 'hoyo... hoyo... hoyo...', 300), 'fork': ('connect_nbr', 'forking (maybe)', 42), 'real_fork': ('fork', 'forking (real)', 0), } def __init__(self, host, port, team): self.host = host self.port = port self.team = team (self.nbconnect, self.map_x, self.map_y) = (-1, -1, -1) self.drone = None self.real_fork_timer = None self.s = socket.socket() def print_action(self, desc): info = '[%s]' % utils.get_colored_text('lpurple', utils.get_id()) if self.drone: info += ' [level=%s]' % utils.get_colored_text('yellow', str(self.drone.level)) if self.drone.leader: info += ' [leader=%s]' % utils.get_colored_text('yellow', self.drone.leader) if self.drone.starvation: info += ' [%s]' % utils.get_colored_text('lblue', 'starvation') info += ' [health=%s]' % utils.get_colored_text('lred', str(self.drone.inventory.get('nourriture', 0))) if self.real_fork_timer: info += ' [birth t=-%s]' % utils.get_colored_text('lgreen', str(self.real_fork_timer)) print('%s %s' % (info, desc)) def send_msg(self, msg): #print('> %s' % msg) self.s.send('%s\n' % msg) def recv_line(self): filesocket = self.s.makefile() for line in filesocket: line = line.strip() #print('< %s' % line) yield line def do_action(self, id, data=None): assert(id in Protocol._ACTIONS) self.last_cmd = id (cmd, desc, t) = Protocol._ACTIONS[id] if self.real_fork_timer is not None: self.real_fork_timer -= t if self.real_fork_timer <= 0: self.real_fork_timer = None args = sys.argv if utils.term: args = [utils.term, '-e'] + args os.spawnv(os.P_NOWAIT, args[0], args) if data: desc %= data self.print_action(desc) msg = cmd if data is None else cmd % data self.send_msg(msg) def connect(self): print('[+] Connection') self.s.settimeout(3) self.s.connect((self.host, self.port)) next_cmd = 'waiting_welcome' for line in self.recv_line(): print(' |-- %s' % line) if next_cmd == 'waiting_welcome': if line == 'BIENVENUE': self.send_msg(self.team) next_cmd = 'waiting_nbconnect' continue raise Exception('It seems we are not welcome... :(') if next_cmd == 'waiting_nbconnect': if line != 'ko': self.nbconnect = int(line) next_cmd = 'waiting_mapinfo' continue raise Exception('Invalid team') if next_cmd == 'waiting_mapinfo': (self.map_x, self.map_y) = [int(i) for i in line.split()] self.drone = Drone(self, self.map_x, self.map_y, self.team) break assert(self.drone) self.s.settimeout(None) print(' `-- Connection OK\n') def run(self): self.connect() self.drone.do_something() for line in self.recv_line(): # Unespected events if line.startswith('mort'): self.print_action("I'm dead. :-(") if utils.term: raw_input('--- Press Enter to end ---') if line.startswith('deplacement'): self.drone.on_forced_move(line.split()[1:]) continue if line.startswith('message'): data = line.split(',') (key, K) = data[0].split() msg = BroadcastIn(data[1].lstrip(), K) if msg.direction is not None: self.drone.on_incoming_message(msg) continue if line.startswith('elevation'): self.drone.on_elevation(line.split()[1:]) continue if line.startswith('niveau actuel :'): lvl = int(line.split()[-1]) self.drone.on_levelup(lvl) self.drone.do_something() continue # Command return if line in ('ok', 'ko'): if line == 'ko': self.drone.on_fail() if self.last_cmd in ('get_inventory' 'see'): continue #raise Exception('Fork?') # Inventory if self.last_cmd == 'get_inventory': inventory = self.drone.inventory for i in line[1:-1].split(','): item, num = i.split() inventory[item] = int(num) # Fork elif self.last_cmd == 'fork': if int(line) == 0: self.do_action('real_fork') self.real_fork_timer = 600 continue else: self.real_fork_timer = 0 # View elif self.last_cmd == 'see': view = [] boxes = line[1:-1].split(',') i = 0 box_to_insert = 1 while i + box_to_insert <= len(boxes): view.append([data.split() for data in boxes[i:i + box_to_insert]]) i += box_to_insert box_to_insert += 2 self.drone.view = view level = len(view) - 1 if level != self.drone.level: self.drone.level = level self.drone.do_something()
def __init__(self, unique_id, radius, field_width, field_height, velocity, rng): Drone.__init__(self, unique_id, radius, field_width, field_height, velocity, rng) self.create_cycle() self.strategy = 'non-partition'
def init(self, *args, **kwargs): self.drone = Drone() self.discover_drone()
def spawn(self): if randint(0, 50) == 0: drone = Drone(self) drone.pos = pos(randint(20, self.size[0]-20*2), -drone.radius-2) self.add_entity(drone)
from drone import Drone # get current working directory path = os.getcwd() open_path = path[:path.rfind('src')] + 'cfg/' filename = 'drone_mouse.cfg' if __name__ == '__main__': # loading the ICE and ROS parameters ic = EasyIce.initialize(['main_drone.py', open_path + filename]) ic, node = comm.init(ic) # creating the object robot = Drone(ic) try: # executing the mouse behavior robot.take_off() time.sleep(1) while True: robot.stop() #~ robot.move("left") #~ time.sleep(3) #~ robot.stop() #~ time.sleep(1) #~ robot.move("back") #~ time.sleep(3)
parser = argparse.ArgumentParser(description="Camera feed grabber", formatter_class=argparse.ArgumentDefaultsHelpFormatter) parser.add_argument("-local", dest="local", help="local transport & ip to push from", default=local) parser.add_argument("-vc", dest="vc", help="video classifier host to pull from", default=vc) parser.add_argument("-i", dest="interval", type=int, help="Classify every i frames", default=50) parser.add_argument("-drone", dest="drone", action="store_true", help="Connect to drone", default=False) parser.add_argument("-d", dest="debug", action="store_true", help="show debug window", default=False) args = parser.parse_args() send_url = args.local + ":5555" if istcp(args.local) else args.local recv_url = args.vc + ":5556" if istcp(args.vc) else args.vc results_url = args.local + ":5557" if istcp(args.local) else args.local if args.drone: drone = Drone() drone.setup() else: drone = None run(send_url, recv_url, results_url, drone=drone, interval=args.interval, debug=args.debug) cv2.destroyAllWindows()
def pso2(): #Main Control Loop (as prototyped on 2/26 in Glennan Lounge) """Main function that runs when the module is called from the command line. Starts a ui_listener and a drone object, then runs a loop with updates every 50 ms. On each loop iteration, check the ui_state, update the PSO, and calculate command (for waypoint mode) or pass gamepad command (for manual override mode). """ #This is secretly a ROS node. Init it. rospy.init_node('pso') # Create listener to receive info from UI ui_listener = pso_network.UIListener() ui_listener.daemon = True ui_listener.start() ui_state = ui_listener.get_ui() # Create listener to recieve waypoints and corrections from planner. planner_listener = pso_network.PlannerListener() planner_listener.daemon = True planner_listener.start() waypoint = cv.CreateMat(4, 1, cv.CV_32FC1) tf_listener = tf.TransformListener() #Instatiate Drone Objects (defined in Drone.py) myDrone = Drone("192.168.1.1") nav = myDrone.get_navdata() #Preset flags running = True wait_on_emergency = False wait_on_liftoff = False wait_on_land = False #Create Kalman filter, state, and command vectors kalman = PsoKalman() u = cv.CreateMat(4, 1, cv.CV_32FC1) z = cv.CreateMat(5, 1, cv.CV_32FC1) sys_time = time.time() #Create PID controllers for each axis yaw_pid = pso_pid.PID() yaw_pid.k = 1.5 yaw_pid.t_i = 1. yaw_pid.angular = True yaw_pid.deadband = .05 z_pid = pso_pid.PID() z_pid.k = .00075 z_pid.i_enable = False z_pid.t_i = 10. z_pid.deadband = 150 roll_pid = pso_pid.PID() roll_pid.k = .0002 roll_pid.i_enable = False roll_pid.d_enable = True roll_pid.t_d = 1 roll_pid.deadband = 50 pitch_pid = pso_pid.PID() pitch_pid.k = .0002 pitch_pid.i_enable = False pitch_pid.d_enable = True pitch_pid.t_d = 1 pitch_pid.deadband = 50 #Logger puts state in csv for matlab-y goodness #logger = debuglogger.Logger() #Fig bucking loop while not rospy.is_shutdown(): time.sleep(.05) os.system("clear") #Get command state from UI prev_ui_state = ui_state ui_state = ui_listener.get_ui() if ui_state[EMERGENCY]: myDrone.emergency() if not prev_ui_state[EMERGENCY]: rospy.loginfo("User ordered emergency"); if ui_state[SHUTDOWN]: #UI has ordered shutdown rospy.loginfo( "Shutting down control loop...") ui_listener.stop() myDrone.kill() running = False if ui_state[TRIM]: myDrone.trim() ui_listener.clear_flag(TRIM) rospy.loginfo("Trimming Drone") if ui_state[FLYING]: myDrone.takeoff() print "Taking Off/Flying" if not prev_ui_state[FLYING]: wait_on_liftoff = 10 else: myDrone.land() print "Landing/Landed" if prev_ui_state[FLYING]: wait_on_land = 5 if ui_state[RESET]: rospy.loginfo("Resetting drone PSO and emergency state.") myDrone.reset() #myDrone.reset_emergency() yaw_pid.flush() z_pid.flush() roll_pid.flush() pitch_pid.flush() ui_listener.clear_flag(RESET) #Get navdata prevnav = nav nav = myDrone.get_navdata() #Print out Drone State if nav.check_state(navdata.EMERGENCY): print "Emergency!" if not prevnav.check_state(navdata.EMERGENCY): if(ui_state[EMERGENCY]): rospy.loginfo("Drone is in Emergency State") else: rospy.logerr("Unexpected drone emergency") elif not nav.check_state(navdata.COM_WATCHDOG): print "WATCHDOG" elif nav.check_state(navdata.COMMAND): print "Watchdog cleared. Not yet ready for commands." else: print "Ready to Fly\n" print "\t\tECACAVNAPCUWAPTHLGCMBNTTTCUACVVF\n{0}\t\t{1:32b}".format(nav.seq,nav.state) #Print navdata state rospy.loginfo("Navdata {0:4}: {1:32b}".format(nav.seq,nav.state)) #Update State (Kalman) dt = time.time()-sys_time print "dt:\t",dt sys_time = time.time() z[0, 0], z[1, 0], z[2, 0], z[3, 0], z[4, 0] = nav.vx, nav.vy, nav.z, nav.vz, nav.psi #z and u need to be cv matrices!!!! sys_state = myDrone.get_state() print "\nDrone Kalman State:" print "x:\t{0}".format(sys_state[0, 0]) print "y:\t{0}".format(sys_state[2, 0]) print "z:\t{0}".format(sys_state[4, 0]) print "vx:\t{0}".format(sys_state[1, 0]) print "vy:\t{0}".format(sys_state[3, 0]) print "vz:\t{0}".format(sys_state[5, 0]) print "theta:\t{0}".format(sys_state[6, 0]) print "vtheta:\t{0}".format(sys_state[7, 0]) print "\nNavdata Euler Angles:" print "theta:\t",nav.theta print "phi:\t",nav.phi print "psi:\t",nav.psi print "\nNavdata Stuff:" print "z:\t",nav.z print "vx:\t",nav.vx print "vy:\t",nav.vy ui_listener.set_state(sys_state, nav) br = tf.TransformBroadcaster() br.sendTransform((sys_state[0,0]/1000., sys_state[2,0]/1000., sys_state[4,0]/1000), tf.transformations.quaternion_from_euler(nav.phi, nav.psi, -sys_state[6, 0]-math.pi/2), rospy.Time.now(), "/base_footprint", "/odom") #print "ROS Time:\t",rospy.Time.now() #logger.log(sys_state) if wait_on_liftoff>0: print "Waiting for liftoff to finish" wait_on_liftoff -= dt u[0, 0], u[1, 0], u[2, 0], u[3, 0] = 0, 0, 1, 0#Assume drone goes full speed up when taking off elif ui_state[FLYING]: #If Drone is in waypoint mode, compute command if not ui_state[OVERRIDE]: #Get waypoint if at_waypoint() and not planner_listener.waypoints.empty(): waypoint = planner_listener.waypoints.get() rospy.loginfo("Next Waypoint: {0}, {1}, {2}, {3}".format(waypoint[0, 0], waypoint[1, 0], waypoint[2, 0], waypoint[3, 0])) print "\nNext Waypoint:" print "X:\t", waypoint[0, 0] print "Y:\t", waypoint[1, 0] print "Z:\t", waypoint[2, 0] print "θ:\t", waypoint[3, 0] #Compute command (roll_des,pitch_des) = world2drone(waypoint[0, 0]-sys_state[0, 0], waypoint[1, 0]-sys_state[2, 0], sys_state[6, 0]) print "Desired Roll:\t", roll_des print "Desired Pitch:\t", pitch_des u[0, 0] = pitch_pid.update(pitch_des) u[1, 0] = roll_pid.update(-roll_des) u[2, 0] = 0#z_pid.update(sys_state[4, 0], waypoint[2, 0]) u[3, 0] = yaw_pid.update(sys_state[6,0], waypoint[3,0]) myDrone.go(u[0,0], u[1,0], u[3, 0], u[2,0]) else: #Manual override: Use command from UI state print "\nManual override mode\n\n\n" myDrone.go(ui_state[COMMAND][0], ui_state[COMMAND][1], ui_state[COMMAND][2], ui_state[COMMAND][3]) rospy.logdebug("Sent Command to Drone: {0}, {1}, {2}, {3}".format(ui_state[COMMAND][0], ui_state[COMMAND][1], ui_state[COMMAND][2], ui_state[COMMAND][3])) u[0, 0], u[1, 0], u[2, 0], u[3, 0] = ui_state[COMMAND] else: print "\nLanded" #Print out commands print "\nCommands:\npitch:\t",u[0, 0] print "roll:\t", u[1, 0] print "gaz:\t", u[2, 0] print "yaw:\t", u[3, 0]