tello.connect() print('Hello Tello!') tello.takeoff() tello.streamon() frame_read = tello.get_frame_read() cv2.imwrite("picture.png", frame_read.frame) tello.move_back(20) frame_read = tello.get_frame_read() cv2.imwrite("picture2.png", frame_read.frame) tello.move_forward(20) tello.land() tello.streamoff() battery_data = tello.get_battery() print('Battery level: ' + battery_data + '%') barometer_data = tello.get_barometer() print('Barometer: ' + barometer_data + ' cm') temperature = tello.get_temperature() print('Temperature: ' + temperature + ' °C')
class TelloSelfControl(object): def __init__(self): self.tello = Tello() self.is_flying = False self.frame_read = None self.cap = None self.faces = None self.displayMsg = '' self.drone_speed = (0, 0, 0, 0) self.vDistance = (0, 0, 0, 0) self.windowSize = None self.windowCenter = None self.objectCenter = None self.isOverride = False self.velocity_left_right = 0 self.velocity_up_down = 0 self.velocity_forward_backward = 0 self.velocity_yaw = 0 self.nFace = (0, 0, 0, 0) self.last_seen = (0, 0, 0, 0) self.last_last_seen = (0, 0, 0, 0) self.valid_lock = False self.valid_lock_count = 0 self.is_face_lost = False self.imgCount = 0 def run(self): # Tello setup if use_drone: self.frame_read = self.drone_setup() else: self.cap = self.video_capture_setup() if not self.cap.isOpened: throw_error('Error opening video capture') # init cv2 window cv.namedWindow('Tracker') # run frames while True: # reset self.isOverride = False self.displayMsg = '' self.velocity_left_right = 0 self.velocity_up_down = 0 self.velocity_forward_backward = 0 self.velocity_yaw = 0 # img count for status data form tello if self.imgCount == 100: if use_drone: self.status_update() self.imgCount = 0 else: self.imgCount += 1 if use_drone: if self.frame_read.stopped: self.frame_read.stop() self.tello.streamoff() throw_error('stream turned off') break # get single frame frame = self.frame_read.frame else: ret, frame = self.cap.read() if frame is None: throw_error('No captured frame -- Break') break # get window properties self.windowSize = cv.getWindowImageRect('Tracker') self.windowCenter = (self.windowSize[2] // 2, self.windowSize[3] // 2) time.sleep(1 / FPS) # face detection self.faces = self.face_detection(frame) # Overridecontrols if use_drone: self.override_detection() # automation controls if not self.isOverride and not manual_flight: self.auto_control() # send rc controls if use_drone: self.tello.send_rc_control(self.velocity_left_right, self.velocity_forward_backward, self.velocity_up_down, self.velocity_yaw) # show frame in window self.display(frame) if self.nFace is not self.last_seen: self.last_last_seen = self.last_seen self.last_seen = self.nFace # Exit key esc key = cv.waitKey(10) if key == 27: self.tello.end() throw_error('Close application') # drone setup def drone_setup(self): if not self.tello.connect(): throw_error('Could not connect to Tello') if not self.tello.streamon(): throw_error('Could not load stream') print(self.tello.get_battery()[:2]) return self.tello.get_frame_read() def video_capture_setup(self): return cv.VideoCapture(args.camera) # frame detection def face_detection(self, frame): gray_frame = cv.cvtColor(frame, cv.COLOR_BGR2GRAY) gray_frame = cv.equalizeHist(gray_frame) objects = face_cascade.detectMultiScale(gray_frame) return objects # frame visualsation def display(self, frame): self.drone_speed = (self.velocity_yaw, self.velocity_up_down, self.velocity_forward_backward) if not None: for (x, y, w, h) in self.faces: frame = cv.circle(frame, (x + w // 2, y + h // 2), 10, (78, 214, 99), 2) # center face frame = cv.rectangle(frame, (x, y), (x + w, y + h), (78, 214, 99), 2) # face frame = cv.putText(frame, 'Width: ' + str(w), LEFT1, FONT, FONTSCALE, FONTCOLOR, LINETYPE) frame = cv.putText(frame, 'Height: {0}'.format(str(h)), LEFT2, FONT, FONTSCALE, FONTCOLOR, LINETYPE) frame = cv.putText(frame, self.displayMsg, LEFT3, FONT, FONTSCALE, FONTCOLOR, LINETYPE) # data message frame = cv.putText(frame, str(self.vDistance), TOPLEFT, FONT, FONTSCALE, FONTCOLOR, LINETYPE) # distance vektor to center frame = cv.putText(frame, str(self.drone_speed), (10, 100), FONT, FONTSCALE, FONTCOLOR, LINETYPE) # speed send to drone frame = cv.circle(frame, self.windowCenter, 10, (225, 15, 47), 2) # center window frame = cv.rectangle(frame, (self.windowCenter[0] - TARGET_SIZE // 2, self.windowCenter[1] - TARGET_SIZE // 2), (self.windowCenter[0] + TARGET_SIZE // 2, self.windowCenter[1] + TARGET_SIZE // 2), (225, 15, 47), 2) # distance area frame = cv.rectangle( frame, (self.windowCenter[0] - tx // 2, self.windowCenter[1] - ty // 2), (self.windowCenter[0] + tx // 2, self.windowCenter[1] + ty // 2), (255, 191, 0), 2) # tolerance area (x, y, w, h) = self.last_last_seen frame = cv.rectangle(frame, (x, y), (x + w, y + h), (255, 0, 238), 2) # last last seen face position (x, y, w, h) = self.last_seen frame = cv.rectangle(frame, (x, y), (x + w, y + h), (255, 191, 0), 2) # last last seen face position cv.imshow('Tracker', frame) # drone autocontrol def auto_control(self): if isinstance(self.faces, tuple): self.face_lost() return # we validate which face has the highest chance to be the same face as the last seen face the valid_value # describes the diffrence between detected and last seen face. the face with the lowest valid_value will be # selected valid_value = math.inf self.nFace = (0, 0, 0, 0) for face in self.faces: vD = self.get_dist_vector(face) value = abs(vD[0]) + abs(vD[1]) + abs(vD[2]) if value < valid_value: self.nFace = face valid_value = value self.displayMsg += str(valid_value) + ' / ' (x, y, w, h) = self.nFace # if the last face isnt locked, we need to check the counter if not self.valid_lock: self.valid_lock_fnc(valid_value) # if the last face was locked and the detected valid_value has more than 200 difference to the last face, # we need to clal face_lost() if valid_value > 200 and self.valid_lock: self.nFace = self.last_seen self.face_lost() print('-- Face lost', valid_value) return # if the face was lost last frame, the lock is open to prevent that something wrong gonna be tracked if self.is_face_lost: self.valid_lock_count = 0 self.valid_lock = False self.is_face_lost = False self.objectCenter = (x + w // 2, y + h // 2) # face center # calculate the distance vector from center of target to center of window(camera view) vCenter = numpy.array( (self.windowCenter[0], self.windowCenter[1], TARGET_SIZE)) vTarget = numpy.array( (self.objectCenter[0], self.objectCenter[1], self.nFace[3])) self.vDistance = vCenter - vTarget # set the right speed for the distance sX = sY = sZ = SPEED if abs(self.vDistance[0]) < tx: sX = sX // 2 if abs(self.vDistance[1]) < ty: sY = sY // 2 if self.vDistance[2] > 2 * tz: sZ = sZ // 2 # set the movement of the drone by analysing the data. (tx, ty, tz are the tolerance distances. Within these # distances the drone stays still) # X - axis correction if self.vDistance[0] + tx // 2 < 0: self.velocity_yaw = sX self.displayMsg += 'turn right /' elif self.vDistance[0] - tx // 2 > 0: self.velocity_yaw = -sX self.displayMsg += 'turn left /' # Y - axis correction if self.vDistance[1] + ty // 2 < 0: self.velocity_up_down = -sY self.displayMsg += 'move down /' elif self.vDistance[1] - ty // 2 > 0: self.velocity_up_down = sY self.displayMsg += 'move up /' # Z - axis correction if self.vDistance[2] < 0: self.velocity_forward_backward = -sZ self.displayMsg += 'move backward /' elif self.vDistance[2] - tz > 0: self.velocity_forward_backward = sZ self.displayMsg += 'move forward /' # last seen valuation def get_dist_vector(self, face): (x, y, w, h) = face (lx, ly, lw, lh) = self.last_seen v1 = numpy.array( (x + w // 2, y + h // 2, w)) # vector off detected face v2 = numpy.array( (lx + lw // 2, ly + lh // 2, lw)) # vector off last valid face return v1 - v2 # we need to handle the drone in case the face got lost def face_lost(self): self.is_face_lost = True (x, y, w, h) = self.last_seen (lx, ly, lw, lh) = self.last_last_seen self.displayMsg = 'x: {}, y: {} / x: {}, y: {}'.format(x, y, lx, ly) if x > lx: self.velocity_yaw = SPEED else: self.velocity_yaw = -SPEED # if a new face is detected and no other was got locked, we need to valid this face. # For example we need to check that the detected face is just caused of a noise image quality. # For this we confirm that the face was 10 frames in a row detected. if not so, an other face could get locked def valid_lock_fnc(self, valid_value): if valid_value < 200: self.valid_lock_count += 1 else: self.valid_lock_count = 0 print('-- valid lock count: ', self.valid_lock_count) if self.valid_lock_count >= 10: self.valid_lock = True print('-- valid lock: TRUE') # override by key input def override_detection(self): key = cv.waitKey(1) if key is not -1: self.isOverride = True if key == ord('t'): if self.tello.takeoff(): self.is_flying = True elif key == ord('l'): self.tello.land() self.is_flying = False elif key == ord('q'): self.tello.land() self.frame_read.stop() self.is_flying = False close() else: if key == ord('w'): self.velocity_forward_backward = SPEED if key == ord('s'): self.velocity_forward_backward = -SPEED if key == ord('a'): self.velocity_left_right = -SPEED if key == ord('d'): self.velocity_left_right = SPEED if key == ord('y'): self.velocity_up_down = SPEED if key == ord('h'): self.velocity_up_down = -SPEED if key == ord('g'): self.velocity_yaw = -3 * SPEED if key == ord('j'): self.velocity_yaw = 3 * SPEED def status_update(self): print('Tello Battery: {}'.format(self.tello.get_battery()[:2])) print('Tello Temperature: {}'.format(self.tello.get_temperature()[:2]))
log.append('{}: Laptop identified, fire the lasers!'.format(datetime.now())) index = label.index('laptop') output_image = addLaser(output_image,index) if 'dog' in label: log.append('{}: Dog identified...woof'.format(datetime.now())) else: output_image = img mpID, x, y, z = missionPadXYZ() if mpID == -1: mp_str = "404: Mission Pad Not Found" else: mp_str = "Mission Pad:{} at X:{} Y:{} Z:{}".format(mpID, x, y, z) bat_str = "Battery: {}%".format(tello.get_battery()) temp_str = "Temperature: {} F".format(round(tello.get_temperature()*(9/5)+32),1) alt_str = "Altitude: {}cm".format(tello.get_distance_tof()) textOnFrame(output_image, [bat_str, temp_str, alt_str, mp_str]) movie.write(output_image) cv2.imshow('Drone TV', output_image) if cv2.waitKey(1) & 0xFF == ord('q'): tello.streamoff() cv2.destroyAllWindows() break if flyMission == True and inFlight == False: tello.takeoff() inFlight = True if flyMission == True and inFlight == True: t2 = datetime.now()
class Mi_Dron(): def __init__(self): self.tello = Tello() self.estado = 'Desconectado' self.conectado = False def conectar(self): # conectar con el dron try: self.tello.connect() except: self.estado = 'PROBLEMAS CONEXION DRON...' self.conectado = False else: self.conectado = True self.estado = 'DRON CONECTADO...' def takeoff(self): self.estado = 'DESPEGANDO...' if self.conectado: self.tello.takeoff() sleep(5) self.estabilizar() def land(self): self.estado = 'ATERRIZANDO...' if self.conectado: self.tello.land() def subir(self): self.estado = 'SUBIENDO...' if self.conectado: self.tello.send_rc_control(0, 0, 50, 0) sleep(3) self.estabilizar() def adelante(self): self.estado = 'ADELANTE...' if self.conectado: self.tello.send_rc_control(0, 50, 0, 0) sleep(3) self.estabilizar() def atras(self): self.estado = 'ATRAS...' if self.conectado: self.tello.send_rc_control(0, -50, 0, 0) sleep(3) self.estabilizar() def bajar(self): self.estado = 'BAJANDO...' if self.conectado: self.tello.send_rc_control(0, 0, -50, 0) sleep(3) self.estabilizar() def derecha(self): self.estado = 'DERECHA...' if self.conectado: self.tello.send_rc_control(50, 0, 0, 0) sleep(3) self.estabilizar() def izquierda(self): self.estado = 'IZQUIERDA...' if self.conectado: self.tello.send_rc_control(-50, 0, 0, 0) sleep(3) self.estabilizar() def giro_derecha(self): self.estado = 'GIRO DERECHA...' if self.conectado: self.tello.send_rc_control(0, 0, 0, 45) sleep(3) self.estabilizar() def giro_izquierda(self): self.estado = 'GIRO IZQUIERDA...' if self.conectado: self.tello.send_rc_control(0, 0, 0, -45) sleep(3) self.estabilizar() def estabilizar(self): self.tello.send_rc_control(0, 0, 0, 0) def adelante_plan(self): self.tello.send_rc_control(0, 50, 0, 0) sleep(5) def atras_plan(self): self.tello.send_rc_control(0, -50, 0, 0) sleep(5) def giro_der_plan(self): self.tello.send_rc_control(0, 0, 0, 45) sleep(3) def giro_izq_plan(self): self.tello.send_rc_control(0, 0, 0, -45) sleep(3) def izquierda_plan(self): self.tello.send_rc_control(0, -50, 0, 0) sleep(5) def derecha_plan(self): self.tello.send_rc_control(0, -50, 0, 0) sleep(5) def takeoff_plan(self): self.tello.takeoff() sleep(3) def land_plan(self): self.tello.land() sleep(3) def subir_plan(self): self.tello.send_rc_control(0, 0, 50, 0) sleep(5) def bajar_plan(self): self.tello.send_rc_control(0, 0, -50, 0) sleep(5) def mostrar_estado(self): datos = 'ESTADO : ' + self.estado.upper() + '\n' if self.conectado: datos += 'Batería: ' + str(self.tello.get_battery()) + '\n' datos += 'Altitud: ' + str(self.tello.get_barometer()) + '\n' datos += 'Temperatura: ' + str(self.tello.get_temperature()) + '\n' datos += 'Velocidad X: ' + str(self.tello.get_speed_x()) + '\n' datos += 'Velocidad Y: ' + str(self.tello.get_speed_y()) + '\n' datos += 'Velocidad Z: ' + str(self.tello.get_speed_z()) + '\n' return (datos)
import matplotlib.pyplot as plt import cvlib as cv from cvlib.object_detection import draw_bbox from djitellopy import Tello import numpy as np tello = Tello() tello.connect() tello.streamon() def get_resized_frame(w=640, h=480): return cv2.resize(tello.get_frame_read().frame, (w, h)) while True: img = get_resized_frame() bbox, label, conf = cv.detect_common_objects(img) output_image = draw_bbox(img, bbox, label, conf) bat = "Battery: {}%".format(tello.get_battery()) cv2.putText(output_image, bat, (5, 480 - 5), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 2) temp = "Temperature: {}%C".format(tello.get_temperature()) cv2.putText(output_image, temp, (5, 480 - 25), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 2) cv2.imshow("Image", output_image) if cv2.waitKey(1) & 0xFF == ord('q'): cv2.destroyAllWindows() break tello.streamoff() cv2.destroyAllWindows()