bebop = Bebop() print("connecting") success = bebop.connect(10) print(success) print("sleeping") bebop.smart_sleep(5) bebop.ask_for_state_update() bebop.safe_takeoff(10) # do it using move_relative() # fly forward bebop.move_relative(1, 0, 0, 0) bebop.smart_sleep(2) # fly right bebop.move_relative(0, 1, 0, 0) bebop.smart_sleep(2) # fly backward bebop.move_relative(-1, 0, 0, 0) bebop.smart_sleep(2) # fly left bebop.move_relative(0, -1, 0, 0)
bebop = Bebop() success = bebop.connect(2) jumpSizes = float( input( "Select your Jump size:\n\n2m Jumps (Type 2)" "\n1m Jumps (Type 1)\n0.5m Jumps (Type 0.5)\n0.25m Jumps (Type 0.25)\n" )) bebop.safe_takeoff(10) totalJump = 2 numberOfJumps = int(totalJump / jumpSizes) startingPosition = getRealLocation(bebop, False) expectedLocation = startingPosition try: for i in range(numberOfJumps): # fly left bebop.move_relative(0, -jumpSizes, 0, 0) #Orient the copter expectedLocation = numpy.add((-jumpSizes, 0), expectedLocation) orientCopter(expectedLocation, bebop) for i in range(numberOfJumps): #fly forward bebop.move_relative(jumpSizes, 0, 0, 0) #Orient the copter expectedLocation = numpy.add((0, jumpSizes), expectedLocation) orientCopter(expectedLocation, bebop) for i in range(numberOfJumps): # fly right bebop.move_relative(0, jumpSizes, 0, 0) #Orient the copter
print("PositionChanged_altitude: " + str(bebop.sensors.sensors_dict["PositionChanged_altitude"])) print("HomeChanged_longitude: " + str(bebop.sensors.sensors_dict["HomeChanged_longitude"])) print("HomeChanged_latitude: " + str(bebop.sensors.sensors_dict["HomeChanged_latitude"])) print("HomeChanged_altitude: " + str(bebop.sensors.sensors_dict["HomeChanged_altitude"])) #bebop.smart_sleep(5) bebop.safe_takeoff(10) bebop.smart_sleep(5) bebop.move_relative(0, 0, -2, 0) #move_relative(self, dx, dy, dz, dradians) print("First Move Done") bebop.smart_sleep(1) print("flip left") print("flying state is %s" % bebop.sensors.flying_state) success = bebop.flip(direction="left") print("mambo flip result %s" % success) bebop.smart_sleep(2) #bebop.move_relative(0, 0, 0, 6.28) #print("Second Move Done") #print("flip right") #print("flying state is %s" % bebop.sensors.flying_state)
# make my mambo object # remember to set True/False for the wifi depending on if you are using the wifi or the BLE to connect if (success): # get the state information print("sleeping") bebop.smart_sleep(2) bebop.ask_for_state_update() bebop.smart_sleep(5) print("taking off!") bebop.safe_takeoff(5) bebop.fly_direct(roll=0, pitch=0, yaw=0, vertical_movement=50, duration=1) bebop.smart_sleep(3) bebop.move_relative(3, 0, 0, 6.3) #bebop.smart_sleep(3) #bebop.fly_direct(roll=0, pitch=0, yaw=0, vertical_movement=-50, duration=1) #bebop.smart_sleep(2) #bebop.flip(direction="front") # bebop.fly_direct(roll=-25,pitch=0, yaw=-25,vertical_movement=10, duration=3) print("landing") bebop.safe_land(5) bebop.smart_sleep(5) print("disconnect") bebop.disconnect()
class PyParrot(gui.MyFrame1): def __init__(self, parent): gui.MyFrame1.__init__(self, parent) self.statusBar.SetStatusWidths([100, -1]) self.statusBar.SetStatusText('Not Connected') self.lc_commands.InsertColumn(0, 'command', width=300) self.bebop = Bebop() # load saved commands from file self._loadJsonFile() def OnClose( self, event ): if wx.MessageBox("Commands are not yet saved. Do you want to save now?", "Save Changes", wx.ICON_QUESTION | wx.YES_NO) == wx.YES: # Retrive commands from lc_commands cmdList = [] for i in range(self.lc_commands.GetItemCount()): cmdList.append(self.lc_commands.GetItemText(i)) self._saveJsonFile(cmdList) self.OnDisconnect(None) event.Skip() # calls the parent close method def OnConnect( self, event ): isConnected = self.bebop.connect(10) statusText = 'Connected' if not isConnected: statusText = 'Not ' + statusText self.statusBar.SetStatusText(statusText) def OnDisconnect( self, event ): self.bebop.disconnect() self.statusBar.SetStatusText('Disconnected') self.statusBar.SetStatusText(f'Battery: {self.bebop.sensors.battery}%', 1) def OnTakeOff( self, event ): self.bebop.safe_takeoff(10) # By default, setting the status text only sets the first # one. So we must specify the second status text (index 1 # , instead of index 0) self.statusBar.SetStatusText('Taking off', 1) def OnLand( self, event ): self.bebop.safe_land(10) self.statusBar.SetStatusText('Landing', 1) def OnAddTakeOff( self, event ): self._addCommand(f'{CmdType.TakeOff.value},10') def OnAddLand( self, event ): self._addCommand(f'{CmdType.Land.value},10') def OnAddRelative( self, event ): x = int(self.fld_x.GetValue()) y = int(self.fld_y.GetValue()) z = int(self.fld_z.GetValue()) rads = int(self.fld_radians.GetValue()) self._addCommand(f'{CmdType.Relative.value},{x},{y},{z},{rads}') def OnAddDirect( self, event ): roll = int(self.fld_roll.GetValue()) pitch = int(self.fld_pitch.GetValue()) yaw = int(self.fld_yaw.GetValue()) vertical = int(self.fld_vertical.GetValue()) duration = int(self.fld_duration.GetValue()) self._addCommand(f'{CmdType.Direct.value},{roll},{pitch},{yaw},{vertical},{duration}') def OnAddSleep( self, event ): sleep = int(self.fld_sleep.GetValue()) self._addCommand(f'{CmdType.Sleep.value},{sleep}') def OnAddFlip( self, event ): dir = str(self.fld_direction.GetValue()) self._addCommand(f'{CmdType.Flip.value},{dir}') def OnAddFlyGrid(self, event): print("Make sure you take off first!") heightGain = int(self.fld_gridHeight.GetValue()) lengthForward = int(self.fld_gridLength.GetValue()) width = int(self.fld_gridWidth.GetValue()) biLines = int(self.fld_gridLines.GetValue()) widthSection = width/(biLines+1) # Rise self._addCommand(f'{CmdType.Relative.value},0,0,{heightGain},0') # Fly forward and turn right self._addCommand(f'{CmdType.Relative.value},{lengthForward},0,0,90') # Flip self._addCommand(f'{"flip"},{"up"}') # Fly right and turn right self._addCommand(f'{CmdType.Relative.value},{widthSection},0,0,90') # Flip self._addCommand(f'{"flip"},{"up"}') # Fly back and turn left self._addCommand(f'{CmdType.Relative.value},{lengthForward},0,0,-90') # Flip self._addCommand(f'{"flip"},{"up"}') while (biLines >= 2): biLines -= 2 # Fly right and turn left self._addCommand(f'{CmdType.Relative.value},{widthSection},0,0,-90') # Flip self._addCommand(f'{"flip"},{"up"}') # Fly forward and turn right self._addCommand(f'{CmdType.Relative.value},{lengthForward},0,0,90') # Flip self._addCommand(f'{"flip"},{"up"}') # Fly right and turn right self._addCommand(f'{CmdType.Relative.value},{widthSection},0,0,90') # Flip self._addCommand(f'{"flip"},{"up"}') # Fly back and turn left self._addCommand(f'{CmdType.Relative.value},{lengthForward},0,0,-90') # Flip self._addCommand(f'{"flip"},{"up"}') if (biLines == 1): biLines -= 1 # Fly right and turn left self._addCommand(f'{CmdType.Relative.value},{widthSection},0,0,-90') # Flip self._addCommand(f'{"flip"},{"up"}') # Fly forward self._addCommand(f'{CmdType.Relative.value},{lengthForward},0,0,0') # Flip self._addCommand(f'{"flip"},{"up"}') # Fly back left to home self._addCommand(f'{CmdType.Relative.value},{-lengthForward},{-width},0,0') # Flip self._addCommand(f'{"flip"},{"up"}') # Descend self._addCommand(f'{CmdType.Relative.value},0,0,{-heightGain},0') else: # Fly left and turn left self._addCommand(f'{CmdType.Relative.value},{-width},0,0,-90') # Flip self._addCommand(f'{"flip"},{"up"}') # Descend self._addCommand(f'{CmdType.Relative.value},0,0,{-heightGain},0') print("Make sure you land!") def OnRemove( self, event ): self.lc_commands.DeleteItem(self.lc_commands.GetFocusedItem()) def OnUp( self, event ): """Swap the selected item with the one above it""" index = self.lc_commands.GetFocusedItem() if index >= 1: selItemStr = self.lc_commands.GetItemText(index) aboveItemStr = self.lc_commands.GetItemText(index-1) self.lc_commands.SetItemText(index, aboveItemStr) self.lc_commands.SetItemText(index-1, selItemStr) self.lc_commands.Focus(index-1) def OnDown( self, event ): """Swap the selected item with the one below it""" index = self.lc_commands.GetFocusedItem() if index < self.lc_commands.GetItemCount() - 1: selItemStr = self.lc_commands.GetItemText(index) belowItemStr = self.lc_commands.GetItemText(index+1) self.lc_commands.SetItemText(index, belowItemStr) self.lc_commands.SetItemText(index+1, selItemStr) self.lc_commands.Focus(index+1) def OnClear( self, event ): self.lc_commands.DeleteAllItems() def OnRunFromSelection( self, event ): index = self.lc_commands.GetFocusedItem() while index > 0: self.lc_commands.DeleteItem(index - 1) index = self.lc_commands.GetFocusedItem() self.OnRunCommands(event) def OnRunCommands( self, event ): """Go through each item in lc_commands and convert the string to a list. Then use the first item in the list to determine the command type, and the rest of the items are the params""" # Retrive commands from lc_commands cmdList = [] for i in range(self.lc_commands.GetItemCount()): cmdList.append(self.lc_commands.GetItemText(i)) self._saveJsonFile(cmdList) # Abort running attempt if not connected if not self.bebop.drone_connection.is_connected: print("No Connection. Aborting process.") return # === Run Commands === for i in range(self.lc_commands.GetItemCount()): args = self.lc_commands.GetItemText(i).split(',') self.statusBar.SetStatusText(f'Executing command: {args}', 1) try: if (args[0] == CmdType.Sleep.value): self.bebop.smart_sleep(int(args[1])) elif args[0] == CmdType.TakeOff.value: self.bebop.safe_takeoff(int(args[1])) elif args[0] == CmdType.Land.value: self.bebop.safe_land(int(args[1])) elif args[0] == CmdType.Direct.value: self.bebop.fly_direct(int(args[1]), int(args[2]), int(args[3]), int(args[4]), int(args[5])) elif args[0] == CmdType.Relative.value: self.bebop.move_relative(int(args[1]), int(args[2]), int(args[3]), math.radians(int(args[4]))) elif args[0] == CmdType.Flip.value: self.bebop.flip(str(args[1])) except Exception as e: print("Error occurred: ") print(e) continue def _loadJsonFile(self): # Open up JSON file with last run commands filePath = os.getcwd() + os.sep + "cmd_data.json" if os.path.isfile(filePath): f = open(filePath,"r") s = f.read() commands = json.loads(s) # Input commands into GUI interface for c in commands: self._addCommand(c) def _saveJsonFile(self, cmdList): # Place all commands into JSON file and write them to the disk jsonStr = json.dumps(cmdList) filePath = os.getcwd() + os.sep + "cmd_data.json" with open(filePath,"w") as f: f.write(jsonStr) def _addCommand(self, cmd: str): self.lc_commands.InsertItem(self.lc_commands.GetItemCount(), cmd) self.statusBar.SetStatusText(f'Added command: {cmd}', 1)
try: exit = input("\n\n\n\tExit the program and land the copter (y/n)?") if exit in yesStrings: break x = float( input("\n\n\n\tPlease input your desired coordinates:\n\tX: ")) y = float(input("\n\tY: ")) z = float(input("\n\tZ: ")) rotation = float(input("\n\tCopter Rotation (Radians): ")) delta = numpy.subtract((x, y, z, rotation), previousPosition) previousPosition = (x, y, z, rotation) print(delta) bebop.move_relative(0, 0, -delta[2], 0) bebop.move_relative(0, delta[0], 0, 0) bebop.move_relative(delta[1], 0, 0, 0) bebop.move_relative(0, 0, 0, delta[3]) except: bebop.emergency_land() bebop.disconnect() print("\tError found. Closing application") import sys sys.exit(0) bebop.safe_land(10) bebop.disconnect()
class drone: def __init__(self, home): self.rango_largo = properties.RANGO_LARGO self.rango_ancho = properties.RANGO_ANCHO self.mapa_largo = properties.MAPA_LARGO / self.rango_largo self.mapa_ancho = properties.MAPA_ANCHO / self.rango_ancho self.ip = None self.port = None self.search_map = [[0 for j in range(int(self.mapa_largo))]for i in range(int(self.mapa_ancho))] self.current_position = home self.current_rotation = math.pi / 2 self.mutex_search_map = threading.Lock() self.poi_position = None self.home = home self.bebop = Bebop() self.init_time = None self.obstaculos = properties.OBSTACLES self.max_altitude = properties.MAX_ALTITUDE self.pathToFollow = None self.destinationZone = None self.countIter = 0 self.logMapTimestamp = None self.logMap = None def initialize(self, ip, port): self.initSearchMapWithObstacles() init_time = time.time() self.init_time = init_time if properties.ALGORITHM == SH_ORIGINAL: self.search_map[self.home[0]][self.home[1]] = 1 elif properties.ALGORITHM == SH_TIMESTAMP or ALGORITHM == SH_NO_GREEDY_TIMESTAMP or ALGORITHM == RANDOM: self.search_map = [[init_time for j in range(int(self.mapa_largo))]for i in range(int(self.mapa_ancho))] # elif properties.ALGORITHM == RANDOM: # self.search_map[self.home[0]][self.home[1]] = 1 self.ip = ip self.port = port # success = self.bebop.connect(10) # print(success) self.bebop.set_max_altitude(self.max_altitude) self.bebop.ask_for_state_update() if properties.STREAMING_MODE_ON: self.initializeStreaming() self.logMap = [[0 for j in range(int(self.mapa_largo))]for i in range(int(self.mapa_ancho))] self.logMapTimestamp = [[init_time for j in range(int(self.mapa_largo))]for i in range(int(self.mapa_ancho))] def initSearchMapWithObstacles(self): for obstacle in self.obstaculos: self.search_map[obstacle[0]][obstacle[1]] = -1 def take_off(self): self.bebop.safe_takeoff(10) def land(self): self.bebop.safe_land(10) def move(self, new_position): verticalMove = self.getDronVerticalAlignment() if properties.ROTATE: rotation_diff = utils.angleDifference(self.current_position, new_position, self.current_rotation) distance_diff = utils.cartesianDistance(self.current_position, new_position) self.bebop.move_relative(0, 0, 0, rotation_diff) time.sleep(2) self.bebop.move_relative(distance_diff, 0, verticalMove, 0) self.current_rotation -= rotation_diff time.sleep(2) else: dx, dy = new_position[0] - self.current_position[0], new_position[1] - self.current_position[1] real_dx, real_dy = dx * self.rango_ancho, dy * self.rango_largo self.bebop.move_relative(real_dx, real_dy, verticalMove, 0) time.sleep(2) self.current_position = new_position self.mutex_search_map.acquire() utils.printMatrix(self.search_map) self.mutex_search_map.release() def setPoiPosition(self, poiPosition): self.poi_position = poiPosition def explore(self, forcePosition): if properties.ALGORITHM == SH_ORIGINAL: return self.explore_sh_original(forcePosition) elif properties.ALGORITHM == SH_TIMESTAMP: return self.explore_sh_timestamp(forcePosition) elif properties.ALGORITHM == RANDOM: return self.explore_random(forcePosition) elif properties.ALGORITHM == SH_NO_GREEDY or properties.ALGORITHM == SH_NO_GREEDY_TIMESTAMP: return self.explore_sh_no_greedy2(forcePosition) def explore_sh_original(self, forcePosition): firstTime = True x = self.current_position[0] y = self.current_position[1] best_values = [] for y2 in range(-1, 2): for x2 in range(-1, 2): x3 = x + x2 y3 = y + y2 if self.validatePosition(x3, y3, forcePosition): if firstTime: self.mutex_search_map.acquire() val = self.search_map[x3][y3] self.mutex_search_map.release() firstTime = False # print("x3: " + str(x3) + " y3: " + str(y3) + " self.mapa_ancho: " + str(self.mapa_ancho) + " self.mapa_largo: " + str(self.mapa_largo)) self.mutex_search_map.acquire() if (self.search_map[x3][y3] == val): best_values.append((x3, y3)) val = self.search_map[x3][y3] elif self.search_map[x3][y3] < val: best_values = [(x3, y3)] val = self.search_map[x3][y3] self.mutex_search_map.release() selected = self.selectBestValue(best_values) return selected def explore_sh_timestamp(self, forcePosition): firstTime = True x = self.current_position[0] y = self.current_position[1] best_values = [] for y2 in range(-1, 2): for x2 in range(-1, 2): x3 = x + x2 y3 = y + y2 if self.validatePosition(x3, y3, forcePosition): if firstTime: self.mutex_search_map.acquire() val = self.search_map[x3][y3] self.mutex_search_map.release() firstTime = False self.mutex_search_map.acquire() if (self.search_map[x3][y3] == val): best_values.append((x3, y3)) val = self.search_map[x3][y3] elif self.search_map[x3][y3] < val: best_values = [(x3, y3)] val = self.search_map[x3][y3] self.mutex_search_map.release() selected = self.selectBestValue(best_values) return selected def explore_random(self, forcePosition): x = self.current_position[0] y = self.current_position[1] best_values = [] for y2 in range(-1, 2): for x2 in range(-1, 2): x3 = x + x2 y3 = y + y2 if self.validatePosition(x3, y3, forcePosition): best_values.append((x3, y3)) selected = self.selectBestValue(best_values) return selected # primer intento de algoritmo no greedy, si se lo trabaja un poco puede funcionar def explore_sh_no_greedy(self, forcePosition): firstTime = True x = self.current_position[0] y = self.current_position[1] best_values = [] for y2 in range(-1, 2): for x2 in range(-1, 2): x3 = x + x2 y3 = y + y2 if self.validatePosition(x3, y3, forcePosition): if firstTime: self.mutex_search_map.acquire() val = self.search_map[x3][y3] self.mutex_search_map.release() firstTime = False self.mutex_search_map.acquire() currentRegion = self.getCurrentRegion() bestRegion = self.selectUnexploredRegion() if self.isClosestToBestRegion(currentRegion, bestRegion, x2, y2): if (self.search_map[x3][y3] == val): best_values.append((x3, y3)) val = self.search_map[x3][y3] elif self.search_map[x3][y3] < val: best_values = [(x3, y3)] val = self.search_map[x3][y3] self.mutex_search_map.release() selected = self.selectBestValue(best_values) return selected def explore_sh_no_greedy2(self, forcePosition): if self.pathToFollow is not None and len(self.pathToFollow) > 0: return self.getNewPositionFromPath() else: newZone = self.isChangeZone() if newZone is not None: self.destinationZone = newZone self.getZonePath(newZone) # self.selectNewZone(newZone) return self.getNewPositionFromPath() else: if ALGORITHM == SH_NO_GREEDY: return self.explore_sh_original(forcePosition) else: return self.explore_sh_timestamp(forcePosition) def explore_sh_no_greedy_timestamp(self, forcePosition): if self.pathToFollow is not None and len(self.pathToFollow) > 0: return self.getNewPositionFromPath() else: newZone = self.isChangeZone() if newZone is not None: self.destinationZone = newZone self.getZonePath(newZone) # self.selectNewZone(newZone) return self.getNewPositionFromPath() else: return self.explore_sh_timestamp(forcePosition) def getNewPositionFromPath(self): new_position = self.pathToFollow[0] del self.pathToFollow[0] if self.getPositionZone(new_position) == self.destinationZone and self.isUnexploredPosition(new_position): self.pathToFollow = None self.destinationZone = None return new_position def isUnexploredPosition(self, new_position): if ALGORITHM == SH_NO_GREEDY_TIMESTAMP or ALGORITHM == SH_TIMESTAMP: timeBetweenLastVisit = time.time() - self.search_map[new_position[0]][new_position[1]] firstTime = self.search_map[new_position[0]][new_position[1]] == self.init_time return timeBetweenLastVisit > TIME_COVERAGE_REFRESH or firstTime else: return self.search_map[new_position[0]][new_position[1]] == self.countIter def isChangeZone(self): currentZone = self.getCurrentRegion() zonesCoverage = self.getZonesCoverage() currentZoneCoverage = zonesCoverage[currentZone - 1] minZoneCoverage = min(zonesCoverage) if minZoneCoverage >= MIN_ACCEPTABLE_COVERAGE: self.countIter += 1 # self.updateCoverageCondition() zonesCoverage = self.getZonesCoverage() currentZoneCoverage = zonesCoverage[currentZone - 1] minZoneCoverage = min(zonesCoverage) if currentZoneCoverage >= MIN_ACCEPTABLE_COVERAGE or currentZoneCoverage - minZoneCoverage > COVERAGE_THRESHOLD: newZone = self.selectUnexploredRegion() return newZone return None def updateCoverageCondition(self): if self.init_time is not None: self.init_time = time.time() else: self.countIter += 1 def getZonePath(self, newZone): zonePosition = self.selectPositionInZone(newZone) pathfinder = Pathfinder(self.current_position, zonePosition) pathToFollow = pathfinder.findPath() self.pathToFollow = pathfinder.parsePathToCoordinates(pathToFollow) def selectPositionInZone(self, zone): if zone == 1: position = (0, 0) elif zone == 2: position = (0, int(self.mapa_largo - 1)) elif zone == 3: position = (int(self.mapa_ancho - 1), 0) else: position = (int(self.mapa_ancho - 1), int(self.mapa_largo - 1)) # if zone == 1 or zone == 2: # while position[0] < self.mapa_ancho and self.search_map[position[0], position[1]] == -1: # position = (position[0] + 1, 0) # elif zone == 3 or zone == 4: # while position[0] < self.mapa_largo and self.search_map[position[0], position[1]] == -1: # position = (0, position[1] + 1) # asumo que encuentro posicion valida return position def selectNewZone(self, currentZone): newZone = random.randint(0, 3) while newZone == currentZone: newZone = random.randint(0, 3) return newZone def isClosestToBestRegion(currentRegion, bestRegion, x, y): if currentRegion == bestRegion: return True bestCoordinates = getBestCoordinates(currentRegion, bestRegion) if x in bestCoordinates[0] and y in bestCoordinates[1]: return True return False def getBestCoordinates(currentRegion, bestRegion): if currentRegion == 1: if bestRegion == 2: return [(-1, 1), (0, 1), (1, 1)] elif bestRegion == 3: return [(1, -1), (1, 0), (1, 1)] else: return [(0, 1), (1, 1), (1, 0)] elif currentRegion == 2: if bestRegion == 1: return [(-1, -1), (0, -1), (1, 1)] elif bestRegion == 3: return [(1, -1), (0, -1), (1, 0)] else: return [(1, -1), (1, 0), (1, 1)] elif currentRegion == 3: if bestRegion == 1: return [(-1, -1), (-1, 0), (-1, 1)] elif bestRegion == 2: return [(-1, 0), (-1, 1), (0, 1)] else: return [(-1, 1), (0, 1), (1, 1)] else: if bestRegion == 1: return [(-1, -1), (-1, 0), (0, -1)] elif bestRegion == 2: return [(-1, -1), (-1, 0), (-1, 1)] else: return [(-1, -1), (0, -1), (1, -1)] def getCurrentRegion(self): return self.getPositionZone(self.current_position) def getPositionZone(self, position): if self.mapa_largo / 2 > position[1] and self .mapa_ancho / 2 > position[0]: return 1 elif self.mapa_largo / 2 <= position[1] and self .mapa_ancho / 2 > position[0]: return 2 elif self.mapa_largo / 2 > position[1] and self .mapa_ancho / 2 <= position[0]: return 3 elif self.mapa_largo / 2 <= position[1] and self .mapa_ancho / 2 <= position[0]: return 4 def selectUnexploredRegion(self): regionCoverage = [] regionCoverage.append(getMapCoverage(self, 0, self.mapa_ancho / 2, 0, self.mapa_largo / 2)) regionCoverage.append(getMapCoverage(self, self.mapa_ancho / 2, self.mapa_ancho, 0, self.mapa_largo / 2)) regionCoverage.append(getMapCoverage(self, 0, self.mapa_ancho / 2, self.mapa_largo / 2, self.mapa_largo)) regionCoverage.append(getMapCoverage(self, self.mapa_ancho / 2, self.mapa_ancho, self.mapa_largo / 2, self.mapa_largo)) selectedZone = regionCoverage.index(min(regionCoverage)) + 1 if selectedZone == self.getCurrentRegion(): return None return selectedZone def getZonesCoverage(self): regionCoverage = [] regionCoverage.append(getMapCoverage(self, 0, self.mapa_ancho / 2, 0, self.mapa_largo / 2)) regionCoverage.append(getMapCoverage(self, self.mapa_ancho / 2, self.mapa_ancho, 0, self.mapa_largo / 2)) regionCoverage.append(getMapCoverage(self, 0, self.mapa_ancho / 2, self.mapa_largo / 2, self.mapa_largo)) regionCoverage.append(getMapCoverage(self, self.mapa_ancho / 2, self.mapa_ancho, self.mapa_largo / 2, self.mapa_largo)) return regionCoverage def selectBestValue(self, best_values): lenght = len(best_values) # print("selectBestValue: " + str(lenght)) if(lenght == 1): return best_values[0] else: selected = random.randint(0, lenght - 1) return best_values[selected] def validatePosition(self, x3, y3, forcePosition): condition = x3 >= 0 and y3 >= 0 and x3 < self.mapa_ancho and y3 < self.mapa_largo tupla = (x3, y3) if self.pointIsObstacule(x3, y3): return False elif (forcePosition is not None): return (condition and self.minDistanceToTarget(self.home, self.current_position, tupla)) elif self.poi_position is not None: return (condition and self.minDistanceToTarget(self.poi_position, self.current_position, tupla)) elif self.checkBatteryStatus() == NORMAL: return condition else: return (condition and self.minDistanceToTarget(self.home, self.current_position, tupla)) def minDistanceToTarget(self, target, positionA, positionB): # print("minDistanceToTarget") distance2 = self.calculateDistance(target, positionA) distance1 = self.calculateDistance(target, positionB) # print("distance1: " + str(distance1) + " distance2: " + str(distance2)) return (distance1 <= distance2) def pointIsObstacule(self, x1, x2): isObstacule = False for obs in self.obstaculos: if obs[0] == x1 and obs[1] == x2: isObstacule = True return isObstacule def calculateDistance(self, tuple1, tuple2): return math.sqrt((tuple2[1] - tuple1[1])**2 + (tuple2[0] - tuple1[0])**2) def updateSearchMap(self, tupla): self.mutex_search_map.acquire() if properties.ALGORITHM == SH_ORIGINAL or properties.ALGORITHM == SH_NO_GREEDY: self.search_map[tupla[0]][tupla[1]] += 1 elif properties.ALGORITHM == SH_TIMESTAMP or ALGORITHM == SH_NO_GREEDY_TIMESTAMP or ALGORITHM == RANDOM: self.search_map[tupla[0]][tupla[1]] = time.time() self.logMap[tupla[0]][tupla[1]] += 1 self.logMapTimestamp[tupla[0]][tupla[1]] = time.time() self.mutex_search_map.release() def getSearchMap(self): # self.mutex_search_map.acquire() return [[self.search_map[i][j] for j in range(int(self.mapa_largo))]for i in range(int(self.mapa_ancho))] # self.mutex_search_map.release() def getDroneAltitude(self): return self.bebop.sensors.sensors_dict["AltitudeChanged_altitude"] def checkDroneAltitudeStatus(self): altitude = self.getDroneAltitude() if (altitude < properties.MIN_ALTITUDE): return TOO_LOW elif (altitude > properties.MAX_ALTITUDE): return TOO_HIGH else: return ALTITUDE_OK def getDronVerticalAlignment(self): droneAltitudeStatus = self.checkDroneAltitudeStatus() verticalAlignment = 0 if (droneAltitudeStatus == TOO_LOW or droneAltitudeStatus == TOO_HIGH): # negative goes up, positive goes down verticalAlignment = self.getDroneAltitude() - properties.OPTIMAL_ALTITUDE else: verticalAlignment = 0 return verticalAlignment def getBatteryPercentage(self): return self.bebop.sensors.battery def checkBatteryStatus(self): batteryPercentage = self.getBatteryPercentage() if batteryPercentage < 5: return CRITICAL elif batteryPercentage < 10: return LOW else: return NORMAL def goHome(self): if self.current_position != self.home: pathfinder = Pathfinder(self.current_position, self.home) pathToFollow = pathfinder.findPath() pathfinder.printFinalMap() for nextPosition in pathfinder.parsePathToCoordinates(pathToFollow): self.move(nextPosition) def getClosestCoordinateToTarget(self, target, pos): res = self.current_position[pos] if res < target[pos]: res = res + 1 elif res > target[pos]: res = res - 1 return res def moveToPoiCritico(self, path): for nextPosition in path: self.move(nextPosition) def moveNextPositionPOICritico(self, new_position): dx, dy = new_position[0] - self.current_position[0], new_position[1] - self.current_position[1] real_dx, real_dy = dx * self.rango_ancho, dy * self.rango_largo verticalMove = self.getDronVerticalAlignment() self.bebop.move_relative(real_dx, real_dy, verticalMove, 0) time.sleep(2) self.current_position = new_position def disconnect(self): if properties.STREAMING_MODE_ON: self.closeStreaming() self.bebop.disconnect() def initializeStreaming(self): print("-- Starting Streaming... --") self.bebop.set_video_resolutions('rec720_stream720') self.bebop.start_video_stream() print("-- Streaming Started! --") def closeStreaming(self): print("-- Stopping Streaming... --") self.bebop.stop_video_stream() print("-- Streaming stopped!")
##bebop.start_video_stream() ##bebop.smart_sleep(5) ## ## #bebop.safe_takeoff(10) #bebop.smart_sleep(10) # ##print("flip left") ##print("flying state is %s" % bebop.sensors.flying_state) ##success = bebop.flip(direction="left") ##print("mambo flip result %s" % success) ##bebop.smart_sleep(5) # #bebop.move_relative(0, 0, -1, -1) #move_relative(self, dx, dy, dz, dradians) #print("First Move Done") ##bebop.move_relative(0, 0, 0, 1) ##print("Second Move Done") # bebop.move_relative(0, 0, -1, 6.28) #move_relative(self, dx, dy, dz, dradians) print("First Move Done") bebop.smart_sleep(1) bebop.safe_land(10) print(bebop.sensors.sensors_dict) print(bebop.sensors.battery) print("DONE - disconnecting") bebop.disconnect()