class ARDrone(Drone): def __init__(self): super().__init__() self.bebop = Bebop() print("connecting to bebop drone") self.connection.emit("progress") self.success = self.bebop.connect(5) if self.success: self.connection.emit("on") self.bebop.set_max_altitude(20) self.bebop.set_max_distance(20) self.bebop.set_max_rotation_speed(180) self.bebop.set_max_vertical_speed(2) self.bebop.enable_geofence(1) self.bebop.set_hull_protection(1) # todo: battery signal to emit (look in sensors) #TODO test this piece of code self.bebop.set_user_sensor_callback(print, self.bebop.sensors.battery) else: print("refresh....") self.connection.emit("off") def take_off(self): self.bebop.safe_takeoff(5) def land(self): self.bebop.safe_land(5) def stop(self): self.bebop.disconnect() def fly_direct(self, roll, pitch, yaw, vertical_movement): my_roll = self.bebop._ensure_fly_command_in_range(roll) my_pitch = self.bebop_ensure_fly_command_in_range(pitch) my_yaw = self.bebop_ensure_fly_command_in_range(yaw) my_vertical = self.bebop_ensure_fly_command_in_range(vertical_movement) command_tuple = self.bebop.command_parser.get_command_tuple( "ardrone3", "Piloting", "PCMD") self.bebop.drone_connection.send_single_pcmd_command( command_tuple, my_roll, my_pitch, my_yaw, my_vertical) def process_motion(self, _up, _rotate, _front, _right): velocity_up = _up * self.max_vert_speed velocity_yaw = _rotate * self.max_rotation_speed velocity_pitch = _front * self.max_horiz_speed velocity_roll = _right * self.max_horiz_speed #print("PRE", velocity_roll, velocity_pitch, velocity_up, velocity_yaw) self.fly_direct(velocity_roll, velocity_pitch, velocity_yaw, velocity_up)
bebop.smart_sleep(2) bebop.ask_for_state_update() print("Prepare for take-off...") # Set safe indoor/outdoor parameters print("Set indoor parameters...") max_tilt = 15 vertical_speed = 2 #1.5 max_altitude = 1.5 max_rotation_speed = 150 tilt_speed = 200 bebop.set_max_tilt(max_tilt) bebop.set_max_vertical_speed(vertical_speed) bebop.set_max_altitude(max_altitude) bebop.set_max_rotation_speed(max_rotation_speed) bebop.set_max_tilt_rotation_speed(tilt_speed) color_print("Indoor parameters set: OK", "SUCCESS") # Get initial sensor data_type print("--------- SENSOR DATA ----------") print("Battery: ", bebop.sensors.battery, "%") print("Flying state:", bebop.sensors.flying_state) print("Altitude: ", max_altitude, " m") print("Pitch/roll rotation speed (degrees): ", tilt_speed) print("Tilt (degrees): ", max_tilt) print("Vertical speed: ", vertical_speed, " m/s") print("Yaw rotation speed (degrees): ", max_rotation_speed) print("--------------------------------")
from pyparrot.Bebop import Bebop from geographiclib.geodesic import Geodesic import geopy.distance bebop = Bebop() print("connecting") success = bebop.connect(10) print(success) print("sleeping") bebop.smart_sleep(1) bebop.ask_for_state_update() bebop.set_max_altitude(5) bebop.set_max_distance(10) bebop.enable_geofence(1) #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"]) + "\n\n") print("First State Update: ") Drone_1a_Lat = bebop.sensors.sensors_dict["GpsLocationChanged_latitude"] print("GpsLocationChanged_latitude: " + str(bebop.sensors.sensors_dict["GpsLocationChanged_latitude"])) Drone_1a_Lon = bebop.sensors.sensors_dict["GpsLocationChanged_longitude"] print("GpsLocationChanged_longitude: " + str(bebop.sensors.sensors_dict["GpsLocationChanged_longitude"]))
## TODO: (?) make drone land instead of return if max vals exceeded # loop for drone control usingdrone = True if usingdrone: InTheAir = False bebop = Bebop(drone_type="Bebop2") print("\nWaking the propellor flailing flying feller...") success = bebop.connect(10) if success: print("\nSuccess! Drone is awake and listening!") else: print("\nNope. Drone isn't connecting.") print("sleeping") bebop.smart_sleep(5) bebop.ask_for_state_update() bebop.set_max_altitude( 20) ##TODO: change to distance when using x and y movement # bebop.set_max_distance(20) bebop.enable_geofence(1) chillcounter = 10 altitude = 0 going = True justatest = True if justatest: usingdrone = False #simplecount = 0 try: while going: # print(simplecount) print(json.loads(DONNA.ws.recv())["com"]) thought = json.loads(DONNA.ws.recv())["com"][0] if usingdrone:
# move forward on x-axis # does not use gps from pyparrot.Bebop import Bebop bebop = Bebop() print("connecting") success = bebop.connect(10) print(success) if (success): # set safe indoor parameters bebop.set_max_tilt(5) bebop.set_max_vertical_speed(1) bebop.set_max_altitude(2) # trying out the new hull protector parameters - set to 1 for a hull protection and 0 without protection bebop.set_hull_protection(1) bebop.safe_takeoff(10) # horizontal move by controlling the pitch. Positive for forward and negative for backwards bebop.fly_direct(roll=0, pitch=30, yaw=0, vertical_movement=0, duration=2) bebop.smart_sleep(2) bebop.fly_direct(roll=0, pitch=-30, yaw=0, vertical_movement=0, duration=2) bebop.safe_land(10) print("DONE - disconnecting") #bebop.stop_video_stream() bebop.smart_sleep(5)
# from pyparrot.Bebop import Bebop bebop = Bebop() print("connecting") success = bebop.connect(10) print(success) if (success): # set safe indoor parameters bebop.set_max_tilt(5) # degrees bebop.set_max_vertical_speed(1) # meters/sec bebop.set_max_altitude(2) # 2 meters max altitude # trying out the new hull protector parameters - set to 1 for a hull protection and 0 without protection bebop.set_hull_protection(1) bebop.safe_takeoff(10) #move up vertical and then down - value for vertical movement represents % of vertical speed bebop.fly_direct(roll=0, pitch=0, yaw=0, vertical_movement=20, duration=1) bebop.smart_sleep(2) bebop.fly_direct(roll=0, pitch=0, yaw=0, vertical_movement=-20, duration=1) bebop.safe_land(10) print("DONE - disconnecting") bebop.smart_sleep(1) 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!")